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

Yaw type rudder/diff_thrust for TPA calculations (for wings) (#13929)

Browse source 
* Yaw type rudder or diff_thrust for TPA calculations (for wings)

* Added yaw_type to blackbox header

* KarateBrot review

* KarateBrot break

Co-authored-by: Jan Post <Rm2k-Freak@web.de>

---------

Co-authored-by: Jan Post <Rm2k-Freak@web.de>
This commit is contained in:
Ivan Efimov 2024-11-04 15:03:31 -06:00 • committed by GitHub
parent b819eceb86
commit a659189bf0
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
6 changed files with 65 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -251,6 +251,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.tpa_speed_adv_thrust = 2000,
.tpa_speed_max_voltage = 2520,
.tpa_speed_pitch_offset = 0,
.yaw_type = YAW_TYPE_RUDDER,
);
}
@ -363,7 +364,6 @@ float getTpaFactorClassic(float tpaArgument)
void pidUpdateTpaFactor(float throttle)
{
// don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point
throttle = constrainf(throttle, 0.0f, 1.0f);
float tpaFactor;
@ -388,6 +388,18 @@ void pidUpdateTpaFactor(float throttle)
DEBUG_SET(DEBUG_TPA, 0, lrintf(tpaFactor * 1000));
pidRuntime.tpaFactor = tpaFactor;
#ifdef USE_WING
switch (currentPidProfile->yaw_type) {
case YAW_TYPE_DIFF_THRUST:
pidRuntime.tpaFactorYaw = getTpaFactorClassic(tpaArgument);
break;
case YAW_TYPE_RUDDER:
default:
pidRuntime.tpaFactorYaw = pidRuntime.tpaFactor;
break;
}
#endif // USE_WING
}
void pidUpdateAntiGravityThrottleFilter(float throttle)
@ -975,6 +987,28 @@ 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)
@ -984,8 +1018,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
calculateSpaValues(pidProfile);
const float tpaFactorKp = (pidProfile->tpa_mode == TPA_MODE_PD) ? pidRuntime.tpaFactor : 1.0f;
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected();
#endif
@ -1160,7 +1192,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// -----calculate P component
pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * getTpaFactor(pidProfile, axis, TERM_P);
if (axis == FD_YAW) {
pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *) &pidRuntime.ptermYawLowpass, pidData[axis].P);
}
@ -1259,7 +1291,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
preTpaD *= dMaxMultiplier;
#endif
pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
pidData[axis].D = preTpaD * getTpaFactor(pidProfile, axis, TERM_D);
// Log the value of D pre application of TPA
if (axis != FD_YAW) {