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

AoA limiter Lift coef forcast algorithm is improved

Browse source
This commit is contained in:
demvlad 2025-04-24 11:04:45 +03:00
parent 84431502b4
commit 5f9409e5be
1 changed files with 8 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/flight/airplane_fcs.c
View file

@ -106,7 +106,7 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif
float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef); float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef);
float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT; float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT;
liftCoefLast = liftCoefF; liftCoefLast = liftCoefF;
liftCoefF += liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f); float liftCoefForcastChange = liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f);
if (pidProfile->afcs_aoa_limiter_gain != 0) { if (pidProfile->afcs_aoa_limiter_gain != 0) {
const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit;
@ -116,6 +116,9 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif
servoVelocity = 0.0f; servoVelocity = 0.0f;
if (liftCoefF > 0.0f) { if (liftCoefF > 0.0f) {
liftCoefDiff = liftCoefF - limitLiftC; liftCoefDiff = liftCoefF - limitLiftC;
if (liftCoefForcastChange > 0.0f) {
liftCoefDiff += liftCoefForcastChange;
}
if (liftCoefDiff > 0.0f) { if (liftCoefDiff > 0.0f) {
isLimitAoA = true; isLimitAoA = true;
servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f);
@ -124,6 +127,9 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif
} }
} else { } else {
liftCoefDiff = liftCoefF + limitLiftC; liftCoefDiff = liftCoefF + limitLiftC;
if (liftCoefForcastChange < 0.0f) {
liftCoefDiff += liftCoefForcastChange;
}
if (liftCoefDiff < 0.0f) { if (liftCoefDiff < 0.0f) {
isLimitAoA = true; isLimitAoA = true;
servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f);
@ -170,7 +176,7 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
} }
// Pitch channel // Pitch channel
pidData[FD_PITCH].I /= 10.0f; //restore % last value pidData[FD_PITCH].I /= 10.0f; //restore % last value
float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH]; float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH];
float gyroPitch = gyro.gyroADCf[FD_PITCH]; float gyroPitch = gyro.gyroADCf[FD_PITCH];
if (pidProfile->afcs_pitch_damping_filter_freq != 0) { if (pidProfile->afcs_pitch_damping_filter_freq != 0) {