+#endif
+
+ height_distrust = ao_raw_height - 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.
+ */
+ 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)
+ height_distrust = speed_distrust;
+#endif
+ if (height_distrust <= 0)
+ height_distrust = 0;
+
+ if (height_distrust) {
+#ifdef AO_FLIGHT_TEST
+ int old_ao_error_h = ao_error_h;
+#endif
+ if (height_distrust > 0x100)
+ height_distrust = 0x100;
+ ao_error_h = (int16_t) (((int32_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",
+ (double) (ao_raw_height - 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);
+ }
+#endif
+ }