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/PIFF refactoring (#5129)

Browse source 
* Stage 1 PID/PIFF refactoring

* Fix F3 compilation

* Minor speedup

* Make PID controller type configurable

* fix compilation

* Further minor optimizations

* Fix mixer integration

* Move some mixer routines to FASTRAM and precompute constants

* Run applyMotorRateLimiting only when needed

* Revert "Move some mixer routines to FASTRAM and precompute constants"

This reverts commit 31370c1288.

* Revert "Minor speedup"

This reverts commit 02b58e5f05.

* fix compilation

* Remove unused parameter

* Fix F3
This commit is contained in:
Paweł Spychalski 2019-11-29 20:22:27 +01:00 • committed by GitHub
parent d816e859f7
commit ca46d7ab28
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 204 additions and 128 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/common/maths.c
View file

@ -23,6 +23,7 @@
#include "maths.h" #include "maths.h"
#include "vector.h" #include "vector.h"
#include "quaternion.h" #include "quaternion.h"
#include "platform.h"
// http://lolengine.net/blog/2011/12/21/better-function-approximations // http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
@ -158,7 +159,7 @@ int constrain(int amt, int low, int high)
return amt; return amt;
} }
float constrainf(float amt, float low, float high) float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
{ {
if (amt < low) if (amt < low)
return low; return low;

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

@ -135,6 +135,9 @@ tables:
- name: dynamicFilterRangeTable - name: dynamicFilterRangeTable
values: ["HIGH", "MEDIUM", "LOW"] values: ["HIGH", "MEDIUM", "LOW"]
enum: dynamicFilterRange_e enum: dynamicFilterRange_e
- name: pidTypeTable
values: ["NONE", "PID", "PIFF", "AUTO"]
enum: pidType_e
groups: groups:
- name: PG_GYRO_CONFIG - name: PG_GYRO_CONFIG
@ -1225,6 +1228,9 @@ groups:
condition: USE_ANTIGRAVITY condition: USE_ANTIGRAVITY
min: 1 min: 1
max: 30 max: 30
- name: pid_type
field: pidControllerType
table: pidTypeTable
- name: PG_PID_AUTOTUNE_CONFIG - name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t type: pidAutotuneConfig_t

92
src/main/flight/mixer.c
View file

@ -110,6 +110,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
static void computeMotorCount(void) static void computeMotorCount(void)
{ {
motorCount = 0; motorCount = 0;
@ -154,6 +157,49 @@ void mixerUpdateStateFlags(void)
} }
} }
void nullMotorRateLimiting(const float dT)
{
UNUSED(dT);
}
void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void mixerInit(void) void mixerInit(void)
{ {
computeMotorCount(); computeMotorCount();
@ -164,6 +210,12 @@ void mixerInit(void)
} }
mixerResetDisarmedMotors(); mixerResetDisarmedMotors();
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
motorRateLimitingApplyFn = applyMotorRateLimiting;
} else {
motorRateLimitingApplyFn = nullMotorRateLimiting;
}
} }
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
@ -240,44 +292,6 @@ void stopPwmAllMotors(void)
pwmShutdownPulsesForAllMotors(motorCount); pwmShutdownPulsesForAllMotors(motorCount);
} }
static void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void FAST_CODE NOINLINE mixTable(const float dT) void FAST_CODE NOINLINE mixTable(const float dT)
{ {
int16_t input[3]; // RPY, range [-500:+500] int16_t input[3]; // RPY, range [-500:+500]
@ -414,7 +428,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
} }
/* Apply motor acceleration/deceleration limit */ /* Apply motor acceleration/deceleration limit */
applyMotorRateLimiting(dT); motorRateLimitingApplyFn(dT);
} }
motorStatus_e getMotorStatus(void) motorStatus_e getMotorStatus(void)

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

@ -91,14 +91,15 @@ typedef struct {
pt1Filter_t dBoostLpf; pt1Filter_t dBoostLpf;
biquadFilter_t dBoostGyroLpf; biquadFilter_t dBoostGyroLpf;
#endif #endif
bool itermLimitActive;
} pidState_t; } pidState_t;
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn; STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
STATIC_FASTRAM bool pidFiltersConfigured = false; STATIC_FASTRAM bool pidFiltersConfigured = false;
FASTRAM float headingHoldCosZLimit; static EXTENDED_FASTRAM float headingHoldCosZLimit;
FASTRAM int16_t headingHoldTarget; static EXTENDED_FASTRAM int16_t headingHoldTarget;
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
STATIC_FASTRAM pt1Filter_t fixedWingTpaFilter; static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
STATIC_FASTRAM bool pidGainsUpdateRequired; STATIC_FASTRAM bool pidGainsUpdateRequired;
@ -132,8 +133,19 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
static EXTENDED_FASTRAM uint16_t yawPLimit; static EXTENDED_FASTRAM uint16_t yawPLimit;
static EXTENDED_FASTRAM uint8_t yawLpfHz; static EXTENDED_FASTRAM uint8_t yawLpfHz;
static EXTENDED_FASTRAM uint16_t pidSumLimit;
static EXTENDED_FASTRAM float motorItermWindupPoint;
static EXTENDED_FASTRAM float antiWindupScaler;
#ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = { .bank_mc = {
@ -238,40 +250,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.antigravityGain = 1.0f, .antigravityGain = 1.0f,
.antigravityAccelerator = 1.0f, .antigravityAccelerator = 1.0f,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO,
); );
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
yawPLimit = pidProfile()->yaw_p_limit;
} else {
yawPLimit = 0;
}
yawLpfHz = pidProfile()->yaw_lpf_hz;
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
}
bool pidInitFilters(void) bool pidInitFilters(void)
{ {
const uint32_t refreshRate = getLooptime(); const uint32_t refreshRate = getLooptime();
@ -338,7 +319,7 @@ bool pidInitFilters(void)
void pidResetTPAFilter(void) void pidResetTPAFilter(void)
{ {
if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) { if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f); pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle); pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
} }
@ -436,7 +417,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
STATIC_FASTRAM uint16_t prevThrottle = 0; STATIC_FASTRAM uint16_t prevThrottle = 0;
// Check if throttle changed. Different logic for fixed wing vs multirotor // Check if throttle changed. Different logic for fixed wing vs multirotor
if (STATE(FIXED_WING) && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT); uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT);
if (filteredThrottle != prevThrottle) { if (filteredThrottle != prevThrottle) {
prevThrottle = filteredThrottle; prevThrottle = filteredThrottle;
@ -451,7 +432,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
} }
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
if (!STATE(FIXED_WING)) { if (usedPidControllerType == PID_TYPE_PID) {
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]); antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
} }
#endif #endif
@ -468,12 +449,12 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
return; return;
} }
const float tpaFactor = STATE(FIXED_WING) ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
// PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments // 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 //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
if (STATE(FIXED_WING)) { if (usedPidControllerType == PID_TYPE_PIFF) {
// Airplanes - scale all PIDs according to TPA // Airplanes - scale all PIDs according to TPA
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; 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].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
@ -598,6 +579,20 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynami
return newPTerm; return newPTerm;
} }
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{ {
const float rateError = pidState->rateTarget - pidState->gyroRate; const float rateError = pidState->rateTarget - pidState->gyroRate;
@ -607,11 +602,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
// Calculate integral // Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT; pidState->errorGyroIf += rateError * pidState->kI * dT;
if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) { applyItermLimiting(pidState);
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
if (pidProfile()->fixedWingItermThrowLimit != 0) { if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
@ -623,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
} }
#endif #endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit); axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidSumLimit, +pidSumLimit);
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm; axisPID_P[axis] = newPTerm;
@ -660,7 +651,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
} }
} }
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
float dBoost = 1.0f; float dBoost = 1.0f;
@ -681,22 +672,21 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
return dBoost; return dBoost;
} }
#else #else
static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { static float applyDBoost(pidState_t *pidState, float dT) {
UNUSED(pidState); UNUSED(pidState);
UNUSED(axis);
UNUSED(dT); UNUSED(dT);
return 1.0f; return 1.0f;
} }
#endif #endif
static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{ {
const float rateError = pidState->rateTarget - pidState->gyroRate; const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, axis, rateError, dT); const float newPTerm = pTermProcess(pidState, axis, rateError, dT);
// Calculate new D-term // Calculate new D-term
float newDTerm; float newDTerm;
if (pidBank()->pid[axis].D == 0) { if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis // optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0; newDTerm = 0;
} else { } else {
@ -707,15 +697,13 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered); deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
// Apply additional lowpass // Apply additional lowpass
if (pidProfile()->dterm_lpf_hz) { deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
}
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered); firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter); newDTerm = firFilterApply(&pidState->gyroRateFilter);
// Calculate derivative // Calculate derivative
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis, dT); newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
// Additionally constrain D // Additionally constrain D
newDTerm = constrainf(newDTerm, -300.0f, 300.0f); newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
@ -723,17 +711,12 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
// TODO: Get feedback from mixer on available correction range for each axis // TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf; const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutputLimited = constrainf(newOutput, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit); const float newOutputLimited = constrainf(newOutput, -pidSumLimit, +pidSumLimit);
// Prevent strong Iterm accumulation during stick inputs
const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
float itermErrorRate = rateError; float itermErrorRate = rateError;
applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate); applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
#ifdef USE_ANTIGRAVITY #ifdef USE_ANTIGRAVITY
const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
itermErrorRate *= iTermAntigravityGain; itermErrorRate *= iTermAntigravityGain;
#endif #endif
@ -741,11 +724,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
// Don't grow I-term if motors are at their limit // Don't grow I-term if motors are at their limit
if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) { applyItermLimiting(pidState);
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
axisPID[axis] = newOutputLimited; axisPID[axis] = newOutputLimited;
@ -937,7 +916,20 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
} }
void FAST_CODE pidController(float dT) void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
shouldActivate = mixerIsOutputSaturated();
}
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
}
void FAST_CODE NOINLINE pidController(float dT)
{ {
if (!pidFiltersConfigured) { if (!pidFiltersConfigured) {
return; return;
@ -988,30 +980,29 @@ void FAST_CODE pidController(float dT)
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
} }
// Apply setpoint rate of change limits // Prevent strong Iterm accumulation during stick inputs
for (int axis = 0; axis < 3; axis++) { antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
} #ifdef USE_ANTIGRAVITY
iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
#endif
// Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller // Apply setpoint rate of change limits
if (STATE(FIXED_WING)) { pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
pidApplyFixedWingRateController(&pidState[axis], axis, dT);
} // Step 4: Run gyro-driven control
else { checkItermLimitingActive(&pidState[axis]);
pidApplyMulticopterRateController(&pidState[axis], axis, dT); pidControllerApplyFn(&pidState[axis], axis, dT);
}
} }
} }
pidType_e pidIndexGetType(pidIndex_e pidIndex) pidType_e pidIndexGetType(pidIndex_e pidIndex)
{ {
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType;
}
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
// FW specific
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return PID_TYPE_PIFF;
}
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) { if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE; return PID_TYPE_NONE;
} }
@ -1022,3 +1013,61 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
} }
return PID_TYPE_PID; return PID_TYPE_PID;
} }
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
yawPLimit = pidProfile()->yaw_p_limit;
} else {
yawPLimit = 0;
}
yawLpfHz = pidProfile()->yaw_lpf_hz;
pidSumLimit = pidProfile()->pidSumLimit;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if (STATE(FIXED_WING)) {
usedPidControllerType = PID_TYPE_PIFF;
} else {
usedPidControllerType = PID_TYPE_PID;
}
} else {
usedPidControllerType = pidProfile()->pidControllerType;
}
if (pidProfile()->dterm_lpf_hz) {
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
} else {
dTermLpfFilterApplyFn = nullFilterApply;
}
if (usedPidControllerType == PID_TYPE_PIFF) {
pidControllerApplyFn = pidApplyFixedWingRateController;
} else if (usedPidControllerType == PID_TYPE_PID) {
pidControllerApplyFn = pidApplyMulticopterRateController;
} else {
pidControllerApplyFn = nullRateController;
}
}

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

@ -72,9 +72,10 @@ typedef enum {
// TODO(agh): PIDFF // TODO(agh): PIDFF
typedef enum { typedef enum {
PID_TYPE_NONE, // Not used in the current platform/mixer/configuration PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D PID_TYPE_PIFF, // Uses P, I and FF, ignoring D
PID_TYPE_AUTO, // Autodetect
} pidType_e; } pidType_e;
typedef struct pid8_s { typedef struct pid8_s {
@ -100,6 +101,7 @@ typedef enum {
} itermRelaxType_e; } itermRelaxType_e;
typedef struct pidProfile_s { typedef struct pidProfile_s {
uint8_t pidControllerType;
pidBank_t bank_fw; pidBank_t bank_fw;
pidBank_t bank_mc; pidBank_t bank_mc;
@ -155,8 +157,10 @@ typedef struct pidAutotuneConfig_s {
PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } static uint8_t usedPidControllerType;
static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
static inline const pidBank_t * pidBank(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
static inline pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
extern int16_t axisPID[]; extern int16_t axisPID[];
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];

2
src/main/target/OMNIBUS/target.h
View file

@ -154,3 +154,5 @@
#define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#undef USE_PWM_SERVO_DRIVER