While on the pad, zero out velocity every second

We integrate acceleration to get velocity, but that means sitting on the pad
for a long time can add substantial error to the velocity value. Each
second, take the velocity value from a full second ago and subtract that out
of the current velocity. Once we detect boost, this will stop, which means
that as long as we detect boost within a second, we won't have subtracted
out any "real" velocity.

This keeps the pad velocity hovering around zero, which is pretty useful.

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

diff --git a/ao_flight.c b/ao_flight.c
index 48f7120260ef981b5c58c66fae449ab58334891d..3e747d0668f02399c0d3ec15850d38e36dead71a 100644 (file)
--- a/ao_flight.c
+++ b/ao_flight.c
@@ -125,6 +125,8 @@ __pdata int16_t ao_raw_accel, ao_raw_accel_prev, ao_raw_pres;
  */
 __pdata int32_t        ao_flight_vel;
 __pdata int32_t ao_min_vel;
  */
 __pdata int32_t        ao_flight_vel;
 __pdata int32_t ao_min_vel;

+__pdata int32_t        ao_old_vel;
+__pdata int16_t ao_old_vel_tick;

 __xdata int32_t ao_raw_accel_sum, ao_raw_pres_sum;
 
 /* Landing is detected by getting constant readings from both pressure and accelerometer
 __xdata int32_t ao_raw_accel_sum, ao_raw_pres_sum;
 
 /* Landing is detected by getting constant readings from both pressure and accelerometer


@@ -211,6 +213,8 @@ ao_flight(void)
                        ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy);
                        ao_flight_vel = 0;
                        ao_min_vel = 0;
                        ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy);
                        ao_flight_vel = 0;
                        ao_min_vel = 0;

+                       ao_old_vel = ao_flight_vel;
+                       ao_old_vel_tick = ao_flight_tick;

 
                        /* Go to launchpad state if the nose is pointing up */
                        ao_config_get();
 
                        /* Go to launchpad state if the nose is pointing up */
                        ao_config_get();


@@ -241,6 +245,16 @@ ao_flight(void)
                        break;
                case ao_flight_launchpad:
 
                        break;
                case ao_flight_launchpad:
 

+                       /* Trim velocity
+                        *
+                        * Once a second, remove any velocity from
+                        * a second ago
+                        */
+                       if ((int16_t) (ao_flight_tick - ao_old_vel_tick) >= AO_SEC_TO_TICKS(1)) {
+                               ao_old_vel_tick = ao_flight_tick;
+                               ao_flight_vel -= ao_old_vel;
+                               ao_old_vel = ao_flight_vel;
+                       }

                        /* pad to boost:
                         *
                         * accelerometer: > 2g AND velocity > 5m/s
                        /* pad to boost:
                         *
                         * accelerometer: > 2g AND velocity > 5m/s