From 118d41d60a90d1063a99edb55272e7de79599e07 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 23 Feb 2021 22:38:24 +0100 Subject: [PATCH] d-term for fixed wing --- src/main/fc/settings.yaml | 22 ++++++++++++++++-- src/main/flight/pid.c | 48 +++++++++++++++++++++++++++------------ src/main/flight/pid.h | 2 +- src/main/io/osd.c | 2 +- 4 files changed, 55 insertions(+), 19 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8..63040416a4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -136,7 +136,7 @@ tables: values: ["HIGH", "MEDIUM", "LOW"] enum: dynamicFilterRange_e - name: pidTypeTable - values: ["NONE", "PID", "PIFF", "AUTO"] + values: ["NONE", "PID", "PIDFF", "AUTO"] enum: pidType_e - name: osd_ahi_style values: ["DEFAULT", "LINE"] @@ -1464,6 +1464,12 @@ groups: field: bank_fw.pid[PID_PITCH].I min: 0 max: 200 + - name: fw_d_pitch + description: "Fixed wing rate stabilisation D-gain for PITCH" + default_value: "0" + field: bank_fw.pid[PID_PITCH].D + min: 0 + max: 200 - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" default_value: "50" @@ -1482,6 +1488,12 @@ groups: field: bank_fw.pid[PID_ROLL].I min: 0 max: 200 + - name: fw_d_roll + description: "Fixed wing rate stabilisation D-gain for roll" + default_value: "0" + field: bank_fw.pid[PID_ROLL].D + min: 0 + max: 200 - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" default_value: "50" @@ -1500,6 +1512,12 @@ groups: field: bank_fw.pid[PID_YAW].I min: 0 max: 200 + - name: fw_d_yaw + description: "Fixed wing rate stabilisation D-gain for YAW" + default_value: "0" + field: bank_fw.pid[PID_YAW].D + min: 0 + max: 200 - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" default_value: "60" @@ -1847,7 +1865,7 @@ groups: min: 1 max: 30 - name: pid_type - description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`" + description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIDFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIDFF` and multirotors `PID`" field: pidControllerType table: pidTypeTable - name: mc_cd_lpf_hz diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 17ba8a758b..089391759d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -343,7 +343,7 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { - if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { + if (usedPidControllerType == PID_TYPE_PIDFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, TASK_PERIOD_HZ(TASK_AUX_RATE_HZ) * 1e-6f); pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } @@ -441,7 +441,7 @@ void updatePIDCoefficients() STATIC_FASTRAM uint16_t prevThrottle = 0; // Check if throttle changed. Different logic for fixed wing vs multirotor - if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { + if (usedPidControllerType == PID_TYPE_PIDFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); if (filteredThrottle != prevThrottle) { prevThrottle = filteredThrottle; @@ -474,16 +474,16 @@ void updatePIDCoefficients() return; } - const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); + const float tpaFactor = usedPidControllerType == PID_TYPE_PIDFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { // Airplanes - scale all PIDs according to TPA pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; - pidState[axis].kD = 0.0f; + pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor; pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; pidState[axis].kCD = 0.0f; pidState[axis].kT = 0.0f; @@ -497,7 +497,7 @@ void updatePIDCoefficients() pidState[axis].kFF = 0.0f; // Tracking anti-windup requires P/I/D to be all defined which is only true for MC - if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) { + if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) { pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP)); } else { pidState[axis].kT = 0; @@ -638,6 +638,21 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } + // Calculate new D-term + float newDTerm; + if (pidState->kD == 0) { + // optimisation for when D8 is zero, often used by YAW axis + newDTerm = 0; + } else { + float delta = pidState->previousRateGyro - pidState->gyroRate; + + delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta); + delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta); + + // Calculate derivative + newDTerm = delta * (pidState->kD / dT); // * applyDBoost(pidState, dT); + } + applyItermLimiting(pidState); if (pidProfile()->fixedWingItermThrowLimit != 0) { @@ -650,14 +665,17 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh } #endif - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; - axisPID_D[axis] = newFFTerm; + axisPID_D[axis] = newDTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif + + pidState->previousRateGyro = pidState->gyroRate; + } static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate) @@ -949,7 +967,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng void checkItermLimitingActive(pidState_t *pidState) { bool shouldActivate; - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { @@ -961,7 +979,7 @@ void checkItermLimitingActive(pidState_t *pidState) void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { + if (usedPidControllerType == PID_TYPE_PIDFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ @@ -1115,7 +1133,7 @@ void pidInit(void) mixerConfig()->platformType == PLATFORM_BOAT || mixerConfig()->platformType == PLATFORM_ROVER ) { - usedPidControllerType = PID_TYPE_PIFF; + usedPidControllerType = PID_TYPE_PIDFF; } else { usedPidControllerType = PID_TYPE_PID; } @@ -1141,7 +1159,7 @@ void pidInit(void) } } - if (usedPidControllerType == PID_TYPE_PIFF) { + if (usedPidControllerType == PID_TYPE_PIDFF) { pidControllerApplyFn = pidApplyFixedWingRateController; } else if (usedPidControllerType == PID_TYPE_PID) { pidControllerApplyFn = pidApplyMulticopterRateController; @@ -1160,15 +1178,15 @@ void pidInit(void) } const pidBank_t * pidBank(void) { - return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; + return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } pidBank_t * pidBankMutable(void) { - return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; + return usedPidControllerType == PID_TYPE_PIDFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex) { - if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) { + if (pidIndexGetType(pidIndex) == PID_TYPE_PIDFF) { return &pidBank->pid[pidIndex].FF; } else { return &pidBank->pid[pidIndex].D; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d627ed4b6c..47185ce0d8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,7 +76,7 @@ typedef enum { typedef enum { PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration PID_TYPE_PID, // Uses P, I and D terms - PID_TYPE_PIFF, // Uses P, I and FF, ignoring D + PID_TYPE_PIDFF, // Uses P, I, D and FF PID_TYPE_AUTO, // Autodetect } pidType_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a02d79720b..bf7de42288 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1195,7 +1195,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIDFF ? pid->FF : pid->D); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);