From 8cb472af31e00c2fe89857bb8c71a445fad516b3 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 18 Dec 2015 21:21:51 -0700 Subject: [PATCH] add RC kill switch --- msg/manual_control_setpoint.msg | 5 +++-- msg/rc_channels.msg | 7 ++++--- src/modules/commander/commander.cpp | 12 ++++++++++++ src/modules/sensors/sensor_params.c | 28 ++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 17 +++++++++++++++++ 5 files changed, 64 insertions(+), 5 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 2ff6fca1f5..424453d699 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,8 +38,9 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL -uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL 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 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 6b30794e00..3171fbc106 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=20 +int32 RC_CHANNELS_FUNCTION_MAX=21 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -19,11 +19,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 +uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[20] channels # Scaled to -1..1 (throttle: 0..1) +float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[20] function # Functions mapping +int8[21] function # Functions mapping uint8 rssi # Receive signal strength index 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 6b5c29ab93..e2d202e9d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2284,6 +2284,18 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "main state transition denied"); } + /* check throttle kill switch */ + int prevLockdown = armed.lockdown; + if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* set lockdown flag */ + armed.lockdown = TRUE; + } else { + armed.lockdown = FALSE; + } + if (prevLockdown != armed.lockdown) { + warnx("armed.lockdown: %d\n", armed.lockdown); + } + } else { if (!status.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index aa91fcf92a..fa1dfeb1b7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2009,6 +2009,15 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Kill switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0); + /** * Flaps channel mapping. * @@ -2243,6 +2252,25 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); */ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); + +/** + * Threshold for selecting offboard mode + * + * 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