|
@@ -31,6 +31,14 @@ struct kalman_s {
|
|
|
#define AVG_COUNT 3
|
|
#define AVG_COUNT 3
|
|
|
#define MIN_DIST_DELTA 2.0
|
|
#define MIN_DIST_DELTA 2.0
|
|
|
|
|
|
|
|
|
|
+/* Elevation dead-band + smoothing, applied to the filtered ascent/descent totals */
|
|
|
|
|
+#define ELEV_SMOOTH_TAU 30.0 /* seconds */
|
|
|
|
|
+#define ELEV_DEADBAND 5.0 /* meters */
|
|
|
|
|
+
|
|
|
|
|
+/* Large-jump detection thresholds (informational logging only, points are not rejected because of these) */
|
|
|
|
|
+#define POS_JUMP_THRESHOLD 100.0 /* meters between consecutive raw fixes */
|
|
|
|
|
+#define ALT_JUMP_THRESHOLD 50.0 /* meters between consecutive raw fixes */
|
|
|
|
|
+
|
|
|
struct prev_points_s {
|
|
struct prev_points_s {
|
|
|
struct location_s data[PREV_POINTS_LENGTH];
|
|
struct location_s data[PREV_POINTS_LENGTH];
|
|
|
unsigned char start;
|
|
unsigned char start;
|
|
@@ -43,6 +51,13 @@ struct avg_store_s {
|
|
|
time_t time;
|
|
time_t time;
|
|
|
};
|
|
};
|
|
|
|
|
|
|
|
|
|
+struct elevation_s {
|
|
|
|
|
+ unsigned char initialized;
|
|
|
|
|
+ float smoothed_alt;
|
|
|
|
|
+ float baseline;
|
|
|
|
|
+ time_t last_time;
|
|
|
|
|
+};
|
|
|
|
|
+
|
|
|
static struct gpx_s {
|
|
static struct gpx_s {
|
|
|
struct prev_points_s prev_points;
|
|
struct prev_points_s prev_points;
|
|
|
unsigned char avg_count;
|
|
unsigned char avg_count;
|
|
@@ -52,11 +67,14 @@ static struct gpx_s {
|
|
|
struct location_s last_saved;
|
|
struct location_s last_saved;
|
|
|
struct location_s last_distance_point; /* Last accepted point for distance calculation */
|
|
struct location_s last_distance_point; /* Last accepted point for distance calculation */
|
|
|
struct kalman_s kalman[2];
|
|
struct kalman_s kalman[2];
|
|
|
|
|
+ struct elevation_s elevation;
|
|
|
} gpx;
|
|
} gpx;
|
|
|
|
|
|
|
|
float kalman_predict(struct kalman_s *k, float data);
|
|
float kalman_predict(struct kalman_s *k, float data);
|
|
|
void kalman_init(struct kalman_s *k);
|
|
void kalman_init(struct kalman_s *k);
|
|
|
float distance(struct location_s *pos1, struct location_s *pos2);
|
|
float distance(struct location_s *pos1, struct location_s *pos2);
|
|
|
|
|
+float elevation_process(float alt, time_t time);
|
|
|
|
|
+void add_elevation_filtered(float amount, unsigned char is_gain);
|
|
|
|
|
|
|
|
void prev_points_append(struct location_s *new){
|
|
void prev_points_append(struct location_s *new){
|
|
|
gpx.prev_points.data[(gpx.prev_points.start + gpx.prev_points.count)%PREV_POINTS_LENGTH] = *new;
|
|
gpx.prev_points.data[(gpx.prev_points.start + gpx.prev_points.count)%PREV_POINTS_LENGTH] = *new;
|
|
@@ -90,6 +108,7 @@ unsigned char gpx_init(FIL *file) {
|
|
|
gpx.last_distance_point.lon = 0;
|
|
gpx.last_distance_point.lon = 0;
|
|
|
gpx.last_distance_point.lat = 0;
|
|
gpx.last_distance_point.lat = 0;
|
|
|
gpx.last_distance_point.time = 0;
|
|
gpx.last_distance_point.time = 0;
|
|
|
|
|
+ gpx.elevation.initialized = 0;
|
|
|
|
|
|
|
|
gpx.paused = 1; /* make it add a <trkseg> tag */
|
|
gpx.paused = 1; /* make it add a <trkseg> tag */
|
|
|
|
|
|
|
@@ -187,19 +206,26 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
filtered_loc.alt = loc->alt;
|
|
filtered_loc.alt = loc->alt;
|
|
|
|
|
|
|
|
if (get_flag(CONFFLAG_DISABLE_FILTERS)) {
|
|
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"));
|
|
|
|
|
|
|
+ /* Write unfiltered data to GPX, but always calculate distance/elevation from filtered data */
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("Write with filters disabled\r\n"));
|
|
|
|
|
|
|
|
gpx_write(loc, file);
|
|
gpx_write(loc, file);
|
|
|
|
|
+ System.points_written++;
|
|
|
|
|
|
|
|
/* Calculate distance and elevation from filtered points */
|
|
/* Calculate distance and elevation from filtered points */
|
|
|
if (gpx.last_distance_point.lat != 0) {
|
|
if (gpx.last_distance_point.lat != 0) {
|
|
|
float ele_change;
|
|
float ele_change;
|
|
|
dist = distance(&gpx.last_distance_point, &filtered_loc);
|
|
dist = distance(&gpx.last_distance_point, &filtered_loc);
|
|
|
|
|
+ if (dist > POS_JUMP_THRESHOLD)
|
|
|
|
|
+ log_jump(0, dist);
|
|
|
add_distance(dist);
|
|
add_distance(dist);
|
|
|
ele_change = filtered_loc.alt - gpx.last_distance_point.alt;
|
|
ele_change = filtered_loc.alt - gpx.last_distance_point.alt;
|
|
|
|
|
+ if (fabs(ele_change) > ALT_JUMP_THRESHOLD)
|
|
|
|
|
+ log_jump(1, ele_change);
|
|
|
add_elevation(ele_change);
|
|
add_elevation(ele_change);
|
|
|
}
|
|
}
|
|
|
|
|
+ elevation_process(filtered_loc.alt, filtered_loc.time);
|
|
|
gpx.last_distance_point = filtered_loc;
|
|
gpx.last_distance_point = filtered_loc;
|
|
|
|
|
|
|
|
} else {
|
|
} else {
|
|
@@ -208,7 +234,9 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
lon_err = fabs(loc->lon - lon_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);
|
|
// 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(lat_err > KALMAN_ERR_MAX || lon_err > KALMAN_ERR_MAX){
|
|
|
- xputs_P(PSTR("KALMAN REJECT\r\n"));
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("KALMAN REJECT\r\n"));
|
|
|
|
|
+ log_reject(REJECT_REASON_KALMAN);
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
@@ -218,27 +246,36 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
float dist12 = distance(prev_points_get(0), prev_points_get(1));
|
|
float dist12 = distance(prev_points_get(0), prev_points_get(1));
|
|
|
float dist34 = distance(prev_points_get(2), prev_points_get(3));
|
|
float dist34 = distance(prev_points_get(2), prev_points_get(3));
|
|
|
float dist32 = distance(prev_points_get(2), prev_points_get(1));
|
|
float dist32 = distance(prev_points_get(2), prev_points_get(1));
|
|
|
- xprintf(PSTR("New distance: %fm\r\n"), dist32);
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xprintf(PSTR("New distance: %fm\r\n"), dist32);
|
|
|
if(dist34 > dist12 && dist32 > dist12){
|
|
if(dist34 > dist12 && dist32 > dist12){
|
|
|
- xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
|
|
|
|
|
+ log_reject(REJECT_REASON_DISTDIFF);
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
|
|
+ if (dist32 > POS_JUMP_THRESHOLD)
|
|
|
|
|
+ log_jump(0, dist32);
|
|
|
ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
|
|
ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
|
|
|
} else {
|
|
} else {
|
|
|
if(gpx.prev_points.count >= PREV_POINTS_LENGTH-2){
|
|
if(gpx.prev_points.count >= PREV_POINTS_LENGTH-2){
|
|
|
ptr = prev_points_get(gpx.prev_points.count - 2);
|
|
ptr = prev_points_get(gpx.prev_points.count - 2);
|
|
|
- xputs_P(PSTR("NEW\r\n"));
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("NEW\r\n"));
|
|
|
} else {
|
|
} else {
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if(distance(&gpx.last_saved, ptr) < MIN_DIST_DELTA){
|
|
if(distance(&gpx.last_saved, ptr) < MIN_DIST_DELTA){
|
|
|
- xputs_P(PSTR("Too small position change REJECT\r\n"));
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("Too small position change REJECT\r\n"));
|
|
|
|
|
+ log_reject(REJECT_REASON_MINDIST);
|
|
|
return;
|
|
return;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- xputs_P(PSTR("ACCEPT\r\n"));
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xputs_P(PSTR("ACCEPT\r\n"));
|
|
|
|
|
|
|
|
/* Calculate distance and elevation for accepted point */
|
|
/* Calculate distance and elevation for accepted point */
|
|
|
if (gpx.last_distance_point.lat != 0) {
|
|
if (gpx.last_distance_point.lat != 0) {
|
|
@@ -246,8 +283,11 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
dist = distance(&gpx.last_distance_point, ptr);
|
|
dist = distance(&gpx.last_distance_point, ptr);
|
|
|
add_distance(dist);
|
|
add_distance(dist);
|
|
|
ele_change = ptr->alt - gpx.last_distance_point.alt;
|
|
ele_change = ptr->alt - gpx.last_distance_point.alt;
|
|
|
|
|
+ if (fabs(ele_change) > ALT_JUMP_THRESHOLD)
|
|
|
|
|
+ log_jump(1, ele_change);
|
|
|
add_elevation(ele_change);
|
|
add_elevation(ele_change);
|
|
|
}
|
|
}
|
|
|
|
|
+ elevation_process(ptr->alt, ptr->time);
|
|
|
gpx.last_distance_point = *ptr;
|
|
gpx.last_distance_point = *ptr;
|
|
|
|
|
|
|
|
gpx.avg_store.lat += ptr->lat;
|
|
gpx.avg_store.lat += ptr->lat;
|
|
@@ -259,12 +299,14 @@ void gpx_process_point(struct location_s *loc, FIL *file){
|
|
|
nloc.lat = gpx.avg_store.lat / AVG_COUNT;
|
|
nloc.lat = gpx.avg_store.lat / AVG_COUNT;
|
|
|
nloc.lon = gpx.avg_store.lon / AVG_COUNT;
|
|
nloc.lon = gpx.avg_store.lon / AVG_COUNT;
|
|
|
nloc.time = gpx.avg_store.time;
|
|
nloc.time = gpx.avg_store.time;
|
|
|
|
|
+ nloc.alt = gpx.elevation.smoothed_alt; /* filtered (smoothed) altitude, for the filtered GPX write */
|
|
|
gpx.avg_count = 0;
|
|
gpx.avg_count = 0;
|
|
|
gpx.avg_store.lat = 0;
|
|
gpx.avg_store.lat = 0;
|
|
|
gpx.avg_store.lon = 0;
|
|
gpx.avg_store.lon = 0;
|
|
|
gpx.avg_store.time = 0;
|
|
gpx.avg_store.time = 0;
|
|
|
gpx.last_saved = nloc;
|
|
gpx.last_saved = nloc;
|
|
|
gpx_write(&nloc, file);
|
|
gpx_write(&nloc, file);
|
|
|
|
|
+ System.points_written++;
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
if (System.time_start == 0)
|
|
if (System.time_start == 0)
|
|
@@ -316,19 +358,79 @@ void add_distance(float dist) {
|
|
|
unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
|
|
unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
|
|
|
if (!paused)
|
|
if (!paused)
|
|
|
System.distance += (dist+0.005)*100.0;
|
|
System.distance += (dist+0.005)*100.0;
|
|
|
- xprintf(PSTR("Distance: %f m; sum: %f m\r\n"), dist, System.distance/100.0);
|
|
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xprintf(PSTR("Distance: %.2f m; sum: %.2f m\r\n"), (double)dist, (double)System.distance/100.0);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+/* Unfiltered (raw), per-point gain/loss - kept only for comparison against the
|
|
|
|
|
+ * filtered (dead-band+smoothed) totals in the periodic status line/session summary. */
|
|
|
void add_elevation(float ele_change) {
|
|
void add_elevation(float ele_change) {
|
|
|
unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
|
|
unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
|
|
|
if (!paused) {
|
|
if (!paused) {
|
|
|
if (ele_change > 0) {
|
|
if (ele_change > 0) {
|
|
|
- System.elevation_gain += (ele_change+0.05)*10.0;
|
|
|
|
|
|
|
+ System.elevation_gain_raw += (ele_change+0.05)*10.0;
|
|
|
} else if (ele_change < 0) {
|
|
} else if (ele_change < 0) {
|
|
|
- System.elevation_loss += (-ele_change+0.05)*10.0;
|
|
|
|
|
|
|
+ System.elevation_loss_raw += (-ele_change+0.05)*10.0;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+ if (get_flag(CONFFLAG_VERBOSE_LOG))
|
|
|
|
|
+ xprintf(PSTR("Elevation change: %.1f m; raw gain: %.1f m, raw loss: %.1f m\r\n"),
|
|
|
|
|
+ (double)ele_change, (double)System.elevation_gain_raw/10.0, (double)System.elevation_loss_raw/10.0);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+/* Filtered (dead-band + smoothed) gain/loss, used for display, GPX ascent stats
|
|
|
|
|
+ * and the periodic status line/session summary. */
|
|
|
|
|
+void add_elevation_filtered(float amount, unsigned char is_gain) {
|
|
|
|
|
+ unsigned long int dm = (unsigned long int)(amount*10.0 + 0.5);
|
|
|
|
|
+ if (is_gain)
|
|
|
|
|
+ System.elevation_gain += dm;
|
|
|
|
|
+ else
|
|
|
|
|
+ System.elevation_loss += dm;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+/* Exponential smoothing (~30s time constant) followed by a 5m dead-band on the
|
|
|
|
|
+ * result, so that a step in the smoothed altitude only counts once it clears
|
|
|
|
|
+ * the dead-band, and only the amount past the dead-band edge is credited. */
|
|
|
|
|
+float elevation_process(float alt, time_t time) {
|
|
|
|
|
+ unsigned char paused = System.tracking_paused || System.tracking_auto_paused;
|
|
|
|
|
+ unsigned long int alt_dm;
|
|
|
|
|
+ float delta;
|
|
|
|
|
+
|
|
|
|
|
+ if (!gpx.elevation.initialized) {
|
|
|
|
|
+ gpx.elevation.smoothed_alt = alt;
|
|
|
|
|
+ gpx.elevation.baseline = alt;
|
|
|
|
|
+ gpx.elevation.last_time = time;
|
|
|
|
|
+ gpx.elevation.initialized = 1;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ float dt = (float)(time - gpx.elevation.last_time);
|
|
|
|
|
+ float alpha;
|
|
|
|
|
+ if (dt <= 0)
|
|
|
|
|
+ dt = 1.0;
|
|
|
|
|
+ gpx.elevation.last_time = time;
|
|
|
|
|
+ alpha = dt / (ELEV_SMOOTH_TAU + dt);
|
|
|
|
|
+ gpx.elevation.smoothed_alt += alpha * (alt - gpx.elevation.smoothed_alt);
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (gpx.elevation.smoothed_alt > 0) {
|
|
|
|
|
+ alt_dm = (unsigned long int)(gpx.elevation.smoothed_alt*10.0 + 0.5);
|
|
|
|
|
+ if (alt_dm > System.alt_max)
|
|
|
|
|
+ System.alt_max = alt_dm;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (paused) {
|
|
|
|
|
+ /* Discard the dead-band reference drift accumulated while paused,
|
|
|
|
|
+ * the same way distance/raw elevation data is discarded when paused. */
|
|
|
|
|
+ gpx.elevation.baseline = gpx.elevation.smoothed_alt;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ delta = gpx.elevation.smoothed_alt - gpx.elevation.baseline;
|
|
|
|
|
+ if (delta > ELEV_DEADBAND) {
|
|
|
|
|
+ add_elevation_filtered(delta - ELEV_DEADBAND, 1);
|
|
|
|
|
+ gpx.elevation.baseline = gpx.elevation.smoothed_alt - ELEV_DEADBAND;
|
|
|
|
|
+ } else if (delta < -ELEV_DEADBAND) {
|
|
|
|
|
+ add_elevation_filtered(-delta - ELEV_DEADBAND, 0);
|
|
|
|
|
+ gpx.elevation.baseline = gpx.elevation.smoothed_alt + ELEV_DEADBAND;
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
- xprintf(PSTR("Elevation change: %f m; gain: %f m, loss: %f m\r\n"),
|
|
|
|
|
- ele_change, System.elevation_gain/10.0, System.elevation_loss/10.0);
|
|
|
|
|
|
|
+ return gpx.elevation.smoothed_alt;
|
|
|
}
|
|
}
|
|
|
|
|
|