diff options
| author | Keith Packard <keithp@keithp.com> | 2014-06-05 18:35:45 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2014-06-05 18:35:45 -0700 |
| commit | eebcf07950e909e4516b08c32e289a870f772793 (patch) | |
| tree | 3893efea33751e306898f851fcf676cd213a82d8 /src | |
| parent | e063701310c1ab5b766c27f24088699aad142fbd (diff) | |
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>
Diffstat (limited to 'src')
| -rw-r--r-- | src/product/ao_tracker.c | 21 |
1 files changed, 17 insertions, 4 deletions
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, |