dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander/manual_control: use msg enum for params 

Instead of using a private enum class we should define the enum in the
vehicle_command message and then use it consistently.
This commit is contained in:
Julian Oes 2021-06-17 11:23:23 +02:00 committed by Matthias Grob
parent f1b1068824
commit b3a5072de5
4 changed files with 49 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
msg/vehicle_command.msg
View File

@ -150,6 +150,16 @@ uint8 FAILURE_TYPE_SLOW = 5
uint8 FAILURE_TYPE_DELAYED = 6
uint8 FAILURE_TYPE_INTERMITTENT = 7
# used as param1 in ARM_DISARM command
int8 ARM_DISARM_ARMING_ACTION_TOGGLE = -1
int8 ARM_DISARM_ARMING_ACTION_DISARM = 0
int8 ARM_DISARM_ARMING_ACTION_ARM = 1
# used as param3 in ARM_DISARM command
int8 ARM_DISARM_ARMING_ORIGIN_GESTURE = 1
int8 ARM_DISARM_ARMING_ORIGIN_SWITCH = 2
int8 ARM_DISARM_ARMING_ORIGIN_BUTTON = 3
uint8 ORB_QUEUE_LENGTH = 8
float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

44
src/modules/commander/Commander.cpp
View File

@ -274,7 +274,9 @@ int Commander::custom_command(int argc, char *argv[])
param2 = 21196.f;
}
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, param2);
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM),
param2);
return 0;
}
@ -287,7 +289,9 @@ int Commander::custom_command(int argc, char *argv[])
param2 = 21196.f;
}
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.f, param2);
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM),
param2);
return 0;
}
@ -295,7 +299,9 @@ int Commander::custom_command(int argc, char *argv[])
if (!strcmp(argv[0], "takeoff")) {
// switch to takeoff mode and arm
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f, 0.f);
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM),
0.f);
return 0;
}
@ -844,27 +850,31 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
const int param1_arm = static_cast<int>(roundf(cmd.param1));
const int8_t arming_action = static_cast<int8_t>(lroundf(cmd.param1));
if (param1_arm != 0 && param1_arm != 1 && param1_arm != -1) {
if (arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM
&& arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM
&& arming_action != vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE) {
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 {
// 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 forced = (static_cast<int>(lroundf(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 int8_t arming_origin = static_cast<int8_t>(lroundf(cmd.param3));
const bool cmd_from_manual_stick = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE);
const bool cmd_from_manual_switch = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH);
const bool cmd_from_manual_button = (arming_origin == vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_BUTTON);
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
if (cmd_from_manual_stick && !_vehicle_control_mode.flag_control_manual_enabled) {
if (param1_arm == 1) {
if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) {
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
} else if (param1_arm == 0) {
} else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM) {
mavlink_log_critical(&_mavlink_log_pub, "Not disarming! Switch to a manual mode first");
}
@ -874,7 +884,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(_status)) {
if (param1_arm == 1) {
if (arming_action == vehicle_command_s::ARM_DISARM_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"),
@ -901,14 +911,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 && (param1_arm == 1)) {
&& cmd_from_io && (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM)) {
_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
}
transition_result_t arming_res = TRANSITION_DENIED;
if (param1_arm == 1) {
if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) {
if (cmd.from_external) {
arming_res = arm(arm_disarm_reason_t::command_external);
@ -924,7 +934,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
}
} else if (param1_arm == 0) {
} else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM) {
if (cmd.from_external) {
arming_res = disarm(arm_disarm_reason_t::command_external);
@ -940,7 +950,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
}
} else if (param1_arm == -1) {
} else if (arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE) {
// -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) {
@ -964,7 +974,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 ((param1_arm == 1) && (arming_res == TRANSITION_CHANGED) &&
if ((arming_action == vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) {
set_home_position();

17
src/modules/manual_control/ManualControl.cpp
View File

@ -131,7 +131,8 @@ void ManualControl::Run()
if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) {
_previous_arm_gesture = true;
send_arm_command(ArmingAction::ARM, ArmingOrigin::GESTURE);
send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM,
vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE);
}
@ -141,7 +142,8 @@ void ManualControl::Run()
if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) {
_previous_disarm_gesture = true;
send_arm_command(ArmingAction::DISARM, ArmingOrigin::GESTURE);
send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM,
vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_GESTURE);
}
@ -180,16 +182,19 @@ void ManualControl::Run()
_button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now);
if (_button_hysteresis.get_state()) {
send_arm_command(ArmingAction::TOGGLE, ArmingOrigin::BUTTON);
send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_TOGGLE,
vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_BUTTON);
}
} else {
// Arming switch
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_arm_command(ArmingAction::ARM, ArmingOrigin::SWITCH);
send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_ARM,
vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH);
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_arm_command(ArmingAction::DISARM, ArmingOrigin::SWITCH);
send_arm_command(vehicle_command_s::ARM_DISARM_ARMING_ACTION_DISARM,
vehicle_command_s::ARM_DISARM_ARMING_ORIGIN_SWITCH);
}
}
}
@ -427,7 +432,7 @@ void ManualControl::send_mode_command(int32_t commander_main_state)
command_pub.publish(command);
}
void ManualControl::send_arm_command(ArmingAction action, ArmingOrigin origin)
void ManualControl::send_arm_command(int8_t action, int8_t origin)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;

14
src/modules/manual_control/ManualControl.hpp
View File

@ -116,23 +116,11 @@ 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,
BUTTON = 3,
};
void Run() override;
void evaluate_mode_slot(uint8_t mode_slot);
void send_mode_command(int32_t commander_main_state);
void send_arm_command(ArmingAction action, ArmingOrigin origin);
void send_arm_command(int8_t action, int8_t origin);
void send_rtl_command();
void send_loiter_command();
void send_offboard_command();