altoslib: Missed a couple of easy mini voltage API changes

[fw/altos] / src / core / ao_kalman.c

diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c
index ee01949e5e54a416bc9737e56bb20a03261f8bc1..9aea1f14762f6139d9fb8dce932b98a6c946068d 100644 (file)
--- a/src/core/ao_kalman.c
+++ b/src/core/ao_kalman.c
@@ -17,10 +17,13 @@
 
 #ifndef AO_FLIGHT_TEST
 #include "ao.h"
+#include "ao_flight.h"
 #endif
 
+#include "ao_sample.h"
 #include "ao_kalman.h"
 
+
 static __pdata int32_t         ao_k_height;
 static __pdata int32_t         ao_k_speed;
 static __pdata int32_t         ao_k_accel;
@@ -37,9 +40,9 @@ static __pdata int32_t                ao_k_accel;
 __pdata int16_t                        ao_height;
 __pdata int16_t                        ao_speed;
 __pdata int16_t                        ao_accel;
-__pdata int16_t                        ao_max_height;
+__xdata int16_t                        ao_max_height;
 static __pdata int32_t         ao_avg_height_scaled;
-__pdata int16_t                        ao_avg_height;
+__xdata int16_t                        ao_avg_height;
 
 __pdata int16_t                        ao_error_h;
 __pdata int16_t                        ao_error_h_sq_avg;
@@ -163,12 +166,13 @@ ao_kalman_err_accel(void)
 {
        int32_t accel;
 
-       accel = (ao_ground_accel - ao_sample_accel) * ao_accel_scale;
+       accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
 
        /* Can't use ao_accel here as it is the pre-prediction value still */
        ao_error_a = (accel - ao_k_accel) >> 16;
 }
 
+#ifndef FORCE_ACCEL
 static void
 ao_kalman_correct_both(void)
 {
@@ -239,7 +243,8 @@ ao_kalman_correct_both(void)
                (int32_t) AO_BOTH_K21_100 * ao_error_a;
 }
 
-#ifdef FORCE_ACCEL
+#else
+
 static void
 ao_kalman_correct_accel(void)
 {
@@ -255,7 +260,8 @@ ao_kalman_correct_accel(void)
        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
+
+#endif /* else FORCE_ACCEL */
 #endif /* HAS_ACCEL */
 
 void
@@ -286,7 +292,4 @@ ao_kalman(void)
        else 
 #endif
                ao_avg_height = (ao_avg_height_scaled + 63) >> 7;
-#ifdef AO_FLIGHT_TEST
-       ao_sample_prev_tick = ao_sample_tick;
-#endif
 }