forked from Archive/PX4-Autopilot
Use separate arm_request instead of vehicle_command for RC arming
This commit is contained in:
parent
c4473bdab7
commit
af607e3040
|
@ -47,6 +47,7 @@ set(msg_files
|
|||
airspeed.msg
|
||||
airspeed_validated.msg
|
||||
airspeed_wind.msg
|
||||
arm_request.msg
|
||||
autotune_attitude_control_status.msg
|
||||
battery_status.msg
|
||||
camera_capture.msg
|
||||
|
|
|
@ -0,0 +1,11 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint8 action # what action is requested
|
||||
uint8 ACTION_DISARM = 0
|
||||
uint8 ACTION_ARM = 1
|
||||
uint8 ACTION_TOGGLE = 2
|
||||
|
||||
uint8 source # how the request was triggered
|
||||
uint8 SOURCE_RC_STICK_GESTURE = 0
|
||||
uint8 SOURCE_RC_SWITCH = 1
|
||||
uint8 SOURCE_RC_BUTTON = 2
|
|
@ -69,7 +69,7 @@ uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto M
|
|||
uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal
|
||||
|
||||
uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)|
|
||||
uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm|-1 to toggle
|
||||
uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm
|
||||
uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes
|
||||
uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
|
||||
uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message
|
||||
|
@ -151,15 +151,9 @@ uint8 FAILURE_TYPE_DELAYED = 6
|
|||
uint8 FAILURE_TYPE_INTERMITTENT = 7
|
||||
|
||||
# used as param1 in ARM_DISARM command
|
||||
int8 ARMING_ACTION_TOGGLE = -1
|
||||
int8 ARMING_ACTION_DISARM = 0
|
||||
int8 ARMING_ACTION_ARM = 1
|
||||
|
||||
# used as param3 in ARM_DISARM command
|
||||
int8 ARMING_ORIGIN_GESTURE = 1
|
||||
int8 ARMING_ORIGIN_SWITCH = 2
|
||||
int8 ARMING_ORIGIN_BUTTON = 3
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 8
|
||||
|
||||
float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
|
||||
|
|
|
@ -562,7 +562,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||
run_preflight_checks = false;
|
||||
}
|
||||
|
||||
if (run_preflight_checks) {
|
||||
if (run_preflight_checks && !_armed.armed) {
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||
!_status.rc_signal_lost && _is_throttle_above_center) {
|
||||
|
@ -584,11 +584,21 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
} else if (calling_reason == arm_disarm_reason_t::rc_stick
|
||||
|| calling_reason == arm_disarm_reason_t::rc_switch
|
||||
|| calling_reason == arm_disarm_reason_t::rc_button) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
|
||||
events::send(events::ID("commander_arm_denied_not_manual"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: switch to manual mode first");
|
||||
tune_negative(true);
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
|
||||
&& !_status_flags.condition_home_position_valid) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home\t");
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
|
||||
events::send(events::ID("commander_arm_denied_geofence_rtl"),
|
||||
{events::Log::Critical, events::LogInternal::Info},
|
||||
"Arming denied: Geofence RTL requires valid home");
|
||||
|
@ -852,8 +862,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
const int8_t arming_action = static_cast<int8_t>(lroundf(cmd.param1));
|
||||
|
||||
if (arming_action != vehicle_command_s::ARMING_ACTION_ARM
|
||||
&& arming_action != vehicle_command_s::ARMING_ACTION_DISARM
|
||||
&& arming_action != vehicle_command_s::ARMING_ACTION_TOGGLE) {
|
||||
&& arming_action != vehicle_command_s::ARMING_ACTION_DISARM) {
|
||||
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);
|
||||
|
@ -861,26 +870,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
} else {
|
||||
// Arm is forced (checks skipped) when param2 is set to a magic number.
|
||||
const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
|
||||
|
||||
const int8_t arming_origin = static_cast<int8_t>(lroundf(cmd.param3));
|
||||
|
||||
const bool cmd_from_manual_stick = (arming_origin == vehicle_command_s::ARMING_ORIGIN_GESTURE);
|
||||
const bool cmd_from_manual_switch = (arming_origin == vehicle_command_s::ARMING_ORIGIN_SWITCH);
|
||||
const bool cmd_from_manual_button = (arming_origin == vehicle_command_s::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 (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Not arming! Switch to a manual mode first");
|
||||
|
||||
} else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Not disarming! 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 (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
|
||||
|
@ -916,53 +907,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
}
|
||||
|
||||
transition_result_t arming_res = TRANSITION_DENIED;
|
||||
arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
|
||||
arm_disarm_reason_t::command_internal;
|
||||
|
||||
if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
|
||||
if (cmd.from_external) {
|
||||
arming_res = arm(arm_disarm_reason_t::command_external);
|
||||
|
||||
} else {
|
||||
if (cmd_from_manual_stick) {
|
||||
arming_res = arm(arm_disarm_reason_t::rc_stick);
|
||||
|
||||
} else if (cmd_from_manual_switch) {
|
||||
arming_res = arm(arm_disarm_reason_t::rc_switch);
|
||||
|
||||
} else {
|
||||
arming_res = arm(arm_disarm_reason_t::command_internal, !forced);
|
||||
}
|
||||
}
|
||||
arming_res = arm(arm_disarm_reason, cmd.from_external || !forced);
|
||||
|
||||
} else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) {
|
||||
if (cmd.from_external) {
|
||||
arming_res = disarm(arm_disarm_reason_t::command_external);
|
||||
|
||||
} else {
|
||||
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 (arming_action == vehicle_command_s::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) {
|
||||
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");
|
||||
}
|
||||
arming_res = disarm(arm_disarm_reason);
|
||||
|
||||
}
|
||||
|
||||
|
@ -2497,6 +2449,38 @@ Commander::run()
|
|||
}
|
||||
}
|
||||
|
||||
while (_arm_request_sub.updated()) {
|
||||
arm_request_s arm_request;
|
||||
|
||||
if (_arm_request_sub.copy(&arm_request)) {
|
||||
arm_disarm_reason_t arm_disarm_reason{};
|
||||
|
||||
switch (arm_request.source) {
|
||||
case arm_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
|
||||
|
||||
case arm_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
|
||||
|
||||
case arm_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break;
|
||||
}
|
||||
|
||||
switch (arm_request.action) {
|
||||
case arm_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break;
|
||||
|
||||
case arm_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
|
||||
|
||||
case arm_request_s::ACTION_TOGGLE:
|
||||
if (_armed.armed) {
|
||||
disarm(arm_disarm_reason);
|
||||
|
||||
} else {
|
||||
arm(arm_disarm_reason);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Check for failure detector status */
|
||||
if (_failure_detector.update(_status, _vehicle_control_mode)) {
|
||||
_status.failure_detector_status = _failure_detector.getStatus().value;
|
||||
|
|
|
@ -61,6 +61,7 @@
|
|||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/arm_request.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/cpuload.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
@ -389,6 +390,7 @@ private:
|
|||
|
||||
// Subscriptions
|
||||
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
|
||||
uORB::Subscription _arm_request_sub {ORB_ID(arm_request)};
|
||||
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
||||
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
|
||||
|
|
|
@ -119,7 +119,7 @@ void ManualControl::Run()
|
|||
_stick_arm_hysteresis.set_state_and_update(stick_lower_right && right_stick_centered, _selector.setpoint().timestamp);
|
||||
|
||||
if (!previous_stick_arm_hysteresis && _stick_arm_hysteresis.get_state()) {
|
||||
send_arm_command(vehicle_command_s::ARMING_ACTION_ARM, vehicle_command_s::ARMING_ORIGIN_GESTURE);
|
||||
sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_STICK_GESTURE);
|
||||
}
|
||||
|
||||
// Disarm gesture
|
||||
|
@ -130,7 +130,7 @@ void ManualControl::Run()
|
|||
_stick_disarm_hysteresis.set_state_and_update(stick_lower_left && right_stick_centered, _selector.setpoint().timestamp);
|
||||
|
||||
if (!previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
|
||||
send_arm_command(vehicle_command_s::ARMING_ACTION_DISARM, vehicle_command_s::ARMING_ORIGIN_GESTURE);
|
||||
sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_STICK_GESTURE);
|
||||
}
|
||||
|
||||
// User override by stick
|
||||
|
@ -165,20 +165,17 @@ void ManualControl::Run()
|
|||
_button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now);
|
||||
|
||||
if (!previous_button_hysteresis && _button_hysteresis.get_state()) {
|
||||
send_arm_command(vehicle_command_s::ARMING_ACTION_TOGGLE,
|
||||
vehicle_command_s::ARMING_ORIGIN_BUTTON);
|
||||
sendArmRequest(arm_request_s::ACTION_TOGGLE, arm_request_s::SOURCE_RC_BUTTON);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Arming switch
|
||||
if (switches.arm_switch != _previous_switches.arm_switch) {
|
||||
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
send_arm_command(vehicle_command_s::ARMING_ACTION_ARM,
|
||||
vehicle_command_s::ARMING_ORIGIN_SWITCH);
|
||||
sendArmRequest(arm_request_s::ACTION_ARM, arm_request_s::SOURCE_RC_SWITCH);
|
||||
|
||||
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
||||
send_arm_command(vehicle_command_s::ARMING_ACTION_DISARM,
|
||||
vehicle_command_s::ARMING_ORIGIN_SWITCH);
|
||||
sendArmRequest(arm_request_s::ACTION_DISARM, arm_request_s::SOURCE_RC_SWITCH);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -419,18 +416,13 @@ void ManualControl::send_mode_command(int32_t commander_main_state)
|
|||
command_pub.publish(command);
|
||||
}
|
||||
|
||||
void ManualControl::send_arm_command(int8_t action, int8_t origin)
|
||||
void ManualControl::sendArmRequest(int8_t action, int8_t source)
|
||||
{
|
||||
vehicle_command_s command{};
|
||||
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
|
||||
command.param1 = static_cast<float>(action);
|
||||
command.param3 = static_cast<float>(origin); // We use param3 to signal the origin.
|
||||
command.target_system = _param_mav_sys_id.get();
|
||||
command.target_component = _param_mav_comp_id.get();
|
||||
|
||||
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
|
||||
command.timestamp = hrt_absolute_time();
|
||||
command_pub.publish(command);
|
||||
arm_request_s arm_request{};
|
||||
arm_request.action = action;
|
||||
arm_request.source = source;
|
||||
arm_request.timestamp = hrt_absolute_time();
|
||||
_arm_request_pub.publish(arm_request);
|
||||
}
|
||||
|
||||
void ManualControl::send_rtl_command()
|
||||
|
|
|
@ -40,6 +40,7 @@
|
|||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/topics/arm_request.h>
|
||||
#include <uORB/topics/manual_control_input.h>
|
||||
#include <uORB/topics/manual_control_switches.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
@ -120,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(int8_t action, int8_t origin);
|
||||
void sendArmRequest(int8_t action, int8_t source);
|
||||
void send_rtl_command();
|
||||
void send_loiter_command();
|
||||
void send_offboard_command();
|
||||
|
@ -128,6 +129,7 @@ private:
|
|||
void publish_landing_gear(int8_t action);
|
||||
void send_vtol_transition_command(uint8_t action);
|
||||
|
||||
uORB::Publication<arm_request_s> _arm_request_pub{ORB_ID(arm_request)};
|
||||
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
|
Loading…
Reference in New Issue