fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 10:03:34 +08:00
Code Issues 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 12:27:58 -04: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
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 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 */
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);
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 &parameter_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;
}
}