diff options
| author | Keith Packard <keithp@keithp.com> | 2009-05-17 22:24:53 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2009-05-17 22:24:53 -0700 |
| commit | 69616104813fc5ba89fb3128d04fb9328961c59c (patch) | |
| tree | 57b1bbcff9013287c642c2050a3f64db6a033988 | |
| parent | 1234694eb903b204488ddc7cb30bcfe34bf1e677 (diff) | |
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>
| -rw-r--r-- | ao_flight.c | 14 |
1 files changed, 14 insertions, 0 deletions
diff --git a/ao_flight.c b/ao_flight.c index 48f71202..3e747d06 100644 --- 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_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 |