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

/* Raw NMEA-reported ground speed (System.speed_x10) is single-epoch Doppler
 * output with no smoothing - real recorded hikes show it read a few km/h for
 * several consecutive seconds even while the receiver is stationary and
 * HDOP/sat count are excellent (multipath/receiver noise, not position
 * error, and correlated over several seconds rather than independent from
 * sample to sample). A strict "N consecutive samples over threshold"
 * debounce is exactly the shape most exposed to this - and even a plain
 * majority-of-a-window vote on the raw value isn't enough on its own, since
 * this noise is persistent enough to occasionally dominate a short window
 * outright. Smooth the speed first (exponential smoothing, same idea as
 * elevation_process()'s altitude smoothing) to knock down the bulk of the
 * noise, *then* still require the smoothed value to clear the threshold for
 * most of a window before resuming - so a single lingering noisy patch
 * cannot itself resume a genuinely stationary logger, but sustained real
 * movement (which pushes the smoothed value well past the threshold, not
 * just brushing it) still resumes within several seconds. */
#define SPEED_SMOOTH_ALPHA		0.25	/* exponential smoothing factor per sample (~1s apart) */
#define SPEED_RESUME_HISTORY_LEN	7	/* same window length as the other resume-history checks */
#define SPEED_RESUME_HISTORY_MASK	((1U << SPEED_RESUME_HISTORY_LEN) - 1)
#define SPEED_RESUME_MIN_COUNT		6	/* stricter majority than distance/heading: speed alone has no
						 * corroborating signal (no distance-from-anchor or heading check
						 * backs it up), so it needs to be almost the whole window */

/* Distance-from-anchor alone can't tell a real departure from a smooth
 * position drift with no real movement behind it: a satellite geometry
 * change (a few satellites dropping out/back in) can walk the computed fix
 * steadily in one direction for tens of seconds while the receiver doesn't
 * move at all - the exact "persist over several samples" shape the resume
 * check above is trying to accept. Real walking keeps a fairly stable
 * step-to-step bearing; a geometry-driven drift has no reason to, so require
 * the bearing between consecutive paused samples to also stay consistent
 * before counting a sample as a real resume signal. Skipped below
 * HEADING_STEP_MIN_DIST_M: bearing is meaningless noise over a sub-meter step
 * (the two fixes are within GPS jitter of each other), and would otherwise
 * make genuinely stationary jitter look "inconsistent" for the wrong reason. */
#define HEADING_STEP_MIN_DIST_M		3	/* ignore bearing between two samples closer than this */
#define HEADING_CONSISTENT_MAX_DEG	60	/* max angle from the previous step's bearing to still agree */
#define HEADING_HISTORY_LEN		7	/* same window length as the distance resume history above */
#define HEADING_HISTORY_MASK		((1U << HEADING_HISTORY_LEN) - 1)
#define HEADING_MIN_COUNT		5	/* same majority-of-window requirement as the distance resume history */

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

static void auto_pause_activate(void) {
	System.tracking_auto_paused = 1;
	pause_time_start();
	/* Otherwise, speed-history progress from just before this decision could
	 * carry over and complete on the very next few samples, instantly
	 * flip-flopping straight back out of the pause this just activated. */
	auto_pause.speed_high_history = 0;
	auto_pause.smoothed_speed_valid = 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;
	auto_pause.prev_location_valid = 0;
	auto_pause.prev_bearing_valid = 0;
	auto_pause.heading_consistent_history = 0;
	log_pause_event(1);
	beep(50, 3);
}

/* Call once, when tracking_pause() (working_modes.c) transitions
 * tracking_paused from set to clear (a manual resume): auto_pause_process()
 * only clears tracking_auto_paused while tracking_paused is *true* (to force
 * an active auto-pause off the instant a manual pause starts), so without
 * this, a manual resume while auto-paused would leave tracking_auto_paused
 * set - tracking_is_paused() (tracking_paused || tracking_auto_paused) would
 * still report paused, silently keeping the logger paused until the
 * speed/distance-based auto-resume condition happened to also be met, well
 * after the user asked to resume. No log/beep here (unlike auto_unpause()) -
 * the manual resume already has its own distinct feedback. */
void auto_pause_clear_manual_resume(void) {
	System.tracking_auto_paused = 0;
	auto_pause.prev_distance = System.distance;
	auto_pause.point_counter = 0;
	auto_pause.speed_high_history = 0;
	auto_pause.smoothed_speed_valid = 0;
}

/* Call once, whenever fix_gap_detected() reports a gap on the point about to
 * be passed to auto_pause_process(). Without this, point_counter keeps
 * counting straight through the gap with no idea how long it was or whether
 * the device kept moving during it, so reacquiring a fix that was lost while
 * genuinely moving could satisfy the stationary-time threshold within
 * moments, using mostly pre-gap progress - the opposite of what the
 * threshold is meant to require (persistent time actually stationary). */
void auto_pause_reset_gap(void) {
	auto_pause.point_counter = 0;
	auto_pause.speed_high_history = 0;
	auto_pause.smoothed_speed_valid = 0;
	auto_pause.prev_distance = System.distance;
	if (System.tracking_auto_paused) {
		/* Same reasoning as auto_pause_activate()'s anchor init: re-anchor on
		 * the fix regained after the gap, not wherever the device was when
		 * the fix was lost - it may have moved (or been carried) an unknown
		 * distance in between, which resume-by-distance must not read as an
		 * instant departure from a now-stale anchor. */
		auto_pause.pause_anchor = location;
		auto_pause.anchor_sample_count = 1;
		auto_pause.resume_high_history = 0;
		auto_pause.prev_location_valid = 0;
		auto_pause.prev_bearing_valid = 0;
		auto_pause.heading_consistent_history = 0;
	}
}

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_high_history = 0;
		auto_pause.smoothed_speed_valid = 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 persistence check below on its own and
	 * resume a logger that is actually sitting still. Such a sample is
	 * simply ignored - it shifts nothing into the history below - so it
	 * can't itself trigger a resume, but also can't erase real progress
	 * made by genuine movement. */
	if (gps_fix_trustworthy()) {
		unsigned char speed_high;
		if (!auto_pause.smoothed_speed_valid) {
			auto_pause.smoothed_speed_x10 = (float)System.speed_x10;
			auto_pause.smoothed_speed_valid = 1;
		} else {
			auto_pause.smoothed_speed_x10 += SPEED_SMOOTH_ALPHA * ((float)System.speed_x10 - auto_pause.smoothed_speed_x10);
		}
		speed_high = auto_pause.smoothed_speed_x10 >= (float)((unsigned int)System.conf.auto_pause_speed*10);
		auto_pause.speed_high_history = (auto_pause.speed_high_history << 1) | speed_high;
		if (__builtin_popcount(auto_pause.speed_high_history & SPEED_RESUME_HISTORY_MASK) >= SPEED_RESUME_MIN_COUNT) {
			auto_pause.point_counter = 0;
			auto_pause.speed_high_history = 0;
			auto_unpause();
			return;
		}
	}
	/* 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. Also
	 * requires a consistent step-to-step bearing (see HEADING_* above) - a
	 * satellite geometry change can drift the fix in a way that also
	 * persists across this many samples, but has no reason to keep the same
	 * heading step to step the way real walking does. */
	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 (auto_pause.prev_location_valid) {
			float step_dist_m = distance(&auto_pause.prev_location, &location);
			if (step_dist_m >= HEADING_STEP_MIN_DIST_M) {
				float this_bearing = bearing(&auto_pause.prev_location, &location);
				if (auto_pause.prev_bearing_valid) {
					float diff = fabs(this_bearing - auto_pause.prev_bearing);
					unsigned char heading_ok;
					if (diff > 180.0)
						diff = 360.0 - diff;
					heading_ok = diff <= HEADING_CONSISTENT_MAX_DEG;
					auto_pause.heading_consistent_history =
						(auto_pause.heading_consistent_history << 1) | heading_ok;
				}
				auto_pause.prev_bearing = this_bearing;
				auto_pause.prev_bearing_valid = 1;
				auto_pause.prev_location = location;
			}
			/* else: too small a step to trust its bearing - leave prev_bearing
			 * and heading_consistent_history untouched, neither confirming nor
			 * breaking the run of consistent headings. */
		}
		if (!auto_pause.prev_location_valid) {
			auto_pause.prev_location = location;
			auto_pause.prev_location_valid = 1;
		}

		if (__builtin_popcount(auto_pause.resume_high_history & AUTO_PAUSE_RESUME_HISTORY_MASK) >= AUTO_PAUSE_RESUME_MIN_COUNT
			&& __builtin_popcount(auto_pause.heading_consistent_history & HEADING_HISTORY_MASK) >= HEADING_MIN_COUNT) {
			auto_pause.point_counter = 0;
			auto_unpause();
			return;
		}
	}
	if (auto_pause.point_counter == System.conf.auto_pause_time - System.conf.auto_pause_time/4)
		auto_pause.quarter_distance = System.distance;
	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.distance - auto_pause.quarter_distance)/100 <= System.conf.auto_pause_dist/4
			&& System.speed_x10 < (unsigned int)System.conf.auto_pause_speed*10)
			auto_pause_activate(); /* pause: not enough net movement, and not currently moving fast */
	}
	auto_pause.prev_distance = System.distance;
}