X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_gps_report_mega.c;h=8a29865577b19a31ec0a7970031a69b20dd9dc4d;hp=5e3c71bfa309e1ecb6f3fecb954dde596ff12283;hb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;hpb=24167015705ae831692b95735968b04a876f935e

diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c
index 5e3c71bf..8a298655 100644
--- a/src/kernel/ao_gps_report_mega.c
+++ b/src/kernel/ao_gps_report_mega.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -18,6 +19,43 @@
 #include "ao.h"
 #include "ao_log.h"
 
+#ifndef GPS_SPARSE_LOG
+#define GPS_SPARSE_LOG	0
+#endif
+
+#if GPS_SPARSE_LOG
+static int32_t	prev_lat, prev_lon, int16_t prev_alt;
+static uint8_t	has_prev, unmoving;
+
+#define GPS_SPARSE_UNMOVING_REPORTS	10
+#define GPS_SPARSE_UNMOVING_GROUND	10
+#define GPS_SPARSE_UNMOVING_AIR		10
+
+static uint8_t
+ao_gps_sparse_should_log(int32_t lat, int32_t lon, int16_t alt)
+{
+	uint8_t	ret = 1;
+
+	if (has_prev && ao_log_running) {
+		uint32_t	h = ao_distance(prev_lat, prev_lon, lat, lon);
+		uint16_t	v = alt > prev_alt ? (alt - prev_alt) : (prev_alt - alt);
+
+		if (h < GPS_SPARSE_UNMOVING_GROUND && v < GPS_SPARSE_UNMOVING_AIR) {
+			if (unmoving < GPS_SPARSE_UNMOVING_REPORTS)
+				++unmoving;
+		} else
+			unmoving = 0;
+	} else
+		unmoving = 0;
+
+	prev_lat = lat;
+	prev_lon = lon;
+	prev_alt = alt;
+	has_prev = 1;
+	return unmoving >= GPS_SPARSE_UNMOVING_REPORTS;
+}
+#endif
+
 void
 ao_gps_report_mega(void)
 {
@@ -38,13 +76,21 @@ ao_gps_report_mega(void)
 		ao_gps_new = 0;
 		ao_mutex_put(&ao_gps_mutex);
 
+#if GPS_SPARSE_LOG
+		/* Don't log data if GPS has a fix and hasn't moved for a while */
+		if ((gps_data.flags & AO_GPS_VALID) &&
+		    !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude,
+					      AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data))
+			continue;
+#endif
 		if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) {
+
 			gps_log.tick = ao_gps_tick;
 			gps_log.type = AO_LOG_GPS_TIME;
 			gps_log.u.gps.latitude = gps_data.latitude;
 			gps_log.u.gps.longitude = gps_data.longitude;
-			gps_log.u.gps.altitude = gps_data.altitude;
-
+			gps_log.u.gps.altitude_low = gps_data.altitude_low;
+			gps_log.u.gps.altitude_high = gps_data.altitude_high;
 			gps_log.u.gps.hour = gps_data.hour;
 			gps_log.u.gps.minute = gps_data.minute;
 			gps_log.u.gps.second = gps_data.second;