From: Keith Packard <keithp@keithp.com>
Date: Fri, 6 Jun 2014 01:35:45 +0000 (-0700)
Subject: altos: Have tracker average 5 GPS samples before moving to pad mode
X-Git-Tag: 1.3.2.2~12
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=eebcf07950e909e4516b08c32e289a870f772793

altos: Have tracker average 5 GPS samples before moving to pad mode

Avoids early GPS noise right after lock

Signed-off-by: Keith Packard <keithp@keithp.com>
---

diff --git a/src/product/ao_tracker.c b/src/product/ao_tracker.c
index 5c2bb73f..80eade5c 100644
--- a/src/product/ao_tracker.c
+++ b/src/product/ao_tracker.c
@@ -36,6 +36,8 @@ ao_usb_connected(void)
 #else
 #define ao_usb_connected()	1
 #endif
+ 
+#define STARTUP_AVERAGE	5
 
 static void
 ao_tracker(void)
@@ -48,6 +50,9 @@ ao_tracker(void)
 	uint32_t	ground_distance;
 	int16_t		height;
 	uint16_t	speed;
+	int64_t		lat_sum = 0, lon_sum = 0;
+	int32_t		alt_sum = 0;
+	int		nsamples = 0;
 
 	ao_timer_set_adc_interval(100);
 
@@ -69,10 +74,18 @@ ao_tracker(void)
 			switch (ao_flight_state) {
 			case ao_flight_startup:
 				/* startup to pad when GPS locks */
-				ao_flight_state = ao_flight_pad;
-				start_latitude = ao_gps_data.longitude;
-				start_longitude = ao_gps_data.latitude;
-				start_altitude = ao_gps_data.altitude;
+
+				lat_sum += ao_gps_data.latitude;
+				lon_sum += ao_gps_data.longitude;
+				alt_sum += ao_gps_data.altitude;
+
+				if (++nsamples >= STARTUP_AVERAGE) {
+					ao_flight_state = ao_flight_pad;
+					ao_wakeup(&ao_flight_state);
+					start_latitude = lat_sum / nsamples;
+					start_longitude = lon_sum / nsamples;
+					start_altitude = alt_sum / nsamples;
+				}
 				break;
 			case ao_flight_pad:
 				ground_distance = ao_distance(ao_gps_data.latitude,