fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: run arming checks @ 10Hz

Browse Source 
Required for things like RC loss.
This commit is contained in:
Beat Küng 2022-09-06 09:26:19 +02:00 committed by Daniel Agar
parent 6fda555cba
commit c72c580a0b
2 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/commander/Commander.cpp
View File

@ -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();

1
src/modules/commander/Commander.hpp
View File

@ -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};