+ if (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_height +=
+ (int32_t) AO_BOTH_K00_1 * ao_error_h +
+ (int32_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_accel +=
+ (int32_t) AO_BOTH_K20_1 * ao_error_h +
+ (int32_t) AO_BOTH_K21_1 * ao_error_a;
+ return;
+ }