X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_kalman.c;h=aaf0595f01ab7d5e345ab5997983386f78bd4c1e;hb=2f32a19aedb7d0e33899038e3fb04fe8a773291a;hp=9aea1f14762f6139d9fb8dce932b98a6c946068d;hpb=24167015705ae831692b95735968b04a876f935e;p=fw%2Faltos

diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index 9aea1f14..aaf0595f 100644
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -23,83 +24,88 @@
 #include "ao_sample.h"
 #include "ao_kalman.h"
 
+static ao_k_t		ao_k_height;
+static ao_k_t		ao_k_speed;
+static ao_k_t		ao_k_accel;
 
-static __pdata int32_t		ao_k_height;
-static __pdata int32_t		ao_k_speed;
-static __pdata int32_t		ao_k_accel;
+#define AO_K_STEP_100		to_fix_v(0.01)
+#define AO_K_STEP_2_2_100	to_fix_v(0.00005)
 
-#define AO_K_STEP_100		to_fix16(0.01)
-#define AO_K_STEP_2_2_100	to_fix16(0.00005)
+#define AO_K_STEP_10		to_fix_v(0.1)
+#define AO_K_STEP_2_2_10	to_fix_v(0.005)
 
-#define AO_K_STEP_10		to_fix16(0.1)
-#define AO_K_STEP_2_2_10	to_fix16(0.005)
+#define AO_K_STEP_1		to_fix_v(1)
+#define AO_K_STEP_2_2_1		to_fix_v(0.5)
 
-#define AO_K_STEP_1		to_fix16(1)
-#define AO_K_STEP_2_2_1		to_fix16(0.5)
+ao_v_t			ao_height;
+ao_v_t			ao_speed;
+ao_v_t			ao_accel;
+ao_v_t			ao_max_height;
+static ao_k_t		ao_avg_height_scaled;
+ao_v_t			ao_avg_height;
 
-__pdata int16_t			ao_height;
-__pdata int16_t			ao_speed;
-__pdata int16_t			ao_accel;
-__xdata int16_t			ao_max_height;
-static __pdata int32_t		ao_avg_height_scaled;
-__xdata int16_t			ao_avg_height;
+ao_v_t			ao_error_h;
+#if !HAS_ACCEL || AO_FLIGHT_TEST
+#define AO_ERROR_H_SQ_AVG	1
+#endif
 
-__pdata int16_t			ao_error_h;
-__pdata int16_t			ao_error_h_sq_avg;
+#if AO_ERROR_H_SQ_AVG
+ao_v_t			ao_error_h_sq_avg;
+#endif
 
 #if HAS_ACCEL
-__pdata int16_t			ao_error_a;
+ao_v_t			ao_error_a;
 #endif
 
 static void
 ao_kalman_predict(void)
 {
 #ifdef AO_FLIGHT_TEST
-	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) {
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1;
 
 		return;
 	}
-	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10;
 
 		return;
 	}
 	if (ao_flight_debug) {
 		printf ("predict speed %g + (%g * %g) = %g\n",
 			ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
-			(ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+			(ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
 	}
 #endif
-	ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
-			(int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
-	ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+	ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 +
+			(ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+	ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
 }
 
+#if HAS_BARO
 static void
 ao_kalman_err_height(void)
 {
-	int16_t	e;
-	int16_t height_distrust;
+#if AO_ERROR_H_SQ_AVG
+	ao_v_t	e;
+#endif
+	ao_v_t height_distrust;
 #if HAS_ACCEL
-	int16_t	speed_distrust;
+	ao_v_t	speed_distrust;
 #endif
 
-	ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16);
+	ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16);
 
+#if AO_ERROR_H_SQ_AVG
 	e = ao_error_h;
 	if (e < 0)
 		e = -e;
 	if (e > 127)
 		e = 127;
-#if HAS_ACCEL
-	ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2;
-	ao_error_h_sq_avg += (e * e) >> 2;
-#else
 	ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4;
 	ao_error_h_sq_avg += (e * e) >> 4;
 #endif
@@ -108,13 +114,13 @@ ao_kalman_err_height(void)
 		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
+	/* speed is stored * 16, but we need to ramp between 248 and 328, so
 	 * we want to multiply by 2. The result is a shift by 3.
 	 */
 	speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1);
-	if (speed_distrust <= 0)
-		speed_distrust = 0;
-	else if (speed_distrust > height_distrust)
+	if (speed_distrust > AO_MAX_SPEED_DISTRUST)
+		speed_distrust = AO_MAX_SPEED_DISTRUST;
+	if (speed_distrust > height_distrust)
 		height_distrust = speed_distrust;
 #endif
 	if (height_distrust > 0) {
@@ -123,7 +129,7 @@ ao_kalman_err_height(void)
 #endif
 		if (height_distrust > 0x100)
 			height_distrust = 0x100;
-		ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+		ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8);
 #ifdef AO_FLIGHT_TEST
 		if (ao_flight_debug) {
 			printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
@@ -135,36 +141,39 @@ ao_kalman_err_height(void)
 #endif
 	}
 }
+#endif
 
+#if HAS_BARO
 static void
 ao_kalman_correct_baro(void)
 {
 	ao_kalman_err_height();
 #ifdef AO_FLIGHT_TEST
-	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_1 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_1 * ao_error_h;
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) {
+		ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_1 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_1 * ao_error_h;
 		return;
 	}
-	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_10 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_10 * ao_error_h;
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
+		ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_10 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_10 * ao_error_h;
 		return;
 	}
 #endif
-	ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
-	ao_k_speed  += (int32_t) AO_BARO_K1_100 * ao_error_h;
-	ao_k_accel  += (int32_t) AO_BARO_K2_100 * ao_error_h;
+	ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h;
+	ao_k_speed  += (ao_k_t) AO_BARO_K1_100 * ao_error_h;
+	ao_k_accel  += (ao_k_t) AO_BARO_K2_100 * ao_error_h;
 }
+#endif
 
 #if HAS_ACCEL
 
 static void
 ao_kalman_err_accel(void)
 {
-	int32_t	accel;
+	ao_k_t	accel;
 
 	accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
 
@@ -172,7 +181,7 @@ ao_kalman_err_accel(void)
 	ao_error_a = (accel - ao_k_accel) >> 16;
 }
 
-#ifndef FORCE_ACCEL
+#if !defined(FORCE_ACCEL) && HAS_BARO
 static void
 ao_kalman_correct_both(void)
 {
@@ -180,46 +189,46 @@ ao_kalman_correct_both(void)
 	ao_kalman_err_accel();
 
 #ifdef AO_FLIGHT_TEST
-	if (ao_sample_tick - ao_sample_prev_tick > 50) {
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50) {
 		if (ao_flight_debug) {
 			printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
 				ao_k_speed / (65536.0 * 16.0),
 				(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_1 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_1 * ao_error_h +
-			(int32_t) AO_BOTH_K01_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_1 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_1 * ao_error_h +
-			(int32_t) AO_BOTH_K11_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_1 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_1 * ao_error_h +
-			(int32_t) AO_BOTH_K21_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_1 * ao_error_a;
 		return;
 	}
-	if (ao_sample_tick - ao_sample_prev_tick > 5) {
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
 		if (ao_flight_debug) {
 			printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n",
 				ao_k_speed / (65536.0 * 16.0),
 				(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_10 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_10 * ao_error_h +
-			(int32_t) AO_BOTH_K01_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_10 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_10 * ao_error_h +
-			(int32_t) AO_BOTH_K11_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_10 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_10 * ao_error_h +
-			(int32_t) AO_BOTH_K21_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_10 * ao_error_a;
 		return;
 	}
 	if (ao_flight_debug) {
@@ -228,19 +237,19 @@ ao_kalman_correct_both(void)
 			(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
 			(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
 			(ao_k_speed +
-			 (int32_t) AO_BOTH_K10_100 * ao_error_h +
-			 (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+			 (ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+			 (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
 	}
 #endif
 	ao_k_height +=
-		(int32_t) AO_BOTH_K00_100 * ao_error_h +
-		(int32_t) AO_BOTH_K01_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K00_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K01_100 * ao_error_a;
 	ao_k_speed +=
-		(int32_t) AO_BOTH_K10_100 * ao_error_h +
-		(int32_t) AO_BOTH_K11_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K11_100 * ao_error_a;
 	ao_k_accel +=
-		(int32_t) AO_BOTH_K20_100 * ao_error_h +
-		(int32_t) AO_BOTH_K21_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K20_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K21_100 * ao_error_a;
 }
 
 #else
@@ -250,15 +259,17 @@ ao_kalman_correct_accel(void)
 {
 	ao_kalman_err_accel();
 
-	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
-		ao_k_speed  += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
-		ao_k_accel  += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+#ifdef AO_FLIGHT_TEST
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5) {
+		ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a;
+		ao_k_speed  += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a;
+		ao_k_accel  += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a;
 		return;
 	}
-	ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
-	ao_k_speed  += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
-	ao_k_accel  += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+#endif
+	ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a;
+	ao_k_speed  += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a;
+	ao_k_accel  += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a;
 }
 
 #endif /* else FORCE_ACCEL */
@@ -268,6 +279,7 @@ void
 ao_kalman(void)
 {
 	ao_kalman_predict();
+#if HAS_BARO
 #if HAS_ACCEL
 	if (ao_flight_state <= ao_flight_coast) {
 #ifdef FORCE_ACCEL
@@ -278,16 +290,25 @@ ao_kalman(void)
 	} else
 #endif
 		ao_kalman_correct_baro();
+#else
+#if HAS_ACCEL
+	ao_kalman_correct_accel();
+#endif
+#endif
 	ao_height = from_fix(ao_k_height);
 	ao_speed = from_fix(ao_k_speed);
 	ao_accel = from_fix(ao_k_accel);
 	if (ao_height > ao_max_height)
 		ao_max_height = ao_height;
+#if HAS_BARO
 	ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height;
+#else
+	ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height;
+#endif
 #ifdef AO_FLIGHT_TEST
-	if (ao_sample_tick - ao_sample_prev_tick > 50)
+	if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 50)
 		ao_avg_height = (ao_avg_height_scaled + 1) >> 1;
-	else if (ao_sample_tick - ao_sample_prev_tick > 5)
+	else if ((int16_t) (ao_sample_tick - ao_sample_prev_tick) > 5)
 		ao_avg_height = (ao_avg_height_scaled + 7) >> 4;
 	else 
 #endif