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

limit max angle to 50 by vector length

Browse source
This commit is contained in:
ctzsnooze 2024-11-06 13:56:26 +11:00
parent add64ebfe1
commit 5d8b4112f8
6 changed files with 16 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -74,6 +74,7 @@ typedef struct {
float lpfCutoff;
float pt1Gain;
bool sticksActive;
float maxAngle;
float pidSumCraft[2];
pt3Filter_t upsample[2];
earthFrame_t efAxis[2];
@ -120,6 +121,7 @@ void initializeEfAxisFilters(earthFrame_t *efAxis, float filterGain) {
void autopilotInit(const autopilotConfig_t *config)
{
posHold.sticksActive = false;
posHold.maxAngle = autopilotConfig()->max_angle;
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
@ -321,6 +323,14 @@ bool positionControl(void)
const float cosHeading = cos_approx(headingRads);
posHold.pidSumCraft[AI_ROLL] = -sinHeading * posHold.efAxis[NS].pidSum + cosHeading * posHold.efAxis[EW].pidSum;
posHold.pidSumCraft[AI_PITCH] = cosHeading * posHold.efAxis[NS].pidSum + sinHeading * posHold.efAxis[EW].pidSum;
// limit angle vector to maxAngle
const float angleMagnitude = sqrtf(posHold.pidSumCraft[AI_ROLL] * posHold.pidSumCraft[AI_ROLL] + posHold.pidSumCraft[AI_PITCH] * posHold.pidSumCraft[AI_PITCH]);
if (angleMagnitude > posHold.maxAngle && angleMagnitude > 0.0f) {
const float limiter = posHold.maxAngle / angleMagnitude;
posHold.pidSumCraft[AI_ROLL] *= limiter; // Scale the roll value
posHold.pidSumCraft[AI_PITCH] *= limiter; // Scale the pitch value
}
}
}