Angular acceleration limit on PID rate setpoint made possible (#427)

* Angular acceleration limit on PID rate setpoint

Author: digitalentity

src/main/common/filter.c

@@ -102,25 +102,28 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
     return filter->state;
 }
 
-// f_cut = cutoff frequency
-// rate_limit = maximum rate of change of the output value in units per second
-float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT)
+void pt1FilterReset(pt1Filter_t *filter, float input)
 {
-    // Pre calculate and store RC
-    if (!filter->RC) {
-        filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
-    }
-
-    const float newState = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-    const float rateLimitPerSample = rate_limit * dT;
-    filter->state = constrainf(newState, filter->state - rateLimitPerSample, filter->state + rateLimitPerSample);
+    filter->state = input;
+}
 
-    return filter->state;
+// rate_limit = maximum rate of change of the output value in units per second
+void rateLimitFilterInit(rateLimitFilter_t *filter)
+{
+    filter->state = 0;
 }
 
-void pt1FilterReset(pt1Filter_t *filter, float input)
+float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT)
 {
-    filter->state = input;
+    if (rate_limit > 0) {
+        const float rateLimitPerSample = rate_limit * dT;
+        filter->state = constrainf(input, filter->state - rateLimitPerSample, filter->state + rateLimitPerSample);
+    }
+    else {
+        filter->state = input;
+    }
+
+    return filter->state;
 }
 
 // FIR filter

src/main/common/filter.h

@@ -17,6 +17,10 @@
 
 #pragma once
 
+typedef struct rateLimitFilter_s {
+    float state;
+} rateLimitFilter_t;
+
 typedef struct pt1Filter_s {
     float state;
     float RC;
@@ -38,9 +42,11 @@ typedef struct firFilter_s {
 
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
-float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT);
 void pt1FilterReset(pt1Filter_t *filter, float input);
 
+void rateLimitFilterInit(rateLimitFilter_t *filter);
+float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
+
 void biquadFilterInit(biquadFilter_t *filter, uint8_t filterCutFreq, int16_t samplingRate);
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 

src/main/config/config.c

@@ -170,7 +170,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 120;
+static const uint8_t EEPROM_CONF_VERSION = 121;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
 {
@@ -222,6 +222,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->rollPitchItermIgnoreRate = 200;     // dps
     pidProfile->yawItermIgnoreRate = 50;            // dps
 
+    pidProfile->axisAccelerationLimitYaw = 10000;       // dps/s
+    pidProfile->axisAccelerationLimitRollPitch = 0;     // dps/s
+
     pidProfile->yaw_p_limit = YAW_P_LIMIT_DEFAULT;
     pidProfile->mag_hold_rate_limit = MAG_HOLD_RATE_LIMIT_DEFAULT;
 

src/main/flight/pid.c

@@ -74,6 +74,7 @@ typedef struct {
     pt1Filter_t angleFilterState;
 
     // Rate filtering
+    rateLimitFilter_t axisAccelFilter;
     pt1Filter_t ptermLpfState;
     pt1Filter_t deltaLpfState;
 } pidState_t;
@@ -278,6 +279,16 @@ static void pidLevel(const pidProfile_t *pidProfile, const controlRateConfig_t *
     }
 }
 
+/* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
+static void pidApplySetpointRateLimiting(const pidProfile_t *pidProfile, pidState_t *pidState, flight_dynamics_index_t axis)
+{
+    const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile->axisAccelerationLimitYaw : pidProfile->axisAccelerationLimitRollPitch;
+
+    if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) {
+        pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT);
+    }
+}
+
 static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *pidState, flight_dynamics_index_t axis)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
@@ -499,6 +510,11 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
         pidTurnAssistant(pidState);
     }
 
+    // Apply setpoint rate of change limits
+    for (int axis = 0; axis < 3; axis++) {
+        pidApplySetpointRateLimiting(pidProfile, &pidState[axis], axis);
+    }
+
     // Step 4: Run gyro-driven control
     for (int axis = 0; axis < 3; axis++) {
         // Apply PID setpoint controller

src/main/flight/pid.h

@@ -33,6 +33,8 @@
 #define MAG_HOLD_RATE_LIMIT_MAX 250
 #define MAG_HOLD_RATE_LIMIT_DEFAULT 90
 
+#define AXIS_ACCEL_MIN_LIMIT    50
+
 typedef enum {
     PIDROLL,
     PIDPITCH,
@@ -63,6 +65,9 @@ typedef struct pidProfile_s {
     uint16_t rollPitchItermIgnoreRate;      // Experimental threshold for ignoring iterm for pitch and roll on certain rates
     uint16_t yawItermIgnoreRate;            // Experimental threshold for ignoring iterm for yaw on certain rates
 
+    uint32_t axisAccelerationLimitYaw;          // Max rate of change of yaw angular rate setpoint (deg/s^2 = dps/s)
+    uint32_t axisAccelerationLimitRollPitch;    // Max rate of change of roll/pitch angular rate setpoint (deg/s^2 = dps/s)
+
     int16_t max_angle_inclination[ANGLE_INDEX_COUNT];       // Max possible inclination (roll and pitch axis separately
 
     uint8_t mag_hold_rate_limit;            //Maximum rotation rate MAG_HOLD mode can feed to yaw rate PID controller

src/main/io/serial_cli.c

@@ -811,6 +811,9 @@ const clivalue_t valueTable[] = {
     { "iterm_ignore_threshold",     VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } },
     { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } },
 
+    { "rate_accel_limit_roll_pitch",VAR_UINT32 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.axisAccelerationLimitRollPitch, .config.minmax = {0, 500000 } },
+    { "rate_accel_limit_yaw",       VAR_UINT32 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.axisAccelerationLimitYaw, .config.minmax = {0, 500000 } },
+
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, .config.minmax = { 1,  32 }, 0 },
     { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, .config.minmax = { 1,  32 }, 0 },