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 5c2bb73ffa29475bd2a2f71dccb2734d23e5ed43..80eade5c63d6173383254589247be992f9b147fe 100644 (file)
--- 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
 #else
 #define ao_usb_connected()     1
 #endif

+ 
+#define STARTUP_AVERAGE        5

 
 static void
 ao_tracker(void)
 
 static void
 ao_tracker(void)


@@ -48,6 +50,9 @@ ao_tracker(void)
        uint32_t        ground_distance;
        int16_t         height;
        uint16_t        speed;
        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);
 
 
        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 */
                        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,
                                break;
                        case ao_flight_pad:
                                ground_distance = ao_distance(ao_gps_data.latitude,