Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Arming: add system check of main loop rate

This commit is contained in:
Randy Mackay 2022-12-05 12:55:07 +09:00
parent 1cc033746f
commit 6713caba55
1 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Arming/AP_Arming.cpp
View File

@ -50,6 +50,7 @@
#include <AP_Mount/AP_Mount.h> #include <AP_Mount/AP_Mount.h>
#include <AP_OpenDroneID/AP_OpenDroneID.h> #include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Scheduler/AP_Scheduler.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS #if HAL_MAX_CAN_PROTOCOL_DRIVERS
#include <AP_CANManager/AP_CANManager.h> #include <AP_CANManager/AP_CANManager.h>
@ -960,6 +961,16 @@ bool AP_Arming::system_checks(bool report)
check_failed(ARMING_CHECK_SYSTEM, report, "Param storage failed"); check_failed(ARMING_CHECK_SYSTEM, report, "Param storage failed");
return false; return false;
} }
// check main loop rate is at least 90% of expected value
const float actual_loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
const uint16_t expected_loop_rate = AP::scheduler().get_loop_rate_hz();
const float loop_rate_pct = actual_loop_rate / expected_loop_rate;
if (loop_rate_pct < 0.90) {
check_failed(ARMING_CHECK_SYSTEM, report, "Main loop slow (%uHz < %uHz)", (unsigned)actual_loop_rate, (unsigned)expected_loop_rate);
return false;
}
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
const AP_Terrain *terrain = AP_Terrain::get_singleton(); const AP_Terrain *terrain = AP_Terrain::get_singleton();
if ((terrain != nullptr) && terrain->init_failed()) { if ((terrain != nullptr) && terrain->init_failed()) {