Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

precalculate divisors in initRcProcessing

Browse source
This commit is contained in:
Nicola De Pasquale 2020-03-13 19:57:34 +01:00
parent 04e5e3bf24
commit 57a5fd937a
1 changed files with 19 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/fc/rc.c
View file

@ -71,6 +71,8 @@ static bool reverseMotors = false;
static applyRatesFn *applyRates;
static uint16_t currentRxRefreshRate;
static bool isRxDataNew = false;
static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f;
FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber;
@ -231,6 +233,7 @@ float getRcCurveSlope(int axis, float deflection)
static void calculateSetpointRate(int axis)
{
float angleRate;
float rcCommandf;
#ifdef USE_GPS_RESCUE
if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
@ -244,9 +247,13 @@ static void calculateSetpointRate(int axis)
} else
#endif
{
const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
// scale rcCommandf to range [-1.0, 1.0]
float rcCommandf = rcCommand[axis] / (500.0f - deadband);
if (axis == FD_YAW) {
rcCommandf = rcCommand[axis] / rcCommandYawDivider;
} else {
rcCommandf = rcCommand[axis] / rcCommandDivider;
}
rcDeflection[axis] = rcCommandf;
const float rcCommandfAbs = fabsf(rcCommandf);
rcDeflectionAbs[axis] = rcCommandfAbs;
@ -683,10 +690,15 @@ FAST_CODE void processRcCommand(void)
#ifdef USE_INTERPOLATED_SP
if (isRxDataNew) {
float rcCommandf;
for (int i = FD_ROLL; i <= FD_YAW; i++) {
oldRcCommand[i] = rcCommand[i];
const uint8_t deadband = i == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
const float rcCommandf = rcCommand[i] / (500.0f - deadband);
if (i == FD_YAW) {
rcCommandf = rcCommand[i] / rcCommandYawDivider;
} else {
rcCommandf = rcCommand[i] / rcCommandDivider;
}
const float rcCommandfAbs = fabsf(rcCommandf);
rawSetpoint[i] = applyRates(i, rcCommandf, rcCommandfAbs);
rawDeflection[i] = rcCommandf;
@ -837,6 +849,9 @@ bool isMotorsReversed(void)
void initRcProcessing(void)
{
rcCommandDivider = 500.0f - rcControlsConfig()->deadband;
rcCommandYawDivider = 500.0f - rcControlsConfig()->yaw_deadband;
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8;
uint8_t y = 1;