From aa984edd90b1a1caa9d7dfe4b74108e2d22fa071 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 28 Nov 2016 11:29:58 +0100 Subject: [PATCH] added an rc switch for arming and disarming with the option to use it as a button --- msg/manual_control_setpoint.msg | 1 + msg/rc_channels.msg | 3 +- src/modules/commander/commander.cpp | 33 ++++++++++++++--- src/modules/commander/commander_params.c | 14 ++++++++ src/modules/sensors/parameters.cpp | 9 +++++ src/modules/sensors/parameters.h | 5 +++ src/modules/sensors/rc_update.cpp | 2 ++ src/modules/sensors/sensor_params.c | 46 ++++++++++++++++++++++++ 8 files changed, 108 insertions(+), 5 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 2b5285b803..febaa29a94 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -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 diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index e62070dd91..7e5004abb7 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 679534690b..e29121425d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index db3efacd9a..a02b047456 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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 * diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index c1669383ad..47ab0e8b2f 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -90,6 +90,7 @@ int initialize_parameter_handles(ParameterHandles ¶meter_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 ¶meter_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 ¶meter_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 ¶meter_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); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index d8b817c1d5..07f55f14de 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -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; diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 3da1afe748..0a4a1921e0 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -414,6 +414,8 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_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, diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index f9ed687dc4..eee1967b75 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -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