rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed rates / interpolation bug.

This commit is contained in:
Michael Keller 2018-01-24 12:27:07 +13:00
parent 67523493a0
commit 1f9c828f2a
1 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/fc/fc_rc.c
View File

@ -192,8 +192,7 @@ void processRcCommand(void)
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
uint8_t updatedChannel = 0;
if (rxConfig()->rcInterpolation) {
// Set RC refresh rate for sampling and channels to filter
@ -213,7 +212,7 @@ void processRcCommand(void)
if (isRXDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel=ROLL; channel < interpolationChannels; channel++) {
for (int channel = ROLL; channel < interpolationChannels; channel++) {
rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
}
@ -229,27 +228,22 @@ void processRcCommand(void)
// Interpolate steps of rcCommand
if (rcInterpolationStepCount > 0) {
for (int channel=ROLL; channel < interpolationChannels; channel++) {
rcCommandInterp[channel] += rcStepSize[channel];
rcCommand[channel] = rcCommandInterp[channel];
readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
}
readyToCalculateRate = true;
}
} else {
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
}
if (readyToCalculateRate || isRXDataNew) {
if (isRXDataNew) {
readyToCalculateRateAxisCnt = FD_YAW;
}
if (isRXDataNew || updatedChannel) {
const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
#if defined(SITL)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
#endif
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) {
for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
#if defined(SITL)
#pragma GCC diagnostic pop
#endif
@ -260,11 +254,14 @@ void processRcCommand(void)
debug[2] = rcInterpolationStepCount;
debug[3] = setpointRate[0];
}
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
}
if (isRXDataNew) {
isRXDataNew = false;
}
}