Merge branch 'master' of ssh://git.gag.com/scm/git/fw/altos

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

diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c
index 3bbc7eea59a09c348d4c4debf4e1e019db45ea4c..f361aa26e7d4ef00ad0d710c477a43f70b1d76a3 100644 (file)
--- a/src/micropeak/ao_micropeak.c
+++ b/src/micropeak/ao_micropeak.c
@@ -19,6 +19,7 @@
 #include <ao_micropeak.h>
 #include <ao_ms5607.h>
 #include <ao_log_micro.h>
+#include <ao_async.h>
 
 static struct ao_ms5607_sample sample;
 static struct ao_ms5607_value  value;
@@ -48,6 +49,23 @@ ao_compute_height(void)
        ao_max_height = max_alt - ground_alt;
 }
 
+static void
+ao_pips(void)
+{
+       uint8_t i;
+       for (i = 0; i < 10; i++) {
+               ao_led_toggle(AO_LED_REPORT);
+               ao_delay(AO_MS_TO_TICKS(80));
+       }
+       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)
 {
@@ -55,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();
@@ -70,6 +89,7 @@ main(void)
        ao_log_micro_restore();
        ao_compute_height();
        ao_report_altitude();
+       ao_pips();
        ao_log_micro_dump();
        
        ao_delay(BOOST_DELAY);
@@ -78,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)
@@ -86,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;
 
@@ -106,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;