diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f39dae63d6..c0c942e314 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1777,20 +1777,24 @@ Commander::run() _actuator_armed.prearmed = getPrearmState(); + const hrt_abstime now = hrt_absolute_time(); + + // Run arming checks @ 10Hz + if (now - _last_health_and_arming_check >= 100_ms || _status_changed || nav_state_or_failsafe_changed) { + _last_health_and_arming_check = now; + + perf_begin(_preflight_check_perf); + _health_and_arming_checks.update(); + _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); + perf_end(_preflight_check_perf); + + check_and_inform_ready_for_takeoff(); + } + // publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed - if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_or_failsafe_changed + if (now - _vehicle_status.timestamp >= 500_ms || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { - // Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already) - if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status.calibration_enabled) { - perf_begin(_preflight_check_perf); - _health_and_arming_checks.update(); - _vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state); - perf_end(_preflight_check_perf); - - check_and_inform_ready_for_takeoff(); - } - // publish actuator_armed first (used by output modules) _actuator_armed.armed = _arm_state_machine.isArmed(); _actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 90dd689c0d..02c481732c 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -317,6 +317,7 @@ private: perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")}; HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status}; + hrt_abstime _last_health_and_arming_check{0}; const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; HomePosition _home_position{_vehicle_status_flags};