micropeak: Record samples before boost detect

[fw/altos] / src / micropeak / ao_micropeak.c

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;