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

d-term for fixed wing

Browse source
This commit is contained in:
Alexander van Saase 2021-02-23 22:38:24 +01:00
parent 7c9d3a6058
commit 118d41d60a
4 changed files with 55 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/main/fc/settings.yaml
View file

@ -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

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

@ -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;

2
src/main/flight/pid.h
View file

@ -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;

2
src/main/io/osd.c
View file

@ -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);