Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

manual_contol_setpoint: fix mode slot numbering (#12578)

Browse Source 
* 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.
This commit is contained in:
Matthias Grob
2019-08-01 18:27:58 +02:00
committed by Daniel Agar
parent 5d986f2030
commit d0f1a551e9
3 changed files with 17 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/manual_control_setpoint.msg
+9 -9
View File
@@ -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_ON = 1 # switch activated (value = 1)
uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0)
uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1)
int8 MODE_SLOT_NONE = -1 # no mode slot assigned uint8 MODE_SLOT_NONE = 0 # no mode slot assigned
int8 MODE_SLOT_1 = 0 # mode slot 1 selected uint8 MODE_SLOT_1 = 1 # mode slot 1 selected
int8 MODE_SLOT_2 = 1 # mode slot 2 selected uint8 MODE_SLOT_2 = 2 # mode slot 2 selected
int8 MODE_SLOT_3 = 2 # mode slot 3 selected uint8 MODE_SLOT_3 = 3 # mode slot 3 selected
int8 MODE_SLOT_4 = 3 # mode slot 4 selected uint8 MODE_SLOT_4 = 4 # mode slot 4 selected
int8 MODE_SLOT_5 = 4 # mode slot 5 selected uint8 MODE_SLOT_5 = 5 # mode slot 5 selected
int8 MODE_SLOT_6 = 5 # mode slot 6 selected uint8 MODE_SLOT_6 = 6 # mode slot 6 selected
int8 MODE_SLOT_MAX = 6 # number of slots plus one uint8 MODE_SLOT_NUM = 6 # number of slots
uint8 SOURCE_RC = 1 # radio control uint8 SOURCE_RC = 1 # radio control
uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0
uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 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 arm_switch # arm/disarm switch: _DISARMED_, ARMED
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 gear_switch # landing gear switch: _DOWN_, UP 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 data_source # where this input is coming from
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED 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 uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
src/modules/commander/Commander.cpp
+3 -3
View File
@@ -139,7 +139,7 @@ static float min_stick_change = 0.25f;
static struct vehicle_status_s status = {}; static struct vehicle_status_s status = {};
static struct actuator_armed_s armed = {}; static struct actuator_armed_s armed = {};
static struct safety_s safety = {}; 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 struct commander_state_s internal_state = {};
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; 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 */ /* 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 != 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"); warnx("m slot overflow");
return TRANSITION_DENIED; 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) { if (new_mode < 0) {
/* slot is unused */ /* slot is unused */
src/modules/sensors/rc_update.cpp
+5 -6
View File
@@ -350,9 +350,8 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f); manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
if (_parameters.rc_map_flightmode > 0) { if (_parameters.rc_map_flightmode > 0) {
/* number of valid slots */
/* the number of valid slots equals the index of the max marker minus one */ const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM;
const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;
/* the half width of the range of a slot is the total range /* the half width of the range of a slot is the total range
* divided by the number of slots, again divided by two * divided by the number of slots, again divided by two
@@ -369,10 +368,10 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
* will take us to the correct final index. * 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) / 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) { if (manual.mode_slot > num_slots) {
manual.mode_slot = num_slots - 1; manual.mode_slot = num_slots;
} }
} }