micropeak: Record samples before boost detect

This saves a ring of 16 samples while waiting for boost, and then goes
back through those looking for the first sample higher than the ground
and writes the remaining ones to the log so that we get a more
complete log of the flight

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

diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c
index 8201280000e9d8c2cfd0491976501529a92b318f..f361aa26e7d4ef00ad0d710c477a43f70b1d76a3 100644 (file)
--- a/src/micropeak/ao_micropeak.c
+++ b/src/micropeak/ao_micropeak.c
@@ -60,6 +60,12 @@ ao_pips(void)
        ao_delay(AO_MS_TO_TICKS(200));
 }
 
+#define NUM_PA_HIST    16
+
+#define SKIP_PA_HIST(i,j)      (((i) + (j)) & (NUM_PA_HIST - 1))
+
+static uint32_t        pa_hist[NUM_PA_HIST];
+
 int
 main(void)
 {
@@ -67,6 +73,7 @@ main(void)
        uint16_t        time;
        uint32_t        pa_interval_min, pa_interval_max;
        int32_t         pa_diff;
+       uint8_t         h, i;
 
        ao_led_init(LEDS_AVAILABLE);
        ao_timer_init();
@@ -91,6 +98,7 @@ main(void)
        ao_pa_get();
        pa_avg = pa_ground = pa << FILTER_SHIFT;
        sample_count = 0;
+       h = 0;
        for (;;) {
                time += SAMPLE_SLEEP;
                if (sample_count == 0)
@@ -99,6 +107,8 @@ main(void)
                ao_pa_get();
                if (sample_count == 0)
                        ao_led_off(AO_LED_REPORT);
+               pa_hist[h] = pa;
+               h = SKIP_PA_HIST(h,1);
                pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa;
                pa_diff = pa_ground - pa_avg;
 
@@ -119,6 +129,19 @@ main(void)
 
        pa_ground >>= FILTER_SHIFT;
 
+       /* Go back and find the first sample a decent interval above the ground */
+       pa_min = pa_ground - LAND_DETECT;
+       for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) {
+               if (pa_hist[i] < pa_min)
+                       break;
+       }
+
+       /* Log the remaining samples so we get a complete history since leaving the ground */
+       for (; i != h; i = SKIP_PA_HIST(i,2)) {
+               pa = pa_hist[i];
+               ao_log_micro_data();
+       }
+
        /* Now sit around until the pressure is stable again and record the max */
 
        sample_count = 0;