|
|
@@ -70,7 +70,6 @@ static struct gpx_s {
|
|
|
unsigned char avg_count;
|
|
|
unsigned char paused;
|
|
|
unsigned char point_count;
|
|
|
- time_t last_point_time; /* time of the last point seen by gpx_process_point(), to detect a fix gap */
|
|
|
struct avg_store_s avg_store;
|
|
|
struct location_s last_saved;
|
|
|
struct location_s last_distance_point; /* Last accepted point for distance calculation */
|
|
|
@@ -116,7 +115,6 @@ unsigned char gpx_init(FIL *file) {
|
|
|
gpx.last_distance_point.lat = 0;
|
|
|
gpx.last_distance_point.time = 0;
|
|
|
gpx.elevation.initialized = 0;
|
|
|
- gpx.last_point_time = 0;
|
|
|
gpx.point_count = 0;
|
|
|
|
|
|
gpx.paused = 1; /* make it add a <trkseg> tag */
|
|
|
@@ -197,17 +195,28 @@ unsigned char gpx_close(FIL *file) {
|
|
|
return f_close(file);
|
|
|
}
|
|
|
|
|
|
-/* A fix normally arrives about once a second; treat a longer gap between
|
|
|
- * accepted points as a fix having been lost and reacquired (regardless of
|
|
|
- * why the epochs in between were rejected/absent - fix loss, a 2D fix, poor
|
|
|
- * HDOP, or the receiver simply not producing lines), even without an
|
|
|
- * explicit auto-pause. A freshly reacquired fix can take a few seconds to
|
|
|
- * converge (seen after a real signal loss: a nominally-3D, in-range-HDOP fix
|
|
|
- * still a couple hundred meters off in altitude until more satellites come
|
|
|
- * in), the same way a fix has never been trusted right after power-on -
|
|
|
- * so re-run the existing skip_points warm-up, and reset the Kalman/window
|
|
|
- * state that would otherwise splice the old and new fixes together. */
|
|
|
-#define FIX_GAP_SECONDS 3
|
|
|
+/* Call once, whenever fix_gap_detected() reports a gap on the point about to
|
|
|
+ * be passed to gpx_process_point() - see fix_gap_detected()'s comment for why
|
|
|
+ * a gap (regardless of cause: fix loss, a 2D fix, poor HDOP, or the receiver
|
|
|
+ * simply not producing lines) needs handling at all. Reset the Kalman/window
|
|
|
+ * state that would otherwise splice the old and new fixes together, and
|
|
|
+ * re-run the existing skip_points warm-up: a freshly reacquired fix can carry
|
|
|
+ * a wildly wrong position/altitude for a few seconds while it converges (seen
|
|
|
+ * after a real signal loss - a nominally-3D, in-range-HDOP fix still a couple
|
|
|
+ * hundred meters off in altitude until more satellites came in), the same way
|
|
|
+ * a fix has never been trusted right after power-on. */
|
|
|
+void gpx_reset_gap(void) {
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
+ xputs_P(PSTR("FIX GAP: resetting warm-up/filter state\r\n"));
|
|
|
+ kalman_init(&gpx.kalman[0]);
|
|
|
+ kalman_init(&gpx.kalman[1]);
|
|
|
+ gpx.prev_points.count = 0;
|
|
|
+ gpx.avg_count = 0;
|
|
|
+ gpx.avg_store.lat = 0;
|
|
|
+ gpx.avg_store.lon = 0;
|
|
|
+ gpx.avg_store.time = 0;
|
|
|
+ gpx.point_count = 0;
|
|
|
+}
|
|
|
|
|
|
void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
float lon_est, lon_err, lat_est, lat_err, dist;
|
|
|
@@ -218,20 +227,6 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
if (tracking_is_paused())
|
|
|
System.points_paused++;
|
|
|
|
|
|
- if (gpx.last_point_time && loc->time - gpx.last_point_time > FIX_GAP_SECONDS) {
|
|
|
- if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
- xputs_P(PSTR("FIX GAP: resetting warm-up/filter state\r\n"));
|
|
|
- kalman_init(&gpx.kalman[0]);
|
|
|
- kalman_init(&gpx.kalman[1]);
|
|
|
- gpx.prev_points.count = 0;
|
|
|
- gpx.avg_count = 0;
|
|
|
- gpx.avg_store.lat = 0;
|
|
|
- gpx.avg_store.lon = 0;
|
|
|
- gpx.avg_store.time = 0;
|
|
|
- gpx.point_count = 0;
|
|
|
- }
|
|
|
- gpx.last_point_time = loc->time;
|
|
|
-
|
|
|
if (gpx.point_count < System.conf.skip_points) { /* Skipping initial points */
|
|
|
gpx.point_count++;
|
|
|
System.points_skipped++;
|