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

manual_control: support arming button

Browse Source 
The arming button required some refactoring in order to support to
toggle arm/disarm using the vehicle_command. Otherwise manual_control
would have to subscribe to the arming topic and we would spread out the
logic again, and risk race conditions.
This commit is contained in:
Julian Oes
2021-05-18 12:36:37 +02:00
committed by Matthias Grob
parent 9cbfa2ca95
commit bd0c1014d9
5 changed files with 62 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/events/enums.json
+4
View File
@@ -172,6 +172,10 @@
"13": {
"name": "unit_test",
"description": "unit tests"
},
"14": {
"name": "rc_button",
"description": "RC (button)"
}
}
},
src/modules/commander/Commander.cpp
+32 -8
View File
@@ -468,6 +468,8 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
case arm_disarm_reason_t::shutdown: return "shutdown request";
case arm_disarm_reason_t::unit_test: return "unit tests";
case arm_disarm_reason_t::rc_button: return "RC (button)";
}
return "";
@@ -911,18 +913,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
// for logic state parameters
const int param1_arm = static_cast<int>(roundf(cmd.param1));
if (param1_arm != 0 && param1_arm != 1) {
if (param1_arm != 0 && param1_arm != 1 && param1_arm != -1) {
mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1);
events::send<float>(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error,
"Unsupported ARM_DISARM param: {1:.3}", cmd.param1);
} else {
const bool cmd_arms = (param1_arm == 1);
// 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_manual_switch = (static_cast<int>(roundf(cmd.param3)) == 2);
const bool cmd_from_manual_button = (static_cast<int>(roundf(cmd.param3)) == 3);
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) {
@@ -933,7 +935,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
if (cmd_arms) {
if (param1_arm == 1) {
if (_armed.armed) {
mavlink_log_warning(&_mavlink_log_pub, "Arming denied! Already armed\t");
events::send(events::ID("commander_arming_denied_already_armed"),
@@ -960,14 +962,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
// 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) {
&& cmd_from_io && (param1_arm == 1)) {
_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
}
transition_result_t arming_res = TRANSITION_DENIED;
if (cmd_arms) {
if (param1_arm == 1) {
if (cmd.from_external) {
arming_res = arm(arm_disarm_reason_t::command_external);
@@ -975,12 +977,15 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (cmd_from_manual_stick) {
arming_res = arm(arm_disarm_reason_t::rc_stick, !forced);
} else if (cmd_from_manual_switch) {
arming_res = arm(arm_disarm_reason_t::rc_switch, !forced);
} else {
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
}
}
} else {
} else if (param1_arm == 0) {
if (cmd.from_external) {
arming_res = disarm(arm_disarm_reason_t::command_external);
@@ -988,10 +993,29 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (cmd_from_manual_stick) {
arming_res = disarm(arm_disarm_reason_t::rc_stick);
} else if (cmd_from_manual_switch) {
arming_res = disarm(arm_disarm_reason_t::rc_switch);
} else {
arming_res = disarm(arm_disarm_reason_t::command_internal);
}
}
} else if (param1_arm == -1) {
// -1 means toggle by a button
// This should come from an arming button internally, otherwise something is wrong.
if (!cmd.from_external && cmd_from_manual_button) {
if (_armed.armed) {
arming_res = disarm(arm_disarm_reason_t::rc_button);
} else {
arming_res = arm(arm_disarm_reason_t::rc_button);
}
} else {
PX4_WARN("ARM_DISARM toggle command ignored");
}
}
if (arming_res == TRANSITION_DENIED) {
@@ -1001,7 +1025,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
if (cmd_arms && (arming_res == TRANSITION_CHANGED) &&
if ((param1_arm == 1) && (arming_res == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) {
set_home_position();
src/modules/manual_control/ManualControl.cpp
+17 -11
View File
@@ -127,7 +127,7 @@ void ManualControl::Run()
if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) {
_previous_arm_gesture = true;
send_arm_command(true, ArmingOrigin::GESTURE);
send_arm_command(ArmingAction::ARM, ArmingOrigin::GESTURE);
} else if (!_selector.setpoint().arm_gesture) {
_previous_arm_gesture = false;
@@ -135,7 +135,7 @@ void ManualControl::Run()
if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) {
_previous_disarm_gesture = true;
send_arm_command(false, ArmingOrigin::GESTURE);
send_arm_command(ArmingAction::DISARM, ArmingOrigin::GESTURE);
} else if (!_selector.setpoint().disarm_gesture) {
_previous_disarm_gesture = false;
@@ -167,16 +167,23 @@ void ManualControl::Run()
}
if (switches.arm_switch != _previous_switches.arm_switch) {
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_arm_command(true, ArmingOrigin::SWITCH);
if (_param_com_arm_swisbtn.get()) {
// Arming button
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON);
}
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_arm_command(false, ArmingOrigin::SWITCH);
} else {
// Arming switch
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_arm_command(ArmingAction::ARM, ArmingOrigin::SWITCH);
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_arm_command(ArmingAction::DISARM, ArmingOrigin::SWITCH);
}
}
}
// TODO: handle case with arming switch as button
if (switches.return_switch != _previous_switches.return_switch) {
if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_rtl_command();
@@ -413,12 +420,11 @@ void ManualControl::send_mode_command(int32_t commander_main_state)
command_pub.publish(command);
}
void ManualControl::send_arm_command(bool should_arm, ArmingOrigin origin)
void ManualControl::send_arm_command(ArmingAction action, ArmingOrigin origin)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
command.param1 = should_arm ? 1.0f : 0.0f;
command.param1 = static_cast<float>(action);
command.param3 = static_cast<float>(origin); // We use param3 to signal the origin.
command.target_system = 1;
src/modules/manual_control/ManualControl.hpp
+8 -1
View File
@@ -105,6 +105,12 @@ public:
private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
enum class ArmingAction {
TOGGLE = -1,
DISARM = 0,
ARM = 1,
};
enum class ArmingOrigin {
GESTURE = 1,
SWITCH = 2,
@@ -115,7 +121,7 @@ private:
void evaluate_mode_slot(uint8_t mode_slot);
void send_mode_command(int32_t commander_main_state);
void send_arm_command(bool should_arm, ArmingOrigin origin);
void send_arm_command(ArmingAction action, ArmingOrigin origin);
void send_rtl_command();
void send_loiter_command();
void send_offboard_command();
@@ -162,6 +168,7 @@ private:
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_com_rc_stick_ov,
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,