gpx: post-gap settling, lon error scaling, fix spike check and avg timestamp

soft/gpx.c

@@ -46,6 +46,25 @@ struct kalman_s {
 #define POS_JUMP_THRESHOLD	100.0	/* meters between consecutive raw fixes */
 #define ALT_JUMP_THRESHOLD	50.0	/* meters between consecutive raw fixes */
 
+/* Post-gap settling: a freshly reacquired fix's position/altitude solution
+ * can keep drifting for several seconds after reacquisition even while
+ * reported quality (satellite count, HDOP) already looks good throughout -
+ * seen on a real recorded track exiting an underground space: HDOP was
+ * already 0.81/17 sats just 7s after reacquiring, yet elevation kept
+ * changing by several meters per sample for another ~10s after that before
+ * settling to normal (sub-meter) noise. Sat count/HDOP were tried as a
+ * "has it converged" signal and rejected: both fluctuate (sat count can
+ * drop as easily as rise while the receiver's tracking set changes) in a
+ * way that gave false "stable" readings well before the position had
+ * actually settled. A fixed wall-clock window matches what the real data
+ * showed converging by, without depending on a quality signal that proved
+ * unreliable for this specific purpose in practice.
+ * Withhold distance/elevation/GPX credit for points within this window
+ * after a gap, while still updating the Kalman/last-position state normally
+ * so a genuine tunnel-with-real-displacement case isn't penalized beyond
+ * losing GAP_SETTLE_SECONDS worth of distance right at the exit. */
+#define GAP_SETTLE_SECONDS	18	/* real data showed convergence taking ~15-17s after reacquisition */
+
 struct prev_points_s {
 	struct location_s data[PREV_POINTS_LENGTH];
 	unsigned char start;
@@ -75,6 +94,7 @@ static struct gpx_s {
 	struct location_s last_distance_point; /* Last accepted point for distance calculation */
 	struct kalman_s kalman[2];
 	struct elevation_s elevation;
+	time_t gap_settle_until; /* points before this time are held back post-gap; 0 = not settling */
 } gpx;
 
 float kalman_predict(struct kalman_s *k, float data);
@@ -112,10 +132,11 @@ unsigned char gpx_init(FIL *file) {
 	gpx.last_saved.lat = 0;
 	gpx.last_saved.time = 0;
 	gpx.last_distance_point.lon = 0;
-	gpx.last_distance_point.lat = 0;
+	gpx.last_distance_point.lat = NAN; /* sentinel: no prior point yet, distinct from a real lat=0 (equator) fix */
 	gpx.last_distance_point.time = 0;
 	gpx.elevation.initialized = 0;
 	gpx.point_count = 0;
+	gpx.gap_settle_until = 0;
 
 	gpx.paused = 1; /* make it add a <trkseg> tag */
 
@@ -216,6 +237,22 @@ void gpx_reset_gap(void) {
 	gpx.avg_store.lon = 0;
 	gpx.avg_store.time = 0;
 	gpx.point_count = 0;
+	/* Without this, the first point(s) accepted after the gap (still inside
+	 * the prev_points warm-up above, so exempt from the distdiff/altdiff
+	 * spike checks until that window refills) would add one uncontrolled
+	 * distance/elevation segment straight from the pre-gap position - which
+	 * may be stale by an arbitrary, unknown amount of real movement during
+	 * the gap - with none of the usual spike protection. Clearing it forces
+	 * distance/elevation accumulation to (re)start fresh from the first
+	 * post-gap point actually reached once warm-up completes, the same way
+	 * it starts fresh after power-on. */
+	gpx.last_distance_point.lat = NAN;
+	/* -1 (all-ones): "settling, but the deadline isn't computed yet" - the
+	 * real deadline needs the first post-gap point's own timestamp, which
+	 * isn't available here (this runs before gpx_process_point() sees that
+	 * point), so gpx_process_point() computes gap_settle_until = that
+	 * point's time + GAP_SETTLE_SECONDS on its first point after this. */
+	gpx.gap_settle_until = (time_t)-1;
 }
 
 void gpx_process_point(struct location_s *loc, FIL *file){
@@ -233,6 +270,18 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 		return;
 	}
 
+	if (gpx.gap_settle_until) {
+		if (gpx.gap_settle_until == (time_t)-1)
+			gpx.gap_settle_until = loc->time + GAP_SETTLE_SECONDS;
+		if (loc->time < gpx.gap_settle_until) {
+			if (get_flag(CONFFLAG_VERBOSE_LOG))
+				xputs_P(PSTR("GAP SETTLE: waiting for fix to stabilize\r\n"));
+			System.points_skipped++;
+			return;
+		}
+		gpx.gap_settle_until = 0; /* window elapsed - resume normal processing below */
+	}
+
 	/* Always apply Kalman filtering for distance calculation */
 	lat_est = kalman_predict(&gpx.kalman[0], loc->lat);
 	lon_est = kalman_predict(&gpx.kalman[1], loc->lon);
@@ -252,7 +301,7 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 		System.points_accepted++;
 
 		/* Calculate distance and elevation from filtered points */
-		if (gpx.last_distance_point.lat != 0) {
+		if (!isnan(gpx.last_distance_point.lat)) {
 			float ele_change;
 			dist = distance(&gpx.last_distance_point, &filtered_loc);
 			if (dist > POS_JUMP_THRESHOLD)
@@ -267,9 +316,16 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 		gpx.last_distance_point = filtered_loc;
 
 	} else {
-		/* Apply Kalman error check */
+		/* Apply Kalman error check. KALMAN_ERR_MAX is a threshold in degrees
+		 * of latitude - a degree of longitude covers less ground the further
+		 * from the equator, by a factor of cos(latitude), so lon_err is
+		 * scaled up here to the equivalent latitude-degree error before the
+		 * same threshold is applied to both. Without this, the effective
+		 * east-west tolerance in meters would shrink at higher latitudes
+		 * (e.g. roughly half as permissive at 60 degrees as at the equator),
+		 * causing spurious rejects that have nothing to do with fix quality. */
 		lat_err = fabs(loc->lat - lat_est);
-		lon_err = fabs(loc->lon - lon_est);
+		lon_err = fabs(loc->lon - lon_est) * cosf(loc->lat * M_PI / 180.0);
 	//	xprintf(PSTR("lat_err: %e, lon_err: %e, limit: %e\r\n"), lat_err, lon_err, (float)KALMAN_ERR_MAX);
 		if(lat_err > KALMAN_ERR_MAX || lon_err > KALMAN_ERR_MAX){
 			if (get_flag(CONFFLAG_VERBOSE_LOG))
@@ -299,13 +355,26 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 			float alt32 = fabs(prev_points_get(2)->alt - prev_points_get(1)->alt);
 			if (get_flag(CONFFLAG_VERBOSE_LOG))
 				xprintf(PSTR("New distance: %fm\r\n"), dist32);
-			if(dist34 > dist12 && dist32 > dist12){
+			/* A real spike must stand out on BOTH sides - bigger than the
+			 * older segment AND bigger than the newer one. The previous
+			 * "dist34 > dist12 && dist32 > dist12" shape only checked dist32
+			 * against dist12 twice from different angles, so it stayed true
+			 * for every step of a sustained, monotonic acceleration (dist12 <
+			 * dist32 < dist34, completely normal e.g. when picking up pace
+			 * while hiking) - rejecting real point after real point until the
+			 * trend broke, then crediting all of that accumulated real
+			 * distance to whichever single point finally got through, as one
+			 * artificially fast-looking GPX segment. Comparing dist32 against
+			 * both neighbors instead lets a genuine acceleration/deceleration
+			 * ramp through cleanly while still catching an isolated outlier
+			 * surrounded by two much smaller segments. */
+			if(dist32 > dist12 && dist32 > dist34){
 				if (get_flag(CONFFLAG_VERBOSE_LOG))
 					xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
 				log_reject(REJECT_REASON_DISTDIFF);
 				return;
 			}
-			if(alt32 > ALT_JUMP_THRESHOLD && alt34 > alt12 && alt32 > alt12){
+			if(alt32 > ALT_JUMP_THRESHOLD && alt32 > alt12 && alt32 > alt34){
 				if (get_flag(CONFFLAG_VERBOSE_LOG))
 					xputs_P(PSTR("ALTITUDE DIFF REJECT\r\n"));
 				log_reject(REJECT_REASON_ALTDIFF);
@@ -337,7 +406,7 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 		System.points_accepted++;
 
 		/* Calculate distance and elevation for accepted point */
-		if (gpx.last_distance_point.lat != 0) {
+		if (!isnan(gpx.last_distance_point.lat)) {
 			float ele_change;
 			dist = distance(&gpx.last_distance_point, ptr);
 			add_distance(dist);
@@ -351,8 +420,16 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 
 		gpx.avg_store.lat += ptr->lat;
 		gpx.avg_store.lon += ptr->lon;
-		if(gpx.avg_count == AVG_COUNT/2)
-			gpx.avg_store.time = ptr->time;
+		/* Use the newest accepted point's time, not e.g. the midpoint's: the
+		 * averaged lat/lon reflects real movement up through this instant, so
+		 * timestamping it any earlier claims that movement happened faster
+		 * than it did. This matters most exactly when the accepted points
+		 * feeding this average were NOT evenly spaced in real time (e.g. a
+		 * reject streak between two of them) - an earlier timestamp there
+		 * would understate the elapsed time for the position delta and
+		 * inflate the speed an external tool derives from consecutive GPX
+		 * points. */
+		gpx.avg_store.time = ptr->time;
 
 		if(++gpx.avg_count == AVG_COUNT){
 			nloc.lat = gpx.avg_store.lat / AVG_COUNT;