diff options
| author | Keith Packard <keithp@keithp.com> | 2011-03-31 20:53:12 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2011-03-31 20:53:12 -0700 |
| commit | 8a14142e7b37031a51409f121b913fe793bf3603 (patch) | |
| tree | af0097dc8990efd7687c876d358ad6845ffd9ed2 | |
| parent | a9dae18a664f70b668159487015e61be7f776926 (diff) | |
altos: Baro useful ceiling is MSL, not AGL
Use MSL instead of AGL for detecting over-range baro sensor values.
Always trust baro sensor during descent; it'll get there eventually.
Signed-off-by: Keith Packard <keithp@keithp.com>
| -rw-r--r-- | src/ao_kalman.c | 11 |
1 files changed, 5 insertions, 6 deletions
diff --git a/src/ao_kalman.c b/src/ao_kalman.c index ee99f375..fb75fba9 100644 --- a/src/ao_kalman.c +++ b/src/ao_kalman.c @@ -89,7 +89,9 @@ ao_kalman_err_height(void) ao_error_h_sq_avg += (e * e) >> 4; #endif - height_distrust = ao_sample_height - AO_MAX_BARO_HEIGHT; + if (ao_flight_state >= ao_flight_drogue) + return; + height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; #if HAS_ACCEL /* speed is stored * 16, but we need to ramp between 200 and 328, so * we want to multiply by 2. The result is a shift by 3. @@ -100,10 +102,7 @@ ao_kalman_err_height(void) else if (speed_distrust > height_distrust) height_distrust = speed_distrust; #endif - if (height_distrust <= 0) - height_distrust = 0; - - if (height_distrust) { + if (height_distrust > 0) { #ifdef AO_FLIGHT_TEST int old_ao_error_h = ao_error_h; #endif @@ -113,7 +112,7 @@ ao_kalman_err_height(void) #ifdef AO_FLIGHT_TEST if (ao_flight_debug) { printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", - (double) (ao_sample_height - AO_MAX_BARO_HEIGHT), + (double) (ao_sample_alt - AO_MAX_BARO_HEIGHT), (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, height_distrust / 256.0, old_ao_error_h, ao_error_h); |