Improve distance calculation, always use filtering for it

soft/gpx.c

@@ -54,6 +54,7 @@ static struct gpx_s {
 	unsigned char point_count;
 	struct avg_store_s avg_store;
 	struct location_s last_saved;
+	struct location_s last_distance_point; /* Last accepted point for distance calculation */
 	struct kalman_s kalman[2];
 } gpx;
 
@@ -82,7 +83,7 @@ struct location_s *prev_points_get(unsigned char index){
 
 unsigned char gpx_init(FIL *file) {
 	unsigned int bw;
-	
+
 	kalman_init(&gpx.kalman[0]);
 	kalman_init(&gpx.kalman[1]);
 	gpx.prev_points.count = 0;
@@ -90,6 +91,9 @@ unsigned char gpx_init(FIL *file) {
 	gpx.last_saved.lon = 0;
 	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.time = 0;
 
 	gpx.paused = 1; /* make it add a <trkseg> tag */
 
@@ -168,26 +172,40 @@ unsigned char gpx_close(FIL *file) {
 }
 
 void gpx_process_point(struct location_s *loc, FIL *file){
-	float lon_est, lon_err, lat_est, lat_err, dist = NAN;
+	float lon_est, lon_err, lat_est, lat_err, dist;
 	struct location_s *ptr;
 	struct location_s nloc;
+	struct location_s filtered_loc;
 
 	if (gpx.point_count < System.conf.skip_points) { /* Skipping initial points */
 		gpx.point_count++;
 		return;
 	}
-	
+
+	/* 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);
+
+	filtered_loc.lat = lat_est;
+	filtered_loc.lon = lon_est;
+	filtered_loc.time = loc->time;
+	filtered_loc.alt = loc->alt;
+
 	if (get_flag(CONFFLAG_DISABLE_FILTERS)) {
+		/* Write unfiltered data to GPX, but calculate distance from filtered data */
 		xputs_P(PSTR("Write with filters disabled\r\n"));
-		prev_points_append(loc);
-		if(gpx.prev_points.count == PREV_POINTS_LENGTH){
-			dist = distance(prev_points_get(2), prev_points_get(3));
-		}
+
 		gpx_write(loc, file);
+
+		/* Calculate distance from filtered points */
+		if (gpx.last_distance_point.lat != 0) {
+			dist = distance(&gpx.last_distance_point, &filtered_loc);
+			add_distance(dist);
+		}
+		gpx.last_distance_point = filtered_loc;
+
 	} else {
-		lat_est = kalman_predict(&gpx.kalman[0], loc->lat);
-		lon_est = kalman_predict(&gpx.kalman[1], loc->lon);
-		
+		/* Apply Kalman error check */
 		lat_err = fabs(loc->lat - lat_est);
 		lon_err = fabs(loc->lon - lon_est);
 	//	xprintf(PSTR("lat_err: %e, lon_err: %e, limit: %e\r\n"), lat_err, lon_err, (float)KALMAN_ERR_MAX);
@@ -195,10 +213,9 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 			xputs_P(PSTR("KALMAN REJECT\r\n"));
 			return;
 		}
-		loc->lat = lat_est;
-		loc->lon = lon_est;
-		
-		prev_points_append(loc);
+
+		prev_points_append(&filtered_loc);
+
 		if(gpx.prev_points.count == PREV_POINTS_LENGTH){
 			float dist12 = distance(prev_points_get(0), prev_points_get(1));
 			float dist34 = distance(prev_points_get(2), prev_points_get(3));
@@ -209,7 +226,6 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 				return;
 			}
 			ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
-			dist = dist34;
 		} else {
 			if(gpx.prev_points.count >= PREV_POINTS_LENGTH-2){
 				ptr = prev_points_get(gpx.prev_points.count - 2);
@@ -226,11 +242,18 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 
 		xputs_P(PSTR("ACCEPT\r\n"));
 
+		/* Calculate distance for accepted point */
+		if (gpx.last_distance_point.lat != 0) {
+			dist = distance(&gpx.last_distance_point, ptr);
+			add_distance(dist);
+		}
+		gpx.last_distance_point = *ptr;
+
 		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;
-		
+
 		if(++gpx.avg_count == AVG_COUNT){
 			nloc.lat = gpx.avg_store.lat / AVG_COUNT;
 			nloc.lon = gpx.avg_store.lon / AVG_COUNT;
@@ -245,10 +268,6 @@ void gpx_process_point(struct location_s *loc, FIL *file){
 	}
 	if (System.time_start == 0)
 		System.time_start = utc;
-	if (isnan(dist))
-		return;
-	/* FIXME distance is always calculated from unfiltered data and never paused! */
-	add_distance(dist);
 }
 
 void kalman_init(struct kalman_s *k){