fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 11:23:33 +08:00
Code Issues 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-11-09 16:05:25 +01: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/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,