While on the pad, zero out velocity every second
authorKeith Packard <keithp@keithp.com>
Mon, 18 May 2009 05:24:53 +0000 (22:24 -0700)
committerKeith Packard <keithp@keithp.com>
Mon, 18 May 2009 05:24:53 +0000 (22:24 -0700)
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>
ao_flight.c

index 48f7120..3e747d0 100644 (file)
@@ -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_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
@@ -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_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();
@@ -241,6 +245,16 @@ ao_flight(void)
                        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