From ae1e9e06c184ec3916fdaf78ef04228fa1f12b32 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:16:30 +1000 Subject: [PATCH] AP_Arming: added arming check for conflicting notch modes --- libraries/AP_Arming/AP_Arming.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index d8baeead15..73a048e4e4 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -394,6 +394,14 @@ bool AP_Arming::ins_checks(bool report) check_failed(ARMING_CHECK_INS, report, "%s", failure_msg); return false; } +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 + if (ins.gyro_harmonic_notch_enabled(0) && + ins.gyro_harmonic_notch_enabled(1) && + ins.get_gyro_harmonic_notch_tracking_mode(0) != HarmonicNotchDynamicMode::Fixed && + ins.get_gyro_harmonic_notch_tracking_mode(0) != ins.get_gyro_harmonic_notch_tracking_mode(1)) { + check_failed(ARMING_CHECK_INS, report, "conflicting notch filters"); + } +#endif } #if HAL_GYROFFT_ENABLED