diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 4415ce7f2f..d90893abc0 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #if HAL_WITH_UAVCAN #include @@ -363,6 +364,15 @@ bool AP_Arming::ins_checks(bool report) check_failed(ARMING_CHECK_INS, report, "%s", failure_msg); return false; } + +#if HAL_GYROFFT_ENABLED + // Check that the noise analyser works + AP_GyroFFT *fft = AP::fft(); + if (fft != nullptr && !fft->calibration_check()) { + check_failed(ARMING_CHECK_INS, report, "FFT self-test failed"); + return false; + }; +#endif } return true; @@ -950,6 +960,13 @@ bool AP_Arming::disarm() } #endif // HAL_HAVE_SAFETY_SWITCH +#if HAL_GYROFFT_ENABLED + AP_GyroFFT *fft = AP::fft(); + if (fft != nullptr) { + fft->save_params_on_disarm(); + } +#endif + return true; }