mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
MW23 delta scaling to cycletime
This commit is contained in:
parent
f5de06c59e
commit
df159d195d
1 changed files with 3 additions and 0 deletions
|
@ -360,6 +360,9 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
||||||
lastErrorForDelta[axis] = gyroError;
|
lastErrorForDelta[axis] = gyroError;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Scale delta to looptime
|
||||||
|
delta = (delta * ((uint16_t) 0xFFFF)) / ((uint16_t)targetPidLooptime << 3);
|
||||||
|
|
||||||
if (deltaStateIsSet) {
|
if (deltaStateIsSet) {
|
||||||
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
|
DTerm = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
|
||||||
} else {
|
} else {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue