fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: handle denying disarm when not landed centrally

Browse Source
This commit is contained in:
Matthias Grob
2021-11-09 16:05:25 +01:00
parent af54ac7cdb
commit 0a02d8e774
2 changed files with 29 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+28 -44
View File
@@ -631,8 +631,29 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
return arming_res;
}
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
{
if (!forced) {
const bool landed = (_land_detector.landed || _land_detector.maybe_landed || is_ground_rover(_status));
const bool mc_manual_thrust_mode = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_climb_rate_enabled;
const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::rc_stick)
|| (calling_reason == arm_disarm_reason_t::rc_switch)
|| (calling_reason == arm_disarm_reason_t::rc_button);
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
if (calling_reason != arm_disarm_reason_t::rc_stick) {
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
events::send(events::ID("commander_disarming_denied_not_landed"),
{events::Log::Critical, events::LogInternal::Info},
"Disarming denied, not landed");
}
return TRANSITION_DENIED;
}
}
transition_result_t arming_res = arming_state_transition(_status,
_safety,
vehicle_status_s::ARMING_STATE_STANDBY,
@@ -873,32 +894,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
if (_armed.armed) {
mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed\t");
events::send(events::ID("commander_arming_denied_already_armed"),
{events::Log::Warning, events::LogInternal::Info },
"Arming denied, already armed");
} else {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Not landed\t");
events::send(events::ID("commander_arming_denied_not_landed"),
{events::Log::Error, events::LogInternal::Info },
"Arming denied, not landed");
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
events::send(events::ID("commander_disarming_denied_not_landed"),
{events::Log::Critical, events::LogInternal::Info },
"Disarming denied, not landed");
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
// Flick to in-air restore first if this comes from an onboard system and from IO
if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
&& cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
@@ -914,7 +909,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
arming_res = arm(arm_disarm_reason, cmd.from_external || !forced);
} else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) {
arming_res = disarm(arm_disarm_reason);
arming_res = disarm(arm_disarm_reason, forced);
}
@@ -1917,20 +1912,9 @@ Commander::run()
_safety.safety_switch_available, _status);
// disarm if safety is now on and still armed
if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off) {
bool safety_disarm_allowed = (_status.hil_state == vehicle_status_s::HIL_STATE_OFF);
// prevent disarming via safety button if not landed
if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) {
if (!_land_detector.landed) {
safety_disarm_allowed = false;
}
}
if (safety_disarm_allowed) {
disarm(arm_disarm_reason_t::safety_button);
}
if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off
&& (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) {
disarm(arm_disarm_reason_t::safety_button);
}
// Notify the user if the status of the safety switch changes
@@ -2061,10 +2045,10 @@ Commander::run()
if (_auto_disarm_killed.get_state()) {
if (_armed.manual_lockdown) {
disarm(arm_disarm_reason_t::kill_switch);
disarm(arm_disarm_reason_t::kill_switch, true);
} else {
disarm(arm_disarm_reason_t::lockdown);
disarm(arm_disarm_reason_t::lockdown, true);
}
}
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -125,7 +125,7 @@ private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
transition_result_t disarm(arm_disarm_reason_t calling_reason);
transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
void battery_status_check();