fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

rc_update: replace RC scaling with interpolate function use

Browse Source
This commit is contained in:
Matthias Grob
2022-11-23 11:06:44 -05:00
committed by Daniel Agar
parent 9de3af1cbc
commit d81ca65c6f
1 changed files with 10 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rc_update/rc_update.cpp
+10 -34
View File
@@ -450,46 +450,22 @@ void RCUpdate::Run()
/* read out and scale values from raw message even if signal is invalid */ /* read out and scale values from raw message even if signal is invalid */
for (unsigned int i = 0; i < channel_count_limited; i++) { 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];
/* // piecewise linear function to apply RC calibration
* 1) Constrain to min/max values, as later processing depends on bounds. _rc.channels[i] = math::interpolateNXY(value,
*/ {min, trim - dz, trim + dz, max},
input_rc.values[i] = math::constrain(input_rc.values[i], _parameters.min[i], _parameters.max[i]); {-1.f, 0.f, 0.f, 1.f});
/*
* 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;
}
if (_parameters.rev[i]) { if (_parameters.rev[i]) {
_rc.channels[i] = -_rc.channels[i]; _rc.channels[i] = -_rc.channels[i];
} }
/* handle any parameter-induced blowups */ /* handle any parameter-induced blowups */
if (!PX4_ISFINITE(_rc.channels[i])) { if (!PX4_ISFINITE(_rc.channels[i])) {
_rc.channels[i] = 0.f; _rc.channels[i] = 0.f;