Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Setpoint PID attenuation (for wings) (#13719)

Browse source 
* Setpoint PID attenuation

* Suggestions from ledvinap's review
This commit is contained in:
Ivan Efimov 2024-06-25 16:38:43 -05:00 • committed by GitHub
parent 8eec0fe8d5
commit 2ceb8e0417
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
11 changed files with 137 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -231,6 +231,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
.ez_landing_limit = 15,
.ez_landing_speed = 50,
.tpa_delay_ms = 0,
.spa_center = { 0, 0, 0 },
.spa_width = { 0, 0, 0 },
.spa_mode = { 0, 0, 0 },
);
#ifndef USE_D_MIN
@ -812,6 +815,47 @@ static float getSterm(int axis, const pidProfile_t *pidProfile)
#endif
}
NOINLINE static void calculateSpaValues(const pidProfile_t *pidProfile)
{
#ifdef USE_WING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float currentRate = getSetpointRate(axis);
pidRuntime.spa[axis] = 1.0f - smoothStepUpTransition(
fabsf(currentRate), pidProfile->spa_center[axis], pidProfile->spa_width[axis]);
DEBUG_SET(DEBUG_SPA, axis, lrintf(pidRuntime.spa[axis] * 1000));
}
#else
UNUSED(pidProfile);
#endif // #ifdef USE_WING ... #else
}
NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile)
{
#ifdef USE_WING
switch(pidProfile->spa_mode[axis]){
case SPA_MODE_PID:
pidData[axis].P *= pidRuntime.spa[axis];
pidData[axis].D *= pidRuntime.spa[axis];
pidData[axis].I *= pidRuntime.spa[axis];
break;
case SPA_MODE_I:
pidData[axis].I *= pidRuntime.spa[axis];
break;
case SPA_MODE_PD_I_FREEZE:
pidData[axis].P *= pidRuntime.spa[axis];
pidData[axis].D *= pidRuntime.spa[axis];
break;
case SPA_MODE_I_FREEZE:
case SPA_MODE_OFF:
default:
break;
}
#else
UNUSED(axis);
UNUSED(pidProfile);
#endif // #ifdef USE_WING ... #else
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@ -819,6 +863,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
static float previousGyroRateDterm[XYZ_AXIS_COUNT];
static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];
calculateSpaValues(pidProfile);
#ifdef USE_TPA_MODE
const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
#else
@ -1015,7 +1061,14 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
pidRuntime.itermAccelerator = 0.0f; // no antigravity on yaw iTerm
}
}
const float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
#ifdef USE_WING
if (pidProfile->spa_mode[axis] != SPA_MODE_OFF) {
// slowing down I-term change, or even making it zero if setpoint is high enough
iTermChange *= pidRuntime.spa[axis];
}
#endif // #ifdef USE_WING
pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate D component
@ -1151,6 +1204,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
pidData[axis].S = getSterm(axis, pidProfile);
applySpa(axis, pidProfile);
// calculating the PID sum
const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F + pidData[axis].S;