dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rc_update: replace RC scaling with interpolate function use

This commit is contained in:
Matthias Grob 2022-11-21 18:43:25 +01:00 committed by Daniel Agar
parent 9de3af1cbc
commit d81ca65c6f
1 changed files with 10 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
src/modules/rc_update/rc_update.cpp
View File

@ -450,46 +450,22 @@ void RCUpdate::Run()
/* read out and scale values from raw message even if signal is invalid */
for (unsigned int i = 0; i < channel_count_limited; i++) {
// float conversions of uint16_t values
const float value = input_rc.values[i];
const float min = _parameters.min[i];
const float trim = _parameters.trim[i];
const float max = _parameters.max[i];
const float dz = _parameters.dz[i];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
input_rc.values[i] = math::constrain(input_rc.values[i], _parameters.min[i], _parameters.max[i]);
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* the total range is 2 (-1..1).
* If center (trim) == min, scale to 0..1, if center (trim) == max,
* scale to -1..0.
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (input_rc.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.channels[i] = (input_rc.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(
_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (input_rc.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
_rc.channels[i] = (input_rc.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(
_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
_rc.channels[i] = 0.f;
}
// piecewise linear function to apply RC calibration
_rc.channels[i] = math::interpolateNXY(value,
{min, trim - dz, trim + dz, max},
{-1.f, 0.f, 0.f, 1.f});
if (_parameters.rev[i]) {
_rc.channels[i] = -_rc.channels[i];
}
/* handle any parameter-induced blowups */
if (!PX4_ISFINITE(_rc.channels[i])) {
_rc.channels[i] = 0.f;