mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
PID/PIFF refactoring (#5129)
* 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 commit31370c1288. * Revert "Minor speedup" This reverts commit02b58e5f05. * fix compilation * Remove unused parameter * Fix F3
This commit is contained in:
Paweł Spychalski
GitHub
parent
d816e859f7
commit
ca46d7ab28
6 changed files with 204 additions and 128 deletions
|
|
@ -23,6 +23,7 @@
|
|||
#include "maths.h"
|
||||
#include "vector.h"
|
||||
#include "quaternion.h"
|
||||
#include "platform.h"
|
||||
|
||||
// http://lolengine.net/blog/2011/12/21/better-function-approximations
|
||||
// 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;
|
||||
}
|
||||
|
||||
float constrainf(float amt, float low, float high)
|
||||
float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
|
||||
{
|
||||
if (amt < low)
|
||||
return low;
|
||||
|
|
|
|||
|
|
@ -135,6 +135,9 @@ tables:
|
|||
- name: dynamicFilterRangeTable
|
||||
values: ["HIGH", "MEDIUM", "LOW"]
|
||||
enum: dynamicFilterRange_e
|
||||
- name: pidTypeTable
|
||||
values: ["NONE", "PID", "PIFF", "AUTO"]
|
||||
enum: pidType_e
|
||||
|
||||
groups:
|
||||
- name: PG_GYRO_CONFIG
|
||||
|
|
@ -1225,6 +1228,9 @@ groups:
|
|||
condition: USE_ANTIGRAVITY
|
||||
min: 1
|
||||
max: 30
|
||||
- name: pid_type
|
||||
field: pidControllerType
|
||||
table: pidTypeTable
|
||||
|
||||
- name: PG_PID_AUTOTUNE_CONFIG
|
||||
type: pidAutotuneConfig_t
|
||||
|
|
|
|||
|
|
@ -110,6 +110,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
|||
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
computeMotorCount();
|
||||
|
|
@ -164,6 +210,12 @@ void mixerInit(void)
|
|||
}
|
||||
|
||||
mixerResetDisarmedMotors();
|
||||
|
||||
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
|
||||
motorRateLimitingApplyFn = applyMotorRateLimiting;
|
||||
} else {
|
||||
motorRateLimitingApplyFn = nullMotorRateLimiting;
|
||||
}
|
||||
}
|
||||
|
||||
void mixerResetDisarmedMotors(void)
|
||||
|
|
@ -240,44 +292,6 @@ void stopPwmAllMotors(void)
|
|||
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)
|
||||
{
|
||||
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 */
|
||||
applyMotorRateLimiting(dT);
|
||||
motorRateLimitingApplyFn(dT);
|
||||
}
|
||||
|
||||
motorStatus_e getMotorStatus(void)
|
||||
|
|
|
|||
|
|
@ -91,14 +91,15 @@ typedef struct {
|
|||
pt1Filter_t dBoostLpf;
|
||||
biquadFilter_t dBoostGyroLpf;
|
||||
#endif
|
||||
bool itermLimitActive;
|
||||
} pidState_t;
|
||||
|
||||
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
|
||||
STATIC_FASTRAM bool pidFiltersConfigured = false;
|
||||
FASTRAM float headingHoldCosZLimit;
|
||||
FASTRAM int16_t headingHoldTarget;
|
||||
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
|
||||
STATIC_FASTRAM pt1Filter_t fixedWingTpaFilter;
|
||||
static EXTENDED_FASTRAM float headingHoldCosZLimit;
|
||||
static EXTENDED_FASTRAM int16_t headingHoldTarget;
|
||||
static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
|
||||
static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
|
||||
|
||||
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
|
||||
STATIC_FASTRAM bool pidGainsUpdateRequired;
|
||||
|
|
@ -132,8 +133,19 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
|
|||
|
||||
static EXTENDED_FASTRAM uint16_t yawPLimit;
|
||||
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,
|
||||
.bank_mc = {
|
||||
|
|
@ -238,40 +250,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
.antigravityGain = 1.0f,
|
||||
.antigravityAccelerator = 1.0f,
|
||||
.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)
|
||||
{
|
||||
const uint32_t refreshRate = getLooptime();
|
||||
|
|
@ -338,7 +319,7 @@ bool pidInitFilters(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);
|
||||
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
|
||||
}
|
||||
|
|
@ -436,7 +417,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
|
|||
STATIC_FASTRAM uint16_t prevThrottle = 0;
|
||||
|
||||
// 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);
|
||||
if (filteredThrottle != prevThrottle) {
|
||||
prevThrottle = filteredThrottle;
|
||||
|
|
@ -451,7 +432,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
|
|||
}
|
||||
|
||||
#ifdef USE_ANTIGRAVITY
|
||||
if (!STATE(FIXED_WING)) {
|
||||
if (usedPidControllerType == PID_TYPE_PID) {
|
||||
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
|
||||
}
|
||||
#endif
|
||||
|
|
@ -468,12 +449,12 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
|
|||
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
|
||||
//TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (usedPidControllerType == PID_TYPE_PIFF) {
|
||||
// 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;
|
||||
|
|
@ -598,6 +579,20 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, flight_dynami
|
|||
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)
|
||||
{
|
||||
const float rateError = pidState->rateTarget - pidState->gyroRate;
|
||||
|
|
@ -607,11 +602,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
|
|||
// Calculate integral
|
||||
pidState->errorGyroIf += rateError * pidState->kI * dT;
|
||||
|
||||
if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) {
|
||||
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
|
||||
} else {
|
||||
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
|
||||
}
|
||||
applyItermLimiting(pidState);
|
||||
|
||||
if (pidProfile()->fixedWingItermThrowLimit != 0) {
|
||||
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
|
||||
|
|
@ -623,7 +614,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
|
|||
}
|
||||
#endif
|
||||
|
||||
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit);
|
||||
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidSumLimit, +pidSumLimit);
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
axisPID_P[axis] = newPTerm;
|
||||
|
|
@ -660,7 +651,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
|
|||
}
|
||||
}
|
||||
#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;
|
||||
|
||||
|
|
@ -681,22 +672,21 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
|
|||
return dBoost;
|
||||
}
|
||||
#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(axis);
|
||||
UNUSED(dT);
|
||||
return 1.0f;
|
||||
}
|
||||
#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 newPTerm = pTermProcess(pidState, axis, rateError, dT);
|
||||
|
||||
// Calculate new D-term
|
||||
float newDTerm;
|
||||
if (pidBank()->pid[axis].D == 0) {
|
||||
if (pidState->kD == 0) {
|
||||
// optimisation for when D8 is zero, often used by YAW axis
|
||||
newDTerm = 0;
|
||||
} else {
|
||||
|
|
@ -707,15 +697,13 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
|
|||
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
|
||||
|
||||
// Apply additional lowpass
|
||||
if (pidProfile()->dterm_lpf_hz) {
|
||||
deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
|
||||
}
|
||||
deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
|
||||
|
||||
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
|
||||
newDTerm = firFilterApply(&pidState->gyroRateFilter);
|
||||
|
||||
// Calculate derivative
|
||||
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis, dT);
|
||||
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
|
||||
|
||||
// Additionally constrain D
|
||||
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
|
||||
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
|
||||
const float newOutputLimited = constrainf(newOutput, -pidProfile()->pidSumLimit, +pidProfile()->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);
|
||||
const float newOutputLimited = constrainf(newOutput, -pidSumLimit, +pidSumLimit);
|
||||
|
||||
float itermErrorRate = rateError;
|
||||
applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
|
||||
|
||||
#ifdef USE_ANTIGRAVITY
|
||||
const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
|
||||
itermErrorRate *= iTermAntigravityGain;
|
||||
#endif
|
||||
|
||||
|
|
@ -741,11 +724,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
|
|||
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
|
||||
|
||||
// Don't grow I-term if motors are at their limit
|
||||
if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) {
|
||||
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
|
||||
} else {
|
||||
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
|
||||
}
|
||||
applyItermLimiting(pidState);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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) {
|
||||
return;
|
||||
|
|
@ -988,30 +980,29 @@ void FAST_CODE pidController(float dT)
|
|||
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
|
||||
}
|
||||
|
||||
// Apply setpoint rate of change limits
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
|
||||
}
|
||||
// Prevent strong Iterm accumulation during stick inputs
|
||||
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
|
||||
|
||||
#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++) {
|
||||
// Apply PID setpoint controller
|
||||
if (STATE(FIXED_WING)) {
|
||||
pidApplyFixedWingRateController(&pidState[axis], axis, dT);
|
||||
}
|
||||
else {
|
||||
pidApplyMulticopterRateController(&pidState[axis], axis, dT);
|
||||
}
|
||||
// Apply setpoint rate of change limits
|
||||
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
|
||||
|
||||
// Step 4: Run gyro-driven control
|
||||
checkItermLimitingActive(&pidState[axis]);
|
||||
pidControllerApplyFn(&pidState[axis], axis, dT);
|
||||
}
|
||||
}
|
||||
|
||||
pidType_e pidIndexGetType(pidIndex_e pidIndex)
|
||||
{
|
||||
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
|
||||
return usedPidControllerType;
|
||||
}
|
||||
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) {
|
||||
return PID_TYPE_NONE;
|
||||
}
|
||||
|
|
@ -1022,3 +1013,61 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
|
|||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,9 +72,10 @@ typedef enum {
|
|||
|
||||
// TODO(agh): PIDFF
|
||||
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_PIFF, // Uses P, I and FF, ignoring D
|
||||
PID_TYPE_AUTO, // Autodetect
|
||||
} pidType_e;
|
||||
|
||||
typedef struct pid8_s {
|
||||
|
|
@ -100,6 +101,7 @@ typedef enum {
|
|||
} itermRelaxType_e;
|
||||
|
||||
typedef struct pidProfile_s {
|
||||
uint8_t pidControllerType;
|
||||
pidBank_t bank_fw;
|
||||
pidBank_t bank_mc;
|
||||
|
||||
|
|
@ -155,8 +157,10 @@ typedef struct pidAutotuneConfig_s {
|
|||
PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
|
||||
PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
|
||||
|
||||
static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
|
||||
static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
|
||||
static uint8_t usedPidControllerType;
|
||||
|
||||
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 int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
|
||||
|
|
|
|||
|
|
@ -154,3 +154,5 @@
|
|||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
|
||||
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
|
||||
|
||||
#undef USE_PWM_SERVO_DRIVER
|
||||
Loading…
Add table
Add a link
Reference in a new issue