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

TPA mode PDS + Wing setpoint attenuation (for wings) (#14010)

Browse source 
* TPA mode PDS + Wing setpoint attenuation

* removed brackets by haslinghuis :)

* Ledvinap's review

---------

Co-authored-by: limonspb <gendalf44@yandex.ru>
This commit is contained in:
Ivan Efimov 2024-11-15 19:07:13 -06:00 • committed by GitHub
parent ed6a4a4769
commit 1c1c78f80f
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
5 changed files with 114 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -118,6 +118,12 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
#ifdef USE_ACC
#define IS_AXIS_IN_ANGLE_MODE(i) (pidRuntime.axisInAngleMode[(i)])
#else
#define IS_AXIS_IN_ANGLE_MODE(i) false
#endif // USE_ACC
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 11);
void resetPidProfile(pidProfile_t *pidProfile)
@ -256,6 +262,21 @@ void resetPidProfile(pidProfile_t *pidProfile)
);
}
static bool isTpaActive(tpaMode_e tpaMode, term_e term) {
switch (tpaMode) {
case TPA_MODE_PD:
return term == TERM_P || term == TERM_D;
case TPA_MODE_D:
return term == TERM_D;
#ifdef USE_WING
case TPA_MODE_PDS:
return term == TERM_P || term == TERM_D || term == TERM_S;
#endif
default:
return false;
}
}
void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
{
for (int i = 0; i < PID_PROFILE_COUNT; i++) {
@ -343,8 +364,50 @@ static float calcWingTpaArgument(void)
return tpaArgument;
}
static void updateStermTpaFactor(int axis, float tpaFactor)
{
float tpaFactorSterm = tpaFactor;
if (pidRuntime.tpaCurveType == TPA_CURVE_HYPERBOLIC) {
const float maxSterm = tpaFactorSterm * (float)currentPidProfile->pid[axis].S * S_TERM_SCALE;
if (maxSterm > 1.0f) {
tpaFactorSterm *= 1.0f / maxSterm;
}
}
pidRuntime.tpaFactorSterm[axis] = tpaFactorSterm;
}
static void updateStermTpaFactors(void) {
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
float tpaFactor = pidRuntime.tpaFactor;
if (i == FD_YAW && currentPidProfile->yaw_type == YAW_TYPE_DIFF_THRUST) {
tpaFactor = pidRuntime.tpaFactorYaw;
}
updateStermTpaFactor(i, tpaFactor);
}
}
#endif // USE_WING
static float wingAdjustSetpoint(float currentPidSetpoint, int axis)
{
#ifdef USE_WING
float adjustedSetpoint = currentPidSetpoint;
if (!IS_AXIS_IN_ANGLE_MODE(axis)) {
const bool skipYaw = axis == FD_YAW && currentPidProfile->yaw_type == YAW_TYPE_DIFF_THRUST;
if (pidRuntime.tpaFactorSterm[axis] > 0.0f && pidRuntime.tpaFactor > 0.0f && !skipYaw) {
adjustedSetpoint = currentPidSetpoint * pidRuntime.tpaFactorSterm[axis] / pidRuntime.tpaFactor;
}
}
DEBUG_SET(DEBUG_WING_SETPOINT, 2 * axis, lrintf(currentPidSetpoint));
DEBUG_SET(DEBUG_WING_SETPOINT, 2 * axis + 1, lrintf(adjustedSetpoint));
return adjustedSetpoint;
#else
UNUSED(axis);
return currentPidSetpoint;
#endif // USE_WING
}
float getTpaFactorClassic(float tpaArgument)
{
static bool isTpaLowFaded = false;
@ -399,6 +462,7 @@ void pidUpdateTpaFactor(float throttle)
pidRuntime.tpaFactorYaw = pidRuntime.tpaFactor;
break;
}
updateStermTpaFactors();
#endif // USE_WING
}
@ -930,18 +994,48 @@ static FAST_CODE_NOINLINE float applyLaunchControl(int axis, const rollAndPitchT
}
#endif
static float getSterm(int axis, const pidProfile_t *pidProfile)
static float getTpaFactor(const pidProfile_t *pidProfile, int axis, term_e term)
{
float tpaFactor = pidRuntime.tpaFactor;
#ifdef USE_WING
if (axis == FD_YAW) {
tpaFactor = pidRuntime.tpaFactorYaw;
}
#else
UNUSED(axis);
#endif
const bool tpaActive = isTpaActive(pidProfile->tpa_mode, term);
switch (term) {
case TERM_P:
return tpaActive ? tpaFactor : 1.0f;
case TERM_D:
return tpaFactor;
#ifdef USE_WING
case TERM_S:
return tpaActive ? pidRuntime.tpaFactorSterm[axis] : 1.0f;
#endif
default:
return 1.0f;
}
}
static float getSterm(int axis, const pidProfile_t *pidProfile, float setpoint)
{
#ifdef USE_WING
const float sTerm = getSetpointRate(axis) / getMaxRcRate(axis) * 1000.0f *
(float)pidProfile->pid[axis].S / 100.0f;
float sTerm = setpoint / getMaxRcRate(axis) * 1000.0f *
(float)pidProfile->pid[axis].S * S_TERM_SCALE;
DEBUG_SET(DEBUG_S_TERM, axis, lrintf(sTerm));
DEBUG_SET(DEBUG_S_TERM, 2 * axis, lrintf(sTerm));
sTerm *= getTpaFactor(pidProfile, axis, TERM_S);
DEBUG_SET(DEBUG_S_TERM, 2 * axis + 1, lrintf(sTerm));
return sTerm;
#else
UNUSED(axis);
UNUSED(pidProfile);
UNUSED(setpoint);
return 0.0f;
#endif
}
@ -993,28 +1087,6 @@ NOINLINE static void applySpa(int axis, const pidProfile_t *pidProfile)
#endif // USE_WING
}
static float getTpaFactor(const pidProfile_t *pidProfile, int axis, term_e term)
{
float tpaFactor = pidRuntime.tpaFactor;
#ifdef USE_WING
if (axis == FD_YAW) {
tpaFactor = pidRuntime.tpaFactorYaw;
}
#else
UNUSED(axis);
#endif
switch (term) {
case TERM_P:
return (pidProfile->tpa_mode == TPA_MODE_PD) ? tpaFactor : 1.0f;
case TERM_D:
return tpaFactor;
default:
return 1.0f;
}
}
// 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)
@ -1145,6 +1217,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
#endif
const float currentPidSetpointBeforeWingAdjust = currentPidSetpoint;
currentPidSetpoint = wingAdjustSetpoint(currentPidSetpoint, axis);
#ifdef USE_ACRO_TRAINER
if ((axis != FD_YAW) && pidRuntime.acroTrainerActive && !pidRuntime.inCrashRecoveryMode && !launchControlActive) {
currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
@ -1367,7 +1442,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
}
pidData[axis].S = getSterm(axis, pidProfile);
pidData[axis].S = getSterm(axis, pidProfile, currentPidSetpointBeforeWingAdjust);
applySpa(axis, pidProfile);
// calculating the PID sum