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

make sure rxRefreshRate > 0

Browse source
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
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(); rxRefreshRate = rxGetRefreshRate();
} }
if (isRXDataNew) { if (isRXDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel=ROLL; channel < interpolationChannels; channel++) { for (int channel=ROLL; channel < interpolationChannels; channel++) {
@ -227,10 +227,8 @@ void processRcCommand(void)
rcCommandInterp[channel] += rcStepSize[channel]; rcCommandInterp[channel] += rcStepSize[channel];
rcCommand[channel] = rcCommandInterp[channel]; rcCommand[channel] = rcCommandInterp[channel];
readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
readyToCalculateRate = true;
} }
} else { readyToCalculateRate = true;
rcInterpolationStepCount = 0;
} }
} else { } else {
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping 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 // -----calculate D component
if (axis != FD_YAW) { if (axis != FD_YAW) {
// apply filters // apply filters
float gyroRateD = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
gyroRateD = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateD); gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
float dynC = dtermSetpointWeight; float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) { if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); 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) // Divide rate change by dT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT; float delta = (rD - previousRateError[axis]) / dT;