Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

commander: fully support MAVLink force arming

Browse Source 
When a magic number is added as param2 for the MAVLink ARM_DISARM
command not just disarming but also arming should be enforced.
This commit is contained in:
Julian Oes
2019-10-29 08:49:17 +01:00
parent 51ba80f33b
commit 300bc0a332
1 changed files with 39 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+39 -37
View File
@@ -804,53 +804,55 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
bool cmd_arms = (static_cast<int>(cmd.param1 + 0.5f) == 1);
// Arm/disarm is enforced only when param2 is set to a magic number.
const bool enforce_in_air = (static_cast<int>(std::round(cmd.param2)) == 21196);
// Arm/disarm is enforced when param2 is set to a magic number.
const bool enforce = (static_cast<int>(std::round(cmd.param2)) == 21196);
if (!enforce) {
if (!land_detector.landed && !is_ground_rover(&status)) {
if (cmd_arms) {
if (armed_local->armed) {
mavlink_log_warning(&mavlink_log_pub, "Arming denied! Already armed");
} else {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Not landed");
}
if (!enforce_in_air && !land_detector.landed && !is_ground_rover(&status)) {
if (cmd_arms) {
if (armed_local->armed) {
mavlink_log_warning(&mavlink_log_pub, "Arming denied! Already armed");
} else {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Not landed");
mavlink_log_critical(&mavlink_log_pub, "Disarming denied! Not landed");
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
// Flick to inair restore first if this comes from an onboard system
if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id) {
status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
} else {
mavlink_log_critical(&mavlink_log_pub, "Disarming denied! Not landed");
}
// Refuse to arm if preflight checks have failed
if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON
&& !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Preflight checks have failed");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
// Refuse to arm if in manual with non-zero throttle
if (cmd_arms
&& (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)
&& (sp_man.z > 0.1f)) {
// Flick to inair restore first if this comes from an onboard system
if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id) {
status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
} else {
// Refuse to arm if preflight checks have failed
if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON
&& !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Preflight checks have failed");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
// Refuse to arm if in manual with non-zero throttle
if (cmd_arms
&& (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)
&& (sp_man.z > 0.1f)) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Manual throttle not zero");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Manual throttle not zero");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
}
}
transition_result_t arming_res = arm_disarm(cmd_arms, true, &mavlink_log_pub, "Arm/Disarm component command");
transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, "Arm/Disarm component command");
if (arming_res == TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;