Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

PID: Removed D from yaw axis - no positive effect

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-04-02 10:09:27 +10:00
parent 580a6209e5
commit e4cda3100f
2 changed files with 25 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/config/config.c
View file

@ -156,8 +156,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PITCH] = 60;
pidProfile->D8[PITCH] = 80;
pidProfile->P8[YAW] = 140; // 3.5 * 40
pidProfile->I8[YAW] = 40; // 4.0 * 40
pidProfile->D8[YAW] = 40; // 0.01 * 4000
pidProfile->I8[YAW] = 40; // 4.0 * 40
pidProfile->D8[YAW] = 0; // not used
pidProfile->P8[PIDALT] = 50; // NAV_POS_Z_P * 100
pidProfile->I8[PIDALT] = 0; // not used
pidProfile->D8[PIDALT] = 0; // not used

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

@ -201,34 +201,40 @@ static void pidApplyRateController(pidProfile_t *pidProfile, pidState_t *pidStat
int n;
float rateError = pidState->rateTarget - pidState->gyroRate;
float newDTerm;
// Calculate new P-term
float newPTerm = rateError * pidState->kP;
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == FD_YAW) {
newPTerm = constrain(newPTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
}
// Calculate new D-term
// Shift old error values
for (n = 4; n > 0; n--) {
pidState->rateErrorBuf[n] = pidState->rateErrorBuf[n-1];
if (axis == FD_YAW) {
newDTerm = 0;
}
// Store new error value
pidState->rateErrorBuf[0] = rateError;
// Calculate derivative using 5-point noise-robust differentiator by Pavel Holoborodko
float newDTerm = ((2 * (pidState->rateErrorBuf[1] - pidState->rateErrorBuf[3]) + (pidState->rateErrorBuf[0] - pidState->rateErrorBuf[4])) / (8 * dT)) * pidState->kD;
// Apply additional lowpass
if (pidProfile->dterm_lpf_hz) {
if (!pidState->deltaFilterInit) {
filterInitBiQuad(pidProfile->dterm_lpf_hz, &pidState->deltaBiQuadState, 0);
pidState->deltaFilterInit = true;
else {
// Shift old error values
for (n = 4; n > 0; n--) {
pidState->rateErrorBuf[n] = pidState->rateErrorBuf[n-1];
}
newDTerm = filterApplyBiQuad(newDTerm, &pidState->deltaBiQuadState);
// Store new error value
pidState->rateErrorBuf[0] = rateError;
// Calculate derivative using 5-point noise-robust differentiator by Pavel Holoborodko
newDTerm = ((2 * (pidState->rateErrorBuf[1] - pidState->rateErrorBuf[3]) + (pidState->rateErrorBuf[0] - pidState->rateErrorBuf[4])) / (8 * dT)) * pidState->kD;
// Apply additional lowpass
if (pidProfile->dterm_lpf_hz) {
if (!pidState->deltaFilterInit) {
filterInitBiQuad(pidProfile->dterm_lpf_hz, &pidState->deltaBiQuadState, 0);
pidState->deltaFilterInit = true;
}
newDTerm = filterApplyBiQuad(newDTerm, &pidState->deltaBiQuadState);
}
}
// TODO: Get feedback from mixer on available correction range for each axis