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

commander: move switch handling to manual_control

Browse Source
This commit is contained in:
Julian Oes
2021-04-16 16:45:50 +02:00
committed by Matthias Grob
parent 08e58a44e9
commit 97aa06cc19
4 changed files with 274 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
-134
View File
@@ -1874,14 +1874,6 @@ Commander::run()
_arm_requirements.mission = _param_arm_mission_required.get();
_arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE;
/* flight mode slots */
_flight_mode_slots[0] = _param_fltmode_1.get();
_flight_mode_slots[1] = _param_fltmode_2.get();
_flight_mode_slots[2] = _param_fltmode_3.get();
_flight_mode_slots[3] = _param_fltmode_4.get();
_flight_mode_slots[4] = _param_fltmode_5.get();
_flight_mode_slots[5] = _param_fltmode_6.get();
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
/* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */
@@ -2466,13 +2458,6 @@ Commander::run()
_landing_gear_pub.publish(landing_gear);
}
}
// evaluate the main state machine according to mode switches
if (set_main_state() == TRANSITION_CHANGED) {
// play tune on mode change only if armed, blink LED always
tune_positive(_armed.armed);
_status_changed = true;
}
}
/* check throttle kill switch */
@@ -3103,125 +3088,6 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
_leds_counter++;
}
transition_result_t Commander::set_main_state()
{
if ((_manual_control_switches.timestamp == 0)
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
// no manual control or no update -> nothing changed
return TRANSITION_NOT_CHANGED;
}
// Note: even if _status_flags.offboard_control_set_by_command is set
// we want to allow rc mode change to take precedence. This is a safety
// feature, just in case offboard control goes crazy.
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
bool should_evaluate_rc_mode_switch =
(_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch)
|| (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch)
|| (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch)
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot);
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// if already armed don't evaluate first time RC
if (_last_manual_control_switches.timestamp == 0) {
should_evaluate_rc_mode_switch = false;
_last_manual_control_switches = _manual_control_switches;
}
} else {
// not armed
if (!should_evaluate_rc_mode_switch) {
// to respect initial switch position (eg POSCTL) force RC switch re-evaluation if estimates become valid
const bool altitude_got_valid = (!_last_condition_local_altitude_valid && _status_flags.condition_local_altitude_valid);
const bool lpos_got_valid = (!_last_condition_local_position_valid && _status_flags.condition_local_position_valid);
const bool gpos_got_valid = (!_last_condition_global_position_valid && _status_flags.condition_global_position_valid);
if (altitude_got_valid || lpos_got_valid || gpos_got_valid) {
should_evaluate_rc_mode_switch = true;
}
}
}
if (!should_evaluate_rc_mode_switch) {
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
_last_manual_control_switches = _manual_control_switches;
// reset the position and velocity validity calculation to give the best change of being able to select
// the desired mode
reset_posvel_validity();
/* set main state according to RC switches */
transition_result_t res = TRANSITION_NOT_CHANGED;
/* offboard switch overrides main switch */
if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state);
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD);
/* mode rejected, continue to evaluate the main system mode */
} else {
/* changed successfully or already in this state */
return res;
}
}
/* RTL switch overrides main switch */
if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
/* changed successfully or already in this state */
return res;
}
/* if we get here mode was rejected, continue to evaluate the main system mode */
}
/* Loiter switch overrides main switch */
if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
if (res == TRANSITION_DENIED) {
print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER);
/* mode rejected, continue to evaluate the main system mode */
} else {
/* changed successfully or already in this state */
return res;
}
}
/* we know something has changed - check if we are in mode slot operation */
if (_manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) {
if (_manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) {
PX4_WARN("m slot overflow");
return TRANSITION_DENIED;
}
int new_mode = _flight_mode_slots[_manual_control_switches.mode_slot - 1];
if (new_mode < 0) {
/* slot is unused */
res = TRANSITION_NOT_CHANGED;
} else {
res = try_mode_change(new_mode);
}
return res;
}
return res;
}
void
Commander::reset_posvel_validity()
{
src/modules/commander/Commander.hpp
-11
View File
@@ -175,9 +175,6 @@ private:
void UpdateEstimateValidity();
// Set the system main state based on the current RC inputs
transition_result_t set_main_state();
bool shutdown_if_allowed();
bool stabilization_required();
@@ -251,13 +248,6 @@ private:
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
// Circuit breakers
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
@@ -361,7 +351,6 @@ private:
manual_control_switches_s _last_manual_control_switches{};
ManualControl _manual_control{this};
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
src/modules/manual_control/ManualControl.cpp
+253 -13
View File
@@ -36,6 +36,9 @@
#include <drivers/drv_hrt.h>
#include <commander/px4_custom_mode.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/commander_state.h>
namespace manual_control
{
@@ -123,7 +126,7 @@ void ManualControl::Run()
if (_selector.setpoint().arm_gesture && !_previous_arm_gesture) {
_previous_arm_gesture = true;
send_arm_command();
send_arm_command(true, ArmingOrigin::GESTURE);
} else if (!_selector.setpoint().arm_gesture) {
_previous_arm_gesture = false;
@@ -131,7 +134,7 @@ void ManualControl::Run()
if (_selector.setpoint().disarm_gesture && !_previous_disarm_gesture) {
_previous_disarm_gesture = true;
send_disarm_command();
send_arm_command(false, ArmingOrigin::GESTURE);
} else if (!_selector.setpoint().disarm_gesture) {
_previous_disarm_gesture = false;
@@ -160,22 +163,111 @@ void ManualControl::Run()
// Only use switches if current source is RC as well.
if (_selector.setpoint().data_source == manual_control_input_s::SOURCE_RC) {
if (_previous_switches_initialized) {
if (switches.mode_slot != _previous_switches.mode_slot) {
switch (switches.mode_slot) {
case manual_control_switches_s::MODE_SLOT_NONE:
_last_mode_slot_flt = -1;
break;
case manual_control_switches_s::MODE_SLOT_1:
_last_mode_slot_flt = _param_fltmode_1.get();
break;
case manual_control_switches_s::MODE_SLOT_2:
_last_mode_slot_flt = _param_fltmode_2.get();
break;
case manual_control_switches_s::MODE_SLOT_3:
_last_mode_slot_flt = _param_fltmode_3.get();
break;
case manual_control_switches_s::MODE_SLOT_4:
_last_mode_slot_flt = _param_fltmode_4.get();
break;
case manual_control_switches_s::MODE_SLOT_5:
_last_mode_slot_flt = _param_fltmode_5.get();
break;
case manual_control_switches_s::MODE_SLOT_6:
_last_mode_slot_flt = _param_fltmode_6.get();
break;
default:
_last_mode_slot_flt = -1;
PX4_WARN("mode slot overflow");
break;
}
send_mode_command(_last_mode_slot_flt);
}
if (switches.arm_switch != _previous_switches.arm_switch) {
if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_arm_command();
send_arm_command(true, ArmingOrigin::SWITCH);
} else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_disarm_command();
send_arm_command(false, 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();
} else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
}
}
// TODO: handle the rest of the buttons
if (switches.loiter_switch != _previous_switches.loiter_switch) {
if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_loiter_command();
} else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
}
}
if (switches.offboard_switch != _previous_switches.offboard_switch) {
if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_offboard_command();
} else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_mode_command(_last_mode_slot_flt);
}
}
if (switches.kill_switch != _previous_switches.kill_switch) {
if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_termination_command(true);
} else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_termination_command(false);
}
}
if (switches.gear_switch != _previous_switches.gear_switch) {
if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
publish_landing_gear(landing_gear_s::GEAR_UP);
} else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
publish_landing_gear(landing_gear_s::GEAR_DOWN);
}
}
if (switches.transition_switch != _previous_switches.transition_switch) {
if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) {
send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
} else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) {
send_vtol_transition_command(vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
}
}
}
_previous_switches = switches;
@@ -184,6 +276,7 @@ void ManualControl::Run()
} else {
_previous_switches = {};
_previous_switches_initialized = false;
_last_mode_slot_flt = -1;
}
}
@@ -219,12 +312,92 @@ void ManualControl::Run()
perf_end(_loop_perf);
}
void ManualControl::send_arm_command()
void ManualControl::send_mode_command(int32_t commander_main_state)
{
if (commander_main_state == -1) {
// Not assigned.
return;
}
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
command.param1 = 1.0;
command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
switch (commander_main_state) {
case commander_state_s::MAIN_STATE_MANUAL:
command.param2 = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case commander_state_s::MAIN_STATE_ALTCTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case commander_state_s::MAIN_STATE_POSCTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL;
command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL;
break;
case commander_state_s::MAIN_STATE_AUTO_MISSION:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case commander_state_s::MAIN_STATE_AUTO_LOITER:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case commander_state_s::MAIN_STATE_AUTO_RTL:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case commander_state_s::MAIN_STATE_ACRO:
command.param2 = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case commander_state_s::MAIN_STATE_OFFBOARD:
command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case commander_state_s::MAIN_STATE_STAB:
command.param2 = PX4_CUSTOM_MAIN_MODE_STABILIZED;
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case commander_state_s::MAIN_STATE_AUTO_LAND:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND;
break;
case commander_state_s::MAIN_STATE_ORBIT:
// TODO: check if this works without the DO_ORBIT command
command.param2 = PX4_CUSTOM_MAIN_MODE_POSCTL;
command.param3 = PX4_CUSTOM_SUB_MODE_POSCTL_ORBIT;
break;
case commander_state_s::MAIN_STATE_MAX:
// FALLTHROUGH
default:
PX4_WARN("Unknown main_state");
return;
}
command.target_system = 1;
command.target_component = 1;
@@ -233,12 +406,14 @@ void ManualControl::send_arm_command()
command_pub.publish(command);
}
void ManualControl::send_disarm_command()
void ManualControl::send_arm_command(bool should_arm, ArmingOrigin origin)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
command.param1 = 0.0;
command.param3 = 1.0; // We use param3 to signal that the origin is manual control stick.
command.param1 = should_arm ? 1.0f : 0.0f;
command.param3 = static_cast<float>(origin); // We use param3 to signal the origin.
command.target_system = 1;
command.target_component = 1;
@@ -251,7 +426,7 @@ void ManualControl::send_rtl_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
command.target_system = 1;
@@ -262,6 +437,71 @@ void ManualControl::send_rtl_command()
command_pub.publish(command);
}
void ManualControl::send_loiter_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::send_offboard_command()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = 1.0f;
command.param2 = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
command.target_system = 1;
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::send_termination_command(bool should_terminate)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
command.param1 = should_terminate ? 1.0f : 0.0f;
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
void ManualControl::publish_landing_gear(int8_t action)
{
landing_gear_s landing_gear{};
landing_gear.landing_gear = action;
landing_gear.timestamp = hrt_absolute_time();
uORB::Publication<landing_gear_s> landing_gear_pub{ORB_ID(landing_gear)};
landing_gear_pub.publish(landing_gear);
}
void ManualControl::send_vtol_transition_command(uint8_t action)
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
command.param1 = action;
command.target_system = 1;
command.target_component = 1;
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
command.timestamp = hrt_absolute_time();
command_pub.publish(command);
}
int ManualControl::task_spawn(int argc, char *argv[])
{
ManualControl *instance = new ManualControl();
src/modules/manual_control/ManualControl.hpp
+21 -3
View File
@@ -76,11 +76,22 @@ public:
private:
static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
enum class ArmingOrigin {
GESTURE = 1,
SWITCH = 2,
BUTTON = 3,
};
void Run() override;
void send_arm_command();
void send_disarm_command();
void send_mode_command(int32_t commander_main_state);
void send_arm_command(bool should_arm, ArmingOrigin origin);
void send_rtl_command();
void send_loiter_command();
void send_offboard_command();
void send_termination_command(bool should_terminate);
void publish_landing_gear(int8_t action);
void send_vtol_transition_command(uint8_t action);
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
@@ -109,6 +120,7 @@ private:
manual_control_switches_s _previous_switches{};
bool _previous_switches_initialized{false};
int32_t _last_mode_slot_flt{-1};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
@@ -117,7 +129,13 @@ private:
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(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
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
)
};
} // namespace manual_control