fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sensors: Add integer rounding prottection, add comments so people less familiar with integer arithmetic can follow

Browse Source
This commit is contained in:
Lorenz Meier 2016-03-13 11:48:41 +01:00
parent 6bcf38c4bb
commit 99e686ea08
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/sensors/sensors.cpp
View File

@ -1900,11 +1900,21 @@ Sensors::rc_poll()
/* the number of valid slots is one less than the max marker */
const unsigned num_slots = manual_control_setpoint_s::MODE_SLOT_MAX - 1;
/* min is -1, max is +1, range is 2. We offset below min and max */
const float slot_min = -1.0f - 2.0f / num_slots / 2.0f;
const float slot_max = 1.0f + 2.0f / num_slots / 2.0f;
/* the half width of the range of a slot is the total range
* divided by the number of slots, again divided by two
*/
const float slot_width_half = 2.0f / num_slots / 2.0f;
manual.mode_slot = (((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) /
/* min is -1, max is +1, range is 2. We offset below min and max */
const float slot_min = -1.0f - slot_width_half;
const float slot_max = 1.0f + slot_width_half;
/* the slot gets mapped by first normalizing into a 0..1 interval using min
* and max. Then the right slot is obtained by multiplying with the number of
* slots. And finally we add half a slot width to ensure that integer rounding
* 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 / num_slots;
}