autopause: fix speed-rounding false resume, add heading-consistency check

System.speed truncated instantaneous NMEA speed to whole km/h before
comparing against auto_pause_speed, so receiver noise straddling the
3km/h default (e.g. 2.57/3.41/3.82/2.72 km/h) could round to three
consecutive >=3 samples and trigger a false resume while stationary -
confirmed against a field log where this fired with no real departure
(erratic heading, position barely moving). Store speed as speed_x10
instead and compare at that precision.

Also add a step-to-step bearing consistency check to the resume-by-
distance path: a satellite geometry change can drift the computed fix
smoothly in one direction for tens of seconds with the receiver not
moving at all, satisfying the existing persistence check the same way
real walking would. Real walking keeps a fairly stable heading between
consecutive fixes; require that before counting a sample as a resume
signal, alongside the existing distance-from-anchor persistence.

gps-test-tool/main.h

@@ -267,6 +267,7 @@ void gps_initialize(void);
 void add_distance(float dist);
 void add_elevation(float ele_change);
 float distance(struct location_s *pos1, struct location_s *pos2);
+float bearing(struct location_s *pos1, struct location_s *pos2);
 unsigned char nmea_epoch_complete(void);
 
 /* The structured system log (soft/syslog.c) is compiled into this test tool

soft/autopause.c

@@ -31,6 +31,24 @@ struct auto_pause_s auto_pause;
 #define AUTO_PAUSE_RESUME_HISTORY_MASK	((1U << AUTO_PAUSE_RESUME_HISTORY_LEN) - 1)
 #define AUTO_PAUSE_RESUME_MIN_COUNT	5
 
+/* 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		1.5	/* 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;
@@ -57,6 +75,9 @@ static void auto_pause_activate(void) {
 	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);
 }
@@ -98,6 +119,9 @@ void auto_pause_reset_gap(void) {
 		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;
 	}
 }
 
@@ -118,7 +142,7 @@ void auto_pause_process(void) {
 	 * 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 (System.speed_x10 >= (unsigned int)System.conf.auto_pause_speed*10) { /* 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;
@@ -137,7 +161,11 @@ void auto_pause_process(void) {
 	 * 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. */
+	 * 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;
@@ -159,7 +187,32 @@ void auto_pause_process(void) {
 		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) {
+
+		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;
+			}
+			/* 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. */
+		}
+		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;
@@ -182,7 +235,7 @@ void auto_pause_process(void) {
 		 * 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)
+			&& 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;

soft/common.h

@@ -122,7 +122,8 @@ struct system_s {
 		// points_paused + points_skipped + rejected_count + points_accepted may fall a couple short of the true candidate-point total: the sliding window used for the distance/altitude spike checks needs a couple of points to refill after each reset (boot, or a fix gap) before it can even decide reject-vs-accept, and those few points aren't tallied anywhere.
 	unsigned int hdop_x100; // HDOP * 100, from GSA
 	unsigned char fix_type; // FIX_TYPE_* from GSA
-	unsigned char speed; // km/h
+	unsigned int speed_x10; // instantaneous NMEA speed, km/h * 10 - see nmea.c's speed_sample_process()
+		// for why whole km/h is too coarse for the auto-pause speed threshold
 	time_t time_start;
 	time_t current_pause_start;
 	time_t pause_time;
@@ -155,4 +156,9 @@ struct auto_pause_s {
 	struct location_s pause_anchor; /* running average position while paused */
 	unsigned int anchor_sample_count;
 	unsigned char resume_high_history; /* bit history: was the anchor-distance/speed resume condition met */
+	struct location_s prev_location; /* last sample seen while paused, for step-to-step bearing */
+	unsigned char prev_location_valid:1;
+	unsigned char prev_bearing_valid:1;
+	float prev_bearing;
+	unsigned char heading_consistent_history; /* bit history: did this step's bearing agree with the last */
 };

soft/gpx.c

@@ -413,6 +413,22 @@ float distance(struct location_s *pos1, struct location_s *pos2){
 	return ret;
 }
 
+/* Initial bearing (degrees, 0-360, 0=north) from pos1 to pos2 - used by the
+ * auto-pause resume-by-distance check (autopause.c) to tell a real, directed
+ * departure from a smooth-but-directionless position drift (e.g. a satellite
+ * geometry change reshaping the fix while the receiver doesn't move at all). */
+float bearing(struct location_s *pos1, struct location_s *pos2){
+	float lat1 = pos1->lat * M_PI / 180.0;
+	float lat2 = pos2->lat * M_PI / 180.0;
+	float dlon = (pos2->lon - pos1->lon) * M_PI / 180.0;
+	float y = sinf(dlon) * cosf(lat2);
+	float x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(dlon);
+	float deg = atan2f(y, x) * 180.0 / M_PI;
+	if (deg < 0)
+		deg += 360.0;
+	return deg;
+}
+
 void add_distance(float dist) {
 	unsigned char paused = tracking_is_paused();
 	if (!paused)

soft/gpx.h

@@ -11,6 +11,7 @@ void gpx_process_point(struct location_s *loc, FIL *file);
 void gpx_reset_gap(void);
 void gpx_save_single_point(struct location_s *loc);
 float distance(struct location_s *pos1, struct location_s *pos2);
+float bearing(struct location_s *pos1, struct location_s *pos2);
 void add_distance(float dist);
 void add_elevation(float ele_change);
 unsigned char is_paused(void);

soft/nmea.c

@@ -141,7 +141,7 @@ static void speed_sample_process(double speed_kmh) {
 	unsigned char paused = tracking_is_paused();
 	unsigned char fix_trustworthy = gps_fix_trustworthy();
 
-	System.speed = speed_kmh+0.5;
+	System.speed_x10 = (unsigned int)(speed_kmh*10.0 + 0.5);
 
 	/* Average speed: fixed-point accumulation of instantaneous samples,
 	 * excluding time spent paused, instead of dividing distance by time. */