altos: Average height values for landing detection

Instead of using the direct output of the kalman filter and hoping
that is quiet enough to detect landing, filter that with a long
exponential decay filter and then check to make sure that doesn't
change more than 2m in 5 seconds as a trigger for landing detection.

Tested with existing telemetrum flight logs and it correctly detects
landing in all cases.

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

diff --git a/src/ao.h b/src/ao.h
index d3e588b6bb9a13e243eb56cb3c330924a80e7bcf..9cde9cba35d6d9e700b8cffe34b7fba2ab80c0f8 100644 (file)
--- a/src/ao.h
+++ b/src/ao.h
@@ -829,6 +829,7 @@ extern __pdata int16_t                      ao_height;      /* meters */
 extern __pdata int16_t                 ao_speed;       /* m/s * 16 */
 extern __pdata int16_t                 ao_accel;       /* m/s² * 16 */
 extern __pdata int16_t                 ao_max_height;  /* max of ao_height */
+extern __pdata int16_t                 ao_avg_height;  /* running average of height */
 
 extern __pdata int16_t                 ao_error_h;
 extern __pdata int16_t                 ao_error_h_sq_avg;

diff --git a/src/ao_flight.c b/src/ao_flight.c
index f1b60d6978713a73012fccc7541fa8d6d9b72edf..a8760ff09a897aee05de58892f7d39ffcdfe565d 100644 (file)
--- a/src/ao_flight.c
+++ b/src/ao_flight.c
@@ -43,7 +43,6 @@ __pdata uint16_t              ao_launch_tick;         /* time of launch detect */
 __pdata uint16_t               ao_interval_end;
 __pdata int16_t                        ao_interval_min_height;
 __pdata int16_t                        ao_interval_max_height;
-
 __pdata uint8_t                        ao_flight_force_idle;
 
 /* We also have a clock, which can be used to sanity check things in
@@ -242,7 +241,7 @@ ao_flight(void)
                                /* initialize interval values */
                                ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 
-                               ao_interval_min_height = ao_interval_max_height = ao_height;
+                               ao_interval_min_height = ao_interval_max_height = ao_avg_height;
 
                                /* and enter drogue state */
                                ao_flight_state = ao_flight_drogue;
@@ -279,14 +278,14 @@ ao_flight(void)
                         * barometer: altitude stable and within 1000m of the launch altitude
                         */
 
-                       if (ao_height < ao_interval_min_height)
-                               ao_interval_min_height = ao_height;
-                       if (ao_height > ao_interval_max_height)
-                               ao_interval_max_height = ao_height;
+                       if (ao_avg_height < ao_interval_min_height)
+                               ao_interval_min_height = ao_avg_height;
+                       if (ao_avg_height > ao_interval_max_height)
+                               ao_interval_max_height = ao_avg_height;
 
                        if ((int16_t) (ao_sample_tick - ao_interval_end) >= 0) {
                                if (ao_height < AO_M_TO_HEIGHT(1000) &&
-                                   ao_interval_max_height - ao_interval_min_height < AO_M_TO_HEIGHT(5))
+                                   ao_interval_max_height - ao_interval_min_height <= AO_M_TO_HEIGHT(2))
                                {
                                        ao_flight_state = ao_flight_landed;
 
@@ -295,7 +294,7 @@ ao_flight(void)
 
                                        ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
                                }
-                               ao_interval_min_height = ao_interval_max_height = ao_height;
+                               ao_interval_min_height = ao_interval_max_height = ao_avg_height;
                                ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
                        }
                        break;

diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c
index 72ad14500b6fb191f94c95e59ebcb88f9925f416..e55d5adee620fbb09ca9f9eb4c03963a6870af42 100644 (file)
--- a/src/ao_flight_test.c
+++ b/src/ao_flight_test.c
@@ -155,6 +155,7 @@ FILE *emulator_in;
 char *emulator_app;
 char *emulator_name;
 double emulator_error_max = 4;
+double emulator_height_error_max = 20; /* noise in the baro sensor */
 
 void
 ao_dump_state(void);
@@ -224,12 +225,20 @@ static int        ao_test_max_height;
 static double  ao_test_max_height_time;
 static int     ao_test_main_height;
 static double  ao_test_main_height_time;
+static double  ao_test_landed_time;
+static double  ao_test_landed_height;
+static double  ao_test_landed_time;
+static int     landed_set;
+static double  landed_time;
+static double  landed_height;
 
 void
 ao_test_exit(void)
 {
        double  drogue_error;
        double  main_error;
+       double  landed_error;
+       double  landed_time_error;
 
        if (!ao_test_main_height_time) {
                ao_test_main_height_time = ao_test_max_height_time;
@@ -237,16 +246,23 @@ ao_test_exit(void)
        }
        drogue_error = fabs(ao_test_max_height_time - drogue_time);
        main_error = fabs(ao_test_main_height_time - main_time);
-       if (drogue_error > emulator_error_max || main_error > emulator_error_max) {
+       landed_error = fabs(ao_test_landed_height - landed_height);
+       landed_time_error = ao_test_landed_time - landed_time;
+       if (drogue_error > emulator_error_max || main_error > emulator_error_max ||
+           landed_time_error > emulator_error_max || landed_error > emulator_height_error_max) {
                printf ("%s %s\n",
                        emulator_app, emulator_name);
                printf ("\tApogee error %g\n", drogue_error);
                printf ("\tMain error %g\n", main_error);
-               printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f\n",
+               printf ("\tLanded height error %g\n", landed_error);
+               printf ("\tLanded time error %g\n", landed_time_error);
+               printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n",
                        ao_test_max_height, ao_test_max_height_time,
-                       ao_test_main_height, ao_test_main_height_time);
-               printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f\n",
-                       drogue_height, drogue_time, main_height, main_time);
+                       ao_test_main_height, ao_test_main_height_time,
+                       ao_test_landed_height, ao_test_landed_time);
+               printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n",
+                       drogue_height, drogue_time, main_height, main_time,
+                       landed_height, landed_time);
                exit (1);
        }
        exit(0);
@@ -274,14 +290,27 @@ ao_insert(void)
                if (ao_test_max_height < height) {
                        ao_test_max_height = height;
                        ao_test_max_height_time = time;
+                       ao_test_landed_height = height;
+                       ao_test_landed_time = time;
                }
                if (height > ao_config.main_deploy) {
                        ao_test_main_height_time = time;
                        ao_test_main_height = height;
                }
 
+               if (ao_test_landed_height > height) {
+                       ao_test_landed_height = height;
+                       ao_test_landed_time = time;
+               }
+
+               if (ao_flight_state == ao_flight_landed && !landed_set) {
+                       landed_set = 1;
+                       landed_time = time;
+                       landed_height = height;
+               }
+
                if (!ao_summary) {
-                       printf("%7.2f height %g accel %g state %s k_height %g k_speed %g k_accel %g drogue %d main %d error %d\n",
+                       printf("%7.2f height %8.2f accel %8.3f state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
                               time,
                               height,
                               accel,
@@ -289,11 +318,13 @@ ao_insert(void)
                               ao_k_height / 65536.0,
                               ao_k_speed / 65536.0 / 16.0,
                               ao_k_accel / 65536.0 / 16.0,
+                              ao_avg_height,
                               drogue_height,
                               main_height,
                               ao_error_h_sq_avg);
-                       if (ao_flight_state == ao_flight_landed)
-                               ao_test_exit();
+                       
+//                     if (ao_flight_state == ao_flight_landed)
+//                             ao_test_exit();
                }
        }
 }
@@ -495,6 +526,17 @@ ao_sleep(void *wchan)
                                        a = atoi(words[12]);
                                        b = atoi(words[14]);
                                }
+                       } else if (nword == 3 && strcmp(words[0], "BARO") == 0) {
+                               tick = strtol(words[1], NULL, 16);
+                               a = 16384 - 328;
+                               b = strtol(words[2], NULL, 10);
+                               type = 'A';
+                               if (!ao_flight_started) {
+                                       ao_flight_ground_accel = 16384 - 328;
+                                       ao_config.accel_plus_g = 16384 - 328;
+                                       ao_config.accel_minus_g = 16384 + 328;
+                                       ao_flight_started = 1;
+                               }
                        } else if (nword == 2 && strcmp(words[0], "TELEM") == 0) {
                                char    *hex = words[1];
                                char    elt[3];

diff --git a/src/ao_kalman.c b/src/ao_kalman.c
index 4e6cbb06c332561939589935aea4da8c65022d46..ab97fc34b91db0673812d7fccd4f77bff11a3742 100644 (file)
--- a/src/ao_kalman.c
+++ b/src/ao_kalman.c
@@ -38,6 +38,8 @@ __pdata int16_t                       ao_height;
 __pdata int16_t                        ao_speed;
 __pdata int16_t                        ao_accel;
 __pdata int16_t                        ao_max_height;
+static __pdata int32_t         ao_avg_height_scaled;
+__pdata int16_t                        ao_avg_height;
 
 __pdata int16_t                        ao_error_h;
 __pdata int16_t                        ao_error_h_sq_avg;
@@ -275,6 +277,18 @@ ao_kalman(void)
        ao_accel = from_fix(ao_k_accel);
        if (ao_height > ao_max_height)
                ao_max_height = ao_height;
+#ifdef AO_FLIGHT_TEST
+       if (ao_sample_tick - ao_sample_prev_tick > 50) {
+               ao_avg_height = ao_height;
+       } else if (ao_sample_tick - ao_sample_prev_tick > 5) {
+               ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height;
+               ao_avg_height = (ao_avg_height_scaled + 3) >> 2;
+       } else 
+#endif
+       {
+               ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height;
+               ao_avg_height = (ao_avg_height_scaled + 15) >> 5;
+       }
 #ifdef AO_FLIGHT_TEST
        ao_sample_prev_tick = ao_sample_tick;
 #endif