limit max angle to 50 by vector length

src/main/blackbox/blackbox.c

@@ -1705,6 +1705,8 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d",           autopilotConfig()->position_I);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d",           autopilotConfig()->position_D);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d",           autopilotConfig()->position_A);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d",      autopilotConfig()->position_cutoff);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d",         autopilotConfig()->max_angle);
 
 #ifdef USE_MAG
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);

src/main/cli/settings.c

@@ -1862,6 +1862,7 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_POSITION_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_D) },
     { PARAM_NAME_POSITION_A,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_A) },
     { PARAM_NAME_POSITION_CUTOFF,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 },    PG_AUTOPILOT, offsetof(autopilotConfig_t, position_cutoff) },
+    { PARAM_NAME_AP_MAX_ANGLE,        VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 70 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, max_angle) },
 
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)

src/main/fc/parameter_names.h

@@ -173,6 +173,7 @@
 #define PARAM_NAME_POSITION_D "autopilot_position_D"
 #define PARAM_NAME_POSITION_A "autopilot_position_A"
 #define PARAM_NAME_POSITION_CUTOFF "autopilot_position_cutoff"
+#define PARAM_NAME_AP_MAX_ANGLE "autopilot_max_angle"
 
 #define PARAM_NAME_ANGLE_FEEDFORWARD "angle_feedforward"
 #define PARAM_NAME_ANGLE_FF_SMOOTHING_MS "angle_feedforward_smoothing_ms"

src/main/flight/autopilot.c

@@ -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
+            }
         }
     }
 

src/main/pg/autopilot.c

@@ -44,4 +44,5 @@ PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
     .position_D = 30,
     .position_A = 30,
     .position_cutoff = 80,
+    .max_angle = 50,
 );

src/main/pg/autopilot.h

@@ -39,6 +39,7 @@ typedef struct autopilotConfig_s {
     uint8_t position_D;
     uint8_t position_A;
     uint8_t position_cutoff;
+    uint8_t max_angle;
 } autopilotConfig_t;
 
 PG_DECLARE(autopilotConfig_t, autopilotConfig);