@@ -61,14 +61,14 @@ static void
61
61
ao_kalman_predict (void )
62
62
{
63
63
#ifdef AO_FLIGHT_TEST
64
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
64
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
65
65
ao_k_height += ((ao_k_t ) ao_speed * AO_K_STEP_1 +
66
66
(ao_k_t ) ao_accel * AO_K_STEP_2_2_1 ) >> 4 ;
67
67
ao_k_speed += (ao_k_t ) ao_accel * AO_K_STEP_1 ;
68
68
69
69
return ;
70
70
}
71
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
71
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
72
72
ao_k_height += ((ao_k_t ) ao_speed * AO_K_STEP_10 +
73
73
(ao_k_t ) ao_accel * AO_K_STEP_2_2_10 ) >> 4 ;
74
74
ao_k_speed += (ao_k_t ) ao_accel * AO_K_STEP_10 ;
@@ -149,13 +149,13 @@ ao_kalman_correct_baro(void)
149
149
{
150
150
ao_kalman_err_height ();
151
151
#ifdef AO_FLIGHT_TEST
152
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
152
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
153
153
ao_k_height += (ao_k_t ) AO_BARO_K0_1 * ao_error_h ;
154
154
ao_k_speed += (ao_k_t ) AO_BARO_K1_1 * ao_error_h ;
155
155
ao_k_accel += (ao_k_t ) AO_BARO_K2_1 * ao_error_h ;
156
156
return ;
157
157
}
158
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
158
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
159
159
ao_k_height += (ao_k_t ) AO_BARO_K0_10 * ao_error_h ;
160
160
ao_k_speed += (ao_k_t ) AO_BARO_K1_10 * ao_error_h ;
161
161
ao_k_accel += (ao_k_t ) AO_BARO_K2_10 * ao_error_h ;
@@ -189,7 +189,7 @@ ao_kalman_correct_both(void)
189
189
ao_kalman_err_accel ();
190
190
191
191
#ifdef AO_FLIGHT_TEST
192
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
192
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 50 ) {
193
193
if (ao_flight_debug ) {
194
194
printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n" ,
195
195
ao_k_speed / (65536.0 * 16.0 ),
@@ -210,7 +210,7 @@ ao_kalman_correct_both(void)
210
210
(ao_k_t ) AO_BOTH_K21_1 * ao_error_a ;
211
211
return ;
212
212
}
213
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
213
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
214
214
if (ao_flight_debug ) {
215
215
printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n" ,
216
216
ao_k_speed / (65536.0 * 16.0 ),
@@ -260,7 +260,7 @@ ao_kalman_correct_accel(void)
260
260
ao_kalman_err_accel ();
261
261
262
262
#ifdef AO_FLIGHT_TEST
263
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
263
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 5 ) {
264
264
ao_k_height += (ao_k_t ) AO_ACCEL_K0_10 * ao_error_a ;
265
265
ao_k_speed += (ao_k_t ) AO_ACCEL_K1_10 * ao_error_a ;
266
266
ao_k_accel += (ao_k_t ) AO_ACCEL_K2_10 * ao_error_a ;
@@ -325,9 +325,9 @@ ao_kalman(void)
325
325
ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height ;
326
326
#endif
327
327
#ifdef AO_FLIGHT_TEST
328
- if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 50 )
328
+ if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 50 )
329
329
ao_avg_height = (ao_avg_height_scaled + 1 ) >> 1 ;
330
- else if ((int16_t ) (ao_sample_tick - ao_sample_prev_tick ) > 5 )
330
+ else if ((AO_TICK_SIGNED ) (ao_sample_tick - ao_sample_prev_tick ) > 5 )
331
331
ao_avg_height = (ao_avg_height_scaled + 7 ) >> 4 ;
332
332
else
333
333
#endif
0 commit comments