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

bit more fine tuning

Browse source
This commit is contained in:
ctzsnooze 2018-06-10 10:15:19 +10:00
parent 6d2c1a3c01
commit 53476d45de
1 changed files with 10 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -819,18 +819,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf)); DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf)); DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf));
} }
if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) { if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate); itermErrorRate = (1 - setpointHpf / 30.0f) * (currentPidSetpoint - gyroRate);
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint)); DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf((1 - setpointHpf / 60.0f) * 100)); DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf((1 - setpointHpf / 30.0f) * 100));
} }
} }
if (itermRelaxType == ITERM_RELAX_GYRO ) { if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(gyroRate + setpointHpf)); DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(gyroRate - setpointHpf));
} }
} }
@ -851,11 +850,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
} else { } else {
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
} }
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(acErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
}
#endif // USE_ABSOLUTE_CONTROL #endif // USE_ABSOLUTE_CONTROL
} else } else
#endif // USE_ITERM_RELAX #endif // USE_ITERM_RELAX
@ -872,8 +866,13 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
currentPidSetpoint += acCorrection; currentPidSetpoint += acCorrection;
itermErrorRate += acCorrection; itermErrorRate += acCorrection;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(acErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
}
} }
#endif #endif
float errorRate = currentPidSetpoint - gyroRate; // r - y float errorRate = currentPidSetpoint - gyroRate; // r - y
handleCrashRecovery( handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,