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

Merge pull request #2117 from betaflight/fix_fpvanglemix

Browse source 
Fix fpv angle mix // minor optimalisation
This commit is contained in:
borisbstyle 2017-01-14 23:16:12 +01:00 • committed by GitHub
commit 05e5beee7e
1 changed files with 12 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

20
src/main/fc/fc_core.c
View file

@ -209,9 +209,10 @@ void processRcCommand(void)
static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor; static int16_t factor, rcInterpolationFactor;
static uint16_t currentRxRefreshRate; static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 1; const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
uint16_t rxRefreshRate; uint16_t rxRefreshRate;
bool readyToCalculateRate = false; bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
if (isRXDataNew) { if (isRXDataNew) {
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
@ -241,7 +242,7 @@ void processRcCommand(void)
debug[3] = rxRefreshRate; debug[3] = rxRefreshRate;
} }
for (int channel=ROLL; channel <= interpolationChannels; channel++) { for (int channel=ROLL; channel < interpolationChannels; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel]; lastCommand[channel] = rcCommand[channel];
} }
@ -252,16 +253,18 @@ void processRcCommand(void)
} }
// Interpolate steps of rcCommand // Interpolate steps of rcCommand
if (factor > 0) { for (int channel=ROLL; channel < interpolationChannels; channel++) {
for (int channel=ROLL; channel <= interpolationChannels; channel++) if (factor > 0) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
} else { readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
factor = 0; } else {
factor = 0;
}
} }
readyToCalculateRate = true; readyToCalculateRate = true;
} else { } else {
factor = 0; // reset factor in case of level modes flip flopping factor = 0; // reset factor in case of level modes flip flopping
readyToCalculateRateAxisCnt = FD_YAW;
} }
if (readyToCalculateRate || isRXDataNew) { if (readyToCalculateRate || isRXDataNew) {
@ -269,7 +272,8 @@ void processRcCommand(void)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle(); scaleRcCommandToFpvCamAngle();
for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]); for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
calculateSetpointRate(axis, rcCommand[axis]);
isRXDataNew = false; isRXDataNew = false;
} }