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

use floats for rc interpolation

Browse source 
do not filter setpoint data
This commit is contained in:
rav 2017-05-04 00:16:46 +02:00
parent ec0373d7c7
commit 33043d79dc
4 changed files with 32 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/flight/pid.c
View file

@ -437,13 +437,18 @@ 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 dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
const float rD = dynC * currentPidSetpoint - gyroRateD; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
// if crash recovery is on check for a crash
@ -457,10 +462,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
}
// apply filters
delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
}