+
+ height_distrust = ao_raw_height - AO_MAX_BARO_HEIGHT;
+#ifdef AO_FLIGHT_TEST
+ if (height_distrust > 0)
+ printf ("height_distrust %d\n", height_distrust);
+#endif
+#if HAS_ACCEL
+ speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> 4;
+#ifdef AO_FLIGHT_TEST
+ if (speed_distrust > 0)
+ printf ("speed distrust %d\n", speed_distrust);
+#endif
+ 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) {
+ if (height_distrust > 0x100)
+ height_distrust = 0x100;
+ ao_error_h = (int16_t) ((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8;
+ }