From d0f1a551e9b9a5abc913b059da39d2489ab50530 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 1 Aug 2019 18:27:58 +0200 Subject: [PATCH] manual_contol_setpoint: fix mode slot numbering (#12578) * This fix is necessary because usually uORB structs get initialized with all zeros and then get filled with the fields that are actually in use. * The number 0 for the mode slot was already commanding to switch to the mode in slot one even though for example the joystick input via mavlink does not use this mechanism at all. --- msg/manual_control_setpoint.msg | 18 +++++++++--------- src/modules/commander/Commander.cpp | 6 +++--- src/modules/sensors/rc_update.cpp | 11 +++++------ 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 10c66a9227..febe5edc4e 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -4,14 +4,14 @@ uint8 SWITCH_POS_NONE = 0 # switch is not mapped uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) -int8 MODE_SLOT_NONE = -1 # no mode slot assigned -int8 MODE_SLOT_1 = 0 # mode slot 1 selected -int8 MODE_SLOT_2 = 1 # mode slot 2 selected -int8 MODE_SLOT_3 = 2 # mode slot 3 selected -int8 MODE_SLOT_4 = 3 # mode slot 4 selected -int8 MODE_SLOT_5 = 4 # mode slot 5 selected -int8 MODE_SLOT_6 = 5 # mode slot 6 selected -int8 MODE_SLOT_MAX = 6 # number of slots plus one +uint8 MODE_SLOT_NONE = 0 # no mode slot assigned +uint8 MODE_SLOT_1 = 1 # mode slot 1 selected +uint8 MODE_SLOT_2 = 2 # mode slot 2 selected +uint8 MODE_SLOT_3 = 3 # mode slot 3 selected +uint8 MODE_SLOT_4 = 4 # mode slot 4 selected +uint8 MODE_SLOT_5 = 5 # mode slot 5 selected +uint8 MODE_SLOT_6 = 6 # mode slot 6 selected +uint8 MODE_SLOT_NUM = 6 # number of slots uint8 SOURCE_RC = 1 # radio control uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 @@ -61,7 +61,7 @@ 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 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/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9ae8fe253f..0349882b1a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -139,7 +139,7 @@ static float min_stick_change = 0.25f; static struct vehicle_status_s status = {}; static struct actuator_armed_s armed = {}; static struct safety_s safety = {}; -static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX]; +static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]; static struct commander_state_s internal_state = {}; static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; @@ -2696,12 +2696,12 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed /* we know something has changed - check if we are in mode slot operation */ if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) { - if (sp_man.mode_slot >= (int)(sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0]))) { + if (sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) { warnx("m slot overflow"); return TRANSITION_DENIED; } - int new_mode = _flight_mode_slots[sp_man.mode_slot]; + int new_mode = _flight_mode_slots[sp_man.mode_slot - 1]; if (new_mode < 0) { /* slot is unused */ diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 8d129c0853..9df56ac5af 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -350,9 +350,8 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f); if (_parameters.rc_map_flightmode > 0) { - - /* the number of valid slots equals the index of the max marker minus one */ - const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX; + /* number of valid slots */ + const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM; /* the half width of the range of a slot is the total range * divided by the number of slots, again divided by two @@ -369,10 +368,10 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) * will take us to the correct final index. */ manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) / - (slot_max - slot_min)) + (1.0f / num_slots)); + (slot_max - slot_min)) + (1.0f / num_slots)) + 1; - if (manual.mode_slot >= num_slots) { - manual.mode_slot = num_slots - 1; + if (manual.mode_slot > num_slots) { + manual.mode_slot = num_slots; } }