fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 05:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

added an rc switch for arming and disarming with the option to use it as a button

Browse Source
This commit is contained in:
Matthias Grob 2016-11-28 11:29:58 +01:00 committed by Lorenz Meier
parent 537f72073d
commit aa984edd90
8 changed files with 108 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/manual_control_setpoint.msg
View File

@ -51,6 +51,7 @@ uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 gear_switch # landing gear switch: _DOWN_, UP
int8 mode_slot # the slot a specific model selector is in

3
msg/rc_channels.msg
View File

@ -22,11 +22,12 @@ uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
uint8 RC_CHANNELS_FUNCTION_GEAR=22
uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[23] function # Functions mapping
int8[24] function # Functions mapping
uint8 rssi # Receive signal strength indicator (0-100)
bool signal_lost # Control signal lost, should be checked together with topic timeout
uint32 frame_drop_count # Number of dropped frames

33
src/modules/commander/commander.cpp
View File

@ -209,6 +209,7 @@ static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
static unsigned _last_mission_instance = 0;
struct manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint
static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch
static uint8_t _last_sp_man_arm_switch = 0;
static struct vtol_vehicle_status_s vtol_status = {};
static struct cpuload_s cpuload = {};
@ -1297,6 +1298,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
param_t _param_offboard_loss_timeout = param_find("COM_OF_LOSS_T");
param_t _param_arm_without_gps = param_find("COM_ARM_WO_GPS");
param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
param_t _param_fmode_1 = param_find("COM_FLTMODE1");
param_t _param_fmode_2 = param_find("COM_FLTMODE2");
@ -1642,6 +1644,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_autostart_id, &autostart_id);
param_get(_param_rc_in_off, &rc_in_off);
param_get(_param_arm_without_gps, &arm_without_gps);
int32_t arm_switch_is_button = 0;
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
can_arm_without_gps = (arm_without_gps == 1);
status.rc_input_mode = rc_in_off;
if (is_hil_setup(autostart_id)) {
@ -1782,6 +1786,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_offboard_loss_act, &offboard_loss_act);
param_get(_param_offboard_loss_rc_act, &offboard_loss_rc_act);
param_get(_param_arm_without_gps, &arm_without_gps);
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
can_arm_without_gps = (arm_without_gps == 1);
/* Autostart id */
@ -2555,7 +2560,7 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = false;
/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
/* check if left stick is in lower left position or arm button is pushed and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings in manual mode or fixed wing if landed */
if ((status.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
(status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
@ -2564,7 +2569,7 @@ int commander_thread_main(int argc, char *argv[])
internal_state.main_state == commander_state_s::MAIN_STATE_STAB ||
internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
land_detector.landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
((sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || ((bool)arm_switch_is_button && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) ) {
if (stick_off_counter > rc_arm_hyst) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@ -2596,8 +2601,10 @@ int commander_thread_main(int argc, char *argv[])
stick_off_counter = 0;
}
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
/* check if left stick is in lower right position or arm button is pushed and we're in MANUAL mode -> arm */
if (status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
(status.arming_state != vehicle_status_s::ARMING_STATE_ARMED && status.arming_state != vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
((sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) || (arm_switch_is_button == 1 && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) ) {
if (stick_on_counter > rc_arm_hyst) {
/* we check outside of the transition function here because the requirement
@ -2697,6 +2704,24 @@ int commander_thread_main(int argc, char *argv[])
}
/* no else case: do not change lockdown flag in unconfigured case */
/* check arm switch */
/* we change the switch from disarm to arm */
if (arm_switch_is_button == 0 && _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "arm switch")) {
mavlink_log_info(&mavlink_log_pub, "arming failed");
} else {
arming_state_changed = true;
}
/* we change the switch from arm to disarm */
} else if (arm_switch_is_button == 0 && _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (TRANSITION_CHANGED != arm_disarm(false, &mavlink_log_pub, "arm switch")) {
mavlink_log_info(&mavlink_log_pub, "rejected disarm");
} else {
arming_state_changed = true;
}
}
/* no else case: do not change arming here if arm switch unconfigured */
_last_sp_man_arm_switch = sp_man.arm_switch;
} else {
if (!status_flags.rc_input_blocked && !status.rc_signal_lost) {
mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);

14
src/modules/commander/commander_params.c
View File

@ -279,6 +279,20 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
*/
PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
/**
* Use arm switch is only a button
*
* The default uses the arm switch as real switch.
* If parameter set button gets handled like stick arming.
*
* @group Commander
* @min 0
* @max 1
* @value 0 Arm switch is a switch that stays on when armed
* @value 1 Arm switch is a button that only triggers arming
*/
PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
/**
* Battery failsafe mode
*

9
src/modules/sensors/parameters.cpp
View File

@ -90,6 +90,7 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
@ -120,6 +121,7 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH");
parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
@ -313,6 +315,10 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_arm_sw, &(parameters.rc_map_arm_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
@ -365,6 +371,9 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th));
parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0);
parameters.rc_killswitch_th = fabs(parameters.rc_killswitch_th);
param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th));
parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0);
parameters.rc_armswitch_th = fabs(parameters.rc_armswitch_th);
param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th));
parameters.rc_trans_inv = (parameters.rc_trans_th < 0);
parameters.rc_trans_th = fabs(parameters.rc_trans_th);

5
src/modules/sensors/parameters.h
View File

@ -83,6 +83,7 @@ struct Parameters {
int rc_map_acro_sw;
int rc_map_offboard_sw;
int rc_map_kill_sw;
int rc_map_arm_sw;
int rc_map_trans_sw;
int rc_map_gear_sw;
@ -108,6 +109,7 @@ struct Parameters {
float rc_acro_th;
float rc_offboard_th;
float rc_killswitch_th;
float rc_armswitch_th;
float rc_trans_th;
float rc_gear_th;
bool rc_assist_inv;
@ -119,6 +121,7 @@ struct Parameters {
bool rc_acro_inv;
bool rc_offboard_inv;
bool rc_killswitch_inv;
bool rc_armswitch_inv;
bool rc_trans_inv;
bool rc_gear_inv;
@ -159,6 +162,7 @@ struct ParameterHandles {
param_t rc_map_acro_sw;
param_t rc_map_offboard_sw;
param_t rc_map_kill_sw;
param_t rc_map_arm_sw;
param_t rc_map_trans_sw;
param_t rc_map_gear_sw;
@ -188,6 +192,7 @@ struct ParameterHandles {
param_t rc_acro_th;
param_t rc_offboard_th;
param_t rc_killswitch_th;
param_t rc_armswitch_th;
param_t rc_trans_th;
param_t rc_gear_th;

2
src/modules/sensors/rc_update.cpp
View File

@ -414,6 +414,8 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
_parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
manual.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_parameters.rc_armswitch_th, _parameters.rc_armswitch_inv);
manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
_parameters.rc_trans_th, _parameters.rc_trans_inv);
manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,

46
src/modules/sensors/sensor_params.c
View File

@ -2488,6 +2488,34 @@ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0);
/**
* Arm switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_ARM_SW, 0);
/**
* Flaps channel
*
@ -2985,6 +3013,24 @@ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
*/
PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
/**
* Threshold for the arm switch
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*
*/
PARAM_DEFINE_FLOAT(RC_ARMSWITCH_TH, 0.25f);
/**
* Threshold for the VTOL transition switch
*