X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fao_kalman.c;h=fb75fba93e5d1c8c15f8cd411a9423ad8a4d9f62;hb=8a14142e7b37031a51409f121b913fe793bf3603;hp=ee99f375635c0f05f1c2c4b64ce946e2b44a92ba;hpb=a9dae18a664f70b668159487015e61be7f776926;p=fw%2Faltos

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);