GPStracker/soft/autopause.c

/*
 * Auto-pause detection: shared between the firmware and gps-test-tool (see
 * autopause_wrapper.c), since it directly gates what ends up in
 * System.distance/elevation_gain/elevation_loss (excluded while paused) and
 * is a major part of what those tools' output actually looks like.
 */
#ifdef PC_BUILD
#include "../gps-test-tool/main.h"
#else
#include "main.h"
#endif

struct auto_pause_s auto_pause;

/* Resuming from a period of auto-pause because of apparent distance needs
 * more apparent movement than triggering the pause did in the first place:
 * position noise while genuinely stationary (multipath, poor sky view) can
 * still occasionally clear the plain auto_pause_dist threshold over enough
 * auto_pause_time windows, and unlike a false pause (which just costs one
 * quiet window before the speed-based check or the next window catches the
 * resume), a false resume pollutes the moving/stopped split and the
 * odometer for as long as it isn't caught. */
#define AUTO_PAUSE_RESUME_MIN_DIST_M	15	/* resume distance-from-anchor floor, regardless of HDOP */
#define AUTO_PAUSE_RESUME_HDOP_MULT	5	/* ...and scale up from there by this many meters per HDOP unit -
						 * a standard rule-of-thumb for GPS position error (HDOP x a typical
						 * few-meter UERE), so a poor fix needs to move further to count */
#define AUTO_PAUSE_RESUME_HISTORY_LEN	7	/* the resume condition must hold for most (see MIN_COUNT below) of
						 * the last this-many samples, not just instantaneously - the same
						 * persistence idea used for the speed cap, so a few seconds of
						 * genuine drift-free movement are needed, not one lucky sample */
#define AUTO_PAUSE_RESUME_HISTORY_MASK	((1U << AUTO_PAUSE_RESUME_HISTORY_LEN) - 1)
#define AUTO_PAUSE_RESUME_MIN_COUNT	5

static void auto_unpause(void) {
	if (!System.tracking_auto_paused)
		return;
	System.tracking_auto_paused = 0;
	log_pause_event(0);
	beep(50, 4);
}

static void auto_pause_activate(void) {
	System.tracking_auto_paused = 1;
	/* Otherwise, speed-counter progress from just before this decision (e.g.
	 * 2 of the 3 consecutive above-threshold samples the resume check below
	 * wants) could carry over and complete on the very next sample,
	 * instantly flip-flopping straight back out of the pause this just
	 * activated. */
	auto_pause.speed_counter = 0;
	/* Anchor the resume-by-distance check on where the pause actually
	 * started, then let it drift towards the running average of positions
	 * seen while paused (steadier than any single fix - see
	 * auto_pause_process()). */
	auto_pause.pause_anchor = location;
	auto_pause.anchor_sample_count = 1;
	auto_pause.resume_high_history = 0;
	log_pause_event(1);
	beep(50, 3);
}

void auto_pause_process(void) {
	if (System.tracking_paused || !get_flag(CONFFLAG_AUTO_PAUSE)) { /* remove auto-pause */
		System.tracking_auto_paused = 0;
		auto_pause.prev_distance = System.distance;
		auto_pause.point_counter = 0;
		auto_pause.speed_counter = 0;
		return;
	}
	/* A poor/invalid fix must not be read as "moving": a single noisy sample
	 * (e.g. a low-satellite, high-HDOP fix briefly reporting tens of km/h,
	 * as seen on a real hike where reception dropped for a few seconds)
	 * could otherwise satisfy the "3 consecutive" debounce below on its own
	 * and resume a logger that is actually sitting still. Such a sample is
	 * simply ignored - neither counted towards, nor resetting, the
	 * consecutive-samples debounce - so it can't itself trigger a resume,
	 * but also can't erase real progress made by genuine movement. */
	if (gps_fix_trustworthy()) {
		if (System.speed >= System.conf.auto_pause_speed) { /* unpause when set speed is exceeded for 3 consecutive measurements */
			if (++auto_pause.speed_counter >= 3) {
				auto_pause.point_counter = 0;
				auto_pause.speed_counter = 0;
				auto_unpause();
				return;
			}
		} else {
			auto_pause.speed_counter = 0;
		}
	}
	/* Resume-by-distance: compared against an anchor updated to the running
	 * average position seen while paused (steadier than any single fix),
	 * scaled by HDOP (a poor fix needs to move further to count), and
	 * required to persist for most of a short window rather than firing on
	 * one sample - stationary GPS jitter (well under a meter per second) can
	 * otherwise accumulate across enough net-movement windows to look like a
	 * real departure. Evaluated every accepted point, not gated on the
	 * auto_pause_time window below (that window is for the pause trigger
	 * only), so the persistence history has one sample per point. */
	if (System.tracking_auto_paused && gps_fix_trustworthy()) {
		float resume_threshold_m = AUTO_PAUSE_RESUME_MIN_DIST_M;
		float hdop_threshold_m = (System.hdop_x100/100.0f) * AUTO_PAUSE_RESUME_HDOP_MULT;
		float dist_from_anchor_m;
		unsigned char resumed_this_sample;

		if (hdop_threshold_m > resume_threshold_m)
			resume_threshold_m = hdop_threshold_m;

		/* Capped well below the 16-bit wraparound point: once the running
		 * average has this many samples behind it, one more changes it
		 * negligibly anyway, and letting the count wrap to 0 would make the
		 * very next sample overwrite the whole average with itself. */
		if (auto_pause.anchor_sample_count < 60000)
			auto_pause.anchor_sample_count++;
		auto_pause.pause_anchor.lat += (location.lat - auto_pause.pause_anchor.lat) / auto_pause.anchor_sample_count;
		auto_pause.pause_anchor.lon += (location.lon - auto_pause.pause_anchor.lon) / auto_pause.anchor_sample_count;

		dist_from_anchor_m = distance(&location, &auto_pause.pause_anchor);
		resumed_this_sample = dist_from_anchor_m > resume_threshold_m;
		auto_pause.resume_high_history = (auto_pause.resume_high_history << 1) | resumed_this_sample;
		if (__builtin_popcount(auto_pause.resume_high_history & AUTO_PAUSE_RESUME_HISTORY_MASK) >= AUTO_PAUSE_RESUME_MIN_COUNT) {
			auto_pause.point_counter = 0;
			auto_unpause();
			return;
		}
	}
	if (++auto_pause.point_counter < System.conf.auto_pause_time)
		return;
	auto_pause.point_counter = 0;
	if (!System.tracking_auto_paused && gps_fix_trustworthy()) {
		/* Net displacement over this window can be small even while genuinely
		 * moving throughout it - a winding path folding back on itself, or a
		 * short out-and-back - and pausing on that alone, only to have the
		 * very next sample or two immediately satisfy the speed-based resume
		 * above, is itself a spurious (if brief) pause. If the logger is
		 * already moving fast enough to resume on its own right now, don't
		 * pause it in the first place.
		 *
		 * Gated on gps_fix_trustworthy(), matching the resume checks above:
		 * System.distance itself must stay ungated (it's the odometer), but
		 * a run of untrustworthy fixes must not be allowed to decide a pause
		 * transition either way here - it's simply skipped for this window. */
		if ((System.distance - auto_pause.prev_distance)/100 <= System.conf.auto_pause_dist
			&& System.speed < System.conf.auto_pause_speed)
			auto_pause_activate(); /* pause: not enough net movement, and not currently moving fast */
	}
	auto_pause.prev_distance = System.distance;
}