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

manual_control: implement (dis)arming via command

Browse Source
This commit is contained in:
Julian Oes
2021-04-15 14:08:26 +02:00
committed by Matthias Grob
parent 8876af9150
commit 1c15cc11d8
3 changed files with 88 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+19 -24
View File
@@ -921,6 +921,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(roundf(cmd.param2)) == 21196);
const bool cmd_from_manual_stick = (static_cast<int>(roundf(cmd.param3)) == 1);
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
if (cmd_arms) {
@@ -948,8 +957,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
}
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
// 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 && cmd_arms) {
@@ -964,7 +971,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
arming_res = arm(arm_disarm_reason_t::command_external);
} else {
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
if (cmd_from_manual_stick) {
arming_res = arm(arm_disarm_reason_t::rc_stick, !forced);
} else {
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
}
}
} else {
@@ -972,7 +983,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
arming_res = disarm(arm_disarm_reason_t::command_external);
} else {
arming_res = disarm(arm_disarm_reason_t::command_internal);
if (cmd_from_manual_stick) {
arming_res = disarm(arm_disarm_reason_t::rc_stick);
} else {
arming_res = disarm(arm_disarm_reason_t::command_internal);
}
}
}
@@ -2383,26 +2398,6 @@ Commander::run()
_status.rc_signal_lost = false;
const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
if (rc_arming_enabled) {
if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
disarm(arm_disarm_reason_t::rc_stick);
}
if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
arm(arm_disarm_reason_t::rc_stick);
} else {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first\t");
events::send(events::ID("commander_arming_denied_not_manual"), {events::Log::Error, events::LogInternal::Info},
"Not arming! Switch to a manual mode first");
tune_negative(true);
}
}
}
// abort autonomous mode and switch to position mode if sticks are moved significantly
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on