From a5f3f65c2b9612339d3e07258f679800805e9fb9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 7 Feb 2017 18:11:36 +0100 Subject: [PATCH] Switch assignment: added manual and stabilized switch for a default stabilized switch scheme, let FW go into Acro --- msg/manual_control_setpoint.msg | 4 +- msg/rc_channels.msg | 7 +- src/modules/commander/commander.cpp | 79 ++++++++++++---- .../commander/state_machine_helper.cpp | 3 + src/modules/sensors/parameters.cpp | 18 ++++ src/modules/sensors/parameters.h | 13 ++- src/modules/sensors/rc_update.cpp | 6 ++ src/modules/sensors/sensor_params.c | 93 +++++++++++++++++++ 8 files changed, 199 insertions(+), 24 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index e491d41cf9..253aa327a8 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -54,10 +54,12 @@ uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUA 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 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 uint8 data_source # where this input is coming from +uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED +uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 7e5004abb7..6d31e9d5a8 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,3 @@ -int32 RC_CHANNELS_FUNCTION_MAX=21 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -23,11 +22,13 @@ uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 uint8 RC_CHANNELS_FUNCTION_TRANSITION=21 uint8 RC_CHANNELS_FUNCTION_GEAR=22 uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23 +uint8 RC_CHANNELS_FUNCTION_STAB=24 +uint8 RC_CHANNELS_FUNCTION_MAN=25 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[24] function # Functions mapping -uint8 rssi # Receive signal strength indicator (0-100) +int8[26] 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 af83c493f2..12e8e4b82c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3321,7 +3321,9 @@ set_main_state_rc(struct vehicle_status_s *status_local) (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) && (_last_sp_man.posctl_switch == sp_man.posctl_switch) && (_last_sp_man.loiter_switch == sp_man.loiter_switch) && - (_last_sp_man.mode_slot == sp_man.mode_slot)))) { + (_last_sp_man.mode_slot == sp_man.mode_slot) && + (_last_sp_man.stab_switch == sp_man.stab_switch) && + (_last_sp_man.man_switch == sp_man.man_switch)))) { // update these fields for the geofence system @@ -3519,31 +3521,72 @@ set_main_state_rc(struct vehicle_status_s *status_local) break; case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL - if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - - /* manual mode is stabilized already for multirotors, so switch to acro - * for any non-manual mode + if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE && + sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { + /* + * Legacy mode: + * Acro switch being used as stabilized switch in FW. */ - // XXX: put ACRO and STAB on separate switches - if (status.is_rotary_wing && !status.is_vtol) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); - } else if (!status.is_rotary_wing) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* manual mode is stabilized already for multirotors, so switch to acro + * for any non-manual mode + */ + if (status.is_rotary_wing && !status.is_vtol) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); + + } else if (!status.is_rotary_wing) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + + } else { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + } + + } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* Similar to acro transitions for multirotors. FW aircraft don't need a + * rattitude mode.*/ + if (status.is_rotary_wing) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + + } else { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + } + } else { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } - } - else if(sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ - /* Similar to acro transitions for multirotors. FW aircraft don't need a - * rattitude mode.*/ - if (status.is_rotary_wing) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + } else { + /* New mode: + * - Acro is Acro + * - Manual is not default anymore when the manaul switch is assigned + */ + if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + + } else if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); + + } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* Similar to acro transitions for multirotors. FW aircraft don't have a + * rattitude mode.*/ + if (status.is_rotary_wing) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + + } else { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + } + + } else if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + + } else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { + // default to MANUAL when no manual switch is set + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + } else { + // default to STAB when the manual switch is assigned (but off) res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); } - }else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); } // TRANSITION_DENIED is not possible here diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0e57f1a507..8344d05bbb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -380,6 +380,9 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; case commander_state_s::MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case commander_state_s::MAIN_STATE_RATTITUDE: /* ACRO and RATTITUDE only implemented in MC */ diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 0f1fa0a28b..b49dfa06d7 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -93,6 +93,8 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) 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"); + parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW"); + parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW"); parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -124,6 +126,8 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles) 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"); + parameter_handles.rc_stab_th = param_find("RC_STAB_TH"); + parameter_handles.rc_man_th = param_find("RC_MAN_TH"); /* RC low pass filter configuration */ parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); @@ -339,6 +343,14 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par PX4_WARN("%s", paramerr); } + if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) { + warnx("%s", paramerr); + } + param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1)); param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2)); param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3)); @@ -388,6 +400,12 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th)); parameters.rc_gear_inv = (parameters.rc_gear_th < 0); parameters.rc_gear_th = fabs(parameters.rc_gear_th); + param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th)); + parameters.rc_stab_inv = (parameters.rc_stab_th < 0); + parameters.rc_stab_th = fabs(parameters.rc_stab_th); + param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th)); + parameters.rc_man_inv = (parameters.rc_man_th < 0); + parameters.rc_man_th = fabs(parameters.rc_man_th); param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate)); parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index b9bfd5d590..072dd3d689 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -87,7 +87,8 @@ struct Parameters { int rc_map_arm_sw; int rc_map_trans_sw; int rc_map_gear_sw; - + int rc_map_stab_sw; + int rc_map_man_sw; int rc_map_flaps; int rc_map_aux1; @@ -113,6 +114,9 @@ struct Parameters { float rc_armswitch_th; float rc_trans_th; float rc_gear_th; + float rc_stab_th; + float rc_man_th; + bool rc_assist_inv; bool rc_auto_inv; bool rc_rattitude_inv; @@ -125,6 +129,8 @@ struct Parameters { bool rc_armswitch_inv; bool rc_trans_inv; bool rc_gear_inv; + bool rc_stab_inv; + bool rc_man_inv; float rc_flt_smp_rate; float rc_flt_cutoff; @@ -169,8 +175,9 @@ struct ParameterHandles { param_t rc_map_arm_sw; param_t rc_map_trans_sw; param_t rc_map_gear_sw; - param_t rc_map_flaps; + param_t rc_map_stab_sw; + param_t rc_map_man_sw; param_t rc_map_aux1; param_t rc_map_aux2; @@ -199,6 +206,8 @@ struct ParameterHandles { param_t rc_armswitch_th; param_t rc_trans_th; param_t rc_gear_th; + param_t rc_stab_th; + param_t rc_man_th; param_t rc_flt_smp_rate; param_t rc_flt_cutoff; diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 7c649b0311..70ce2145b9 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -103,6 +103,8 @@ void RCUpdate::update_rc_functions() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _parameters.rc_map_arm_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _parameters.rc_map_stab_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _parameters.rc_map_man_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; @@ -434,6 +436,10 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) _parameters.rc_trans_th, _parameters.rc_trans_inv); manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR, _parameters.rc_gear_th, _parameters.rc_gear_inv); + manual.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB, + _parameters.rc_stab_th, _parameters.rc_stab_inv); + manual.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN, + _parameters.rc_man_th, _parameters.rc_man_inv); /* publish manual_control_setpoint topic */ orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_control_pub, &manual, &instance, diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2bea85bf1b..ab49693083 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2600,6 +2600,63 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0); * @value 18 Channel 18 */ PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0); + +/** + * Stabilize switch channel mapping. + * + * @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_STAB_SW, 0); + +/** + * Manual switch channel mapping. + * + * @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_MAN_SW, 0); + /** * AUX1 Passthrough RC Channel * @@ -3068,6 +3125,42 @@ PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.25f); +/** + * Threshold for the stabilize 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 channelth + * negative : true when channel