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

make sure rxRefreshRate > 0

This commit is contained in:
rav 2017-05-05 17:04:27 +02:00
parent d7c0fa4d6e
commit 001fb8d432
2 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -204,7 +204,7 @@ void processRcCommand(void)
rxRefreshRate = rxGetRefreshRate();
}
if (isRXDataNew) {
if (isRXDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel=ROLL; channel < interpolationChannels; channel++) {
@ -227,10 +227,8 @@ void processRcCommand(void)
rcCommandInterp[channel] += rcStepSize[channel];
rcCommand[channel] = rcCommandInterp[channel];
readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
readyToCalculateRate = true;
}
} else {
rcInterpolationStepCount = 0;
readyToCalculateRate = true;
}
} else {
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping

6
src/main/flight/pid.c
View File

@ -438,14 +438,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate D component
if (axis != FD_YAW) {
// apply filters
float gyroRateD = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
gyroRateD = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateD);
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRateD; // cr - y
const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT;