diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index daccde964b..c2858c1761 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -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;