Added accel Z astatic regulator into pitch channel

src/main/cli/settings.c

@@ -1408,6 +1408,9 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN,         VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) },
     { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ,  VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) },
     { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN,       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) },
+    { PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN,         VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) },
+    { PARAM_NAME_AFCS_PITCH_ACCEL_MAX,            VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_max) },
+    { PARAM_NAME_AFCS_PITCH_ACCEL_MIN,            VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_min) },
 
     { PARAM_NAME_AFCS_ROLL_STICK_GAIN,   VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) },
     { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_ROLL]) },

src/main/fc/parameter_names.h

@@ -282,6 +282,9 @@
 #define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain"
 #define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ "afcs_pitch_damping_filter_freq"
 #define PARAM_NAME_AFCS_PITCH_STABILITY_GAIN "afcs_pitch_stability_gain"
+#define PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN "afcs_pitch_accel_i_gain"
+#define PARAM_NAME_AFCS_PITCH_ACCEL_MAX "afcs_pitch_accel_max"
+#define PARAM_NAME_AFCS_PITCH_ACCEL_MIN "afcs_pitch_accel_min"
 #define PARAM_NAME_AFCS_ROLL_STICK_GAIN "afcs_roll_stick_gain"
 #define PARAM_NAME_AFCS_ROLL_DAMPING_GAIN "afcs_roll_damping_gain"
 #define PARAM_NAME_AFCS_YAW_STICK_GAIN "afcs_yaw_stick_gain"

src/main/flight/airplane_fcs.c

@@ -10,6 +10,7 @@ void afcsInit(const pidProfile_t *pidProfile)
 {
     pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT));
     pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT));
+    pidRuntime.afcsAccelError = 0.0f;
 }
 
 void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@@ -27,12 +28,36 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
     // Pitch channel
     float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH];
     float gyroPitch = gyro.gyroADCf[FD_PITCH];
-    float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch);
+    if (pidProfile->afcs_pitch_damping_filter_freq != 0) {
-    float gyroPitchHigh = gyroPitch - gyroPitchLow;
+        float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch);
-    float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f);
+        gyroPitch -= gyroPitchLow;      // Damping the pitch gyro high freq part only
-    float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f);
+    }
+    float pitchDampingCtrl = -1.0f * gyroPitch * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f);
+    float accelZ = acc.accADC.z * acc.dev.acc_1G_rec;
+    float pitchStabilityCtrl = (accelZ - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f);
+
     pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl;
-    pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
+    pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f);
+    if (pidProfile->afcs_pitch_accel_i_gain != 0) {
+        float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f
+                                               : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f;
+        float accelDelta = accelReq - accelZ;
+        pidRuntime.afcsAccelError += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT;
+        float output = pidData[FD_PITCH].Sum + pidRuntime.afcsAccelError;
+        if ( output > 100.0f) {
+            pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum;
+        } else if (output < -100.0f) {
+            pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum;
+        }
+        pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError;
+        DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError));
+        DEBUG_SET(DEBUG_AFCS, 4, lrintf(pidData[FD_PITCH].Sum));
+        DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelReq * 10.0f));
+        DEBUG_SET(DEBUG_AFCS, 6, lrintf(accelZ * 10.0f));
+        DEBUG_SET(DEBUG_AFCS, 7, lrintf(accelDelta * 10.0f));
+    }
+
+    pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f;
 
     // Save control components instead of PID to get logging without additional variables
     pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl;
@@ -52,10 +77,12 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
     // Yaw channel
     float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]);
     float gyroYaw = gyro.gyroADCf[FD_YAW];
-    float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw);
+    if (pidProfile->afcs_yaw_damping_filter_freq != 0) {
-    float gyroYawHigh = gyroYaw - gyroYawLow;
+        float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw);
-    float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f);
+        gyroYaw -= gyroYawLow;      // Damping the yaw gyro high freq part only
-    float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f);
+    }
+    float yawDampingCtrl = gyroYaw * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f);
+    float yawStabilityCtrl = acc.accADC.y *  acc.dev.acc_1G_rec * (pidProfile->afcs_yaw_stability_gain * 0.01f);
     pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl;
     pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
 
@@ -63,5 +90,10 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
     pidData[FD_YAW].F = 10.0f * yawPilotCtrl;
     pidData[FD_YAW].D = 10.0f * yawDampingCtrl;
     pidData[FD_YAW].P = 10.0f * yawStabilityCtrl;
+
+    DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl));
+    DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl));
+    DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl));
+
 }
 #endif

src/main/flight/pid.c

@@ -271,12 +271,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .chirp_frequency_end_deci_hz = 6000,
         .chirp_time_seconds = 20,
 #ifdef USE_AIRPLANE_FCS
-        .afcs_stick_gain = { 100, 100, 100 },   // Percent control output with full stick
+        .afcs_stick_gain = { 100, 100, 100 },  // Percent control output
-        .afcs_damping_gain = { 20, 30, 100 },   // percent control range addition by 1 degree per second angle rate * 1000
+        .afcs_damping_gain = { 20, 30, 50 },   // percent control range addition by 1 degree per second angle rate * 1000
-        .afcs_pitch_damping_filter_freq = 160,  // pitch damping filter cut freq Hz * 100
+        .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100
-        .afcs_pitch_stability_gain = 0,         // percent control range addition by 1g accel z change *100
+        .afcs_pitch_stability_gain = 0,        // percent control range addition by 1g accel z change *100
-        .afcs_yaw_damping_filter_freq = 5,      // yaw damping filter cut freq Hz * 100
+        .afcs_yaw_damping_filter_freq = 5,     // yaw damping filter cut freq Hz * 100
-        .afcs_yaw_stability_gain = 0,            // percent control range by 1g accel y change *100
+        .afcs_yaw_stability_gain = 0,          // percent control by 1g accel change *100
+        .afcs_pitch_accel_i_gain = 0,          // elevator speed for 1g accel difference in %/sec *10
+        .afcs_pitch_accel_max = 80,            // maximal positive z accel value *10
+        .afcs_pitch_accel_min = 60,            // maximal negative z accel value *10
 #endif
     );
 }

src/main/flight/pid.h

@@ -338,6 +338,9 @@ typedef struct pidProfile_s {
     uint16_t afcs_damping_gain[XYZ_AXIS_COUNT];
     uint16_t afcs_pitch_damping_filter_freq;
     uint16_t afcs_pitch_stability_gain;
+    uint8_t afcs_pitch_accel_i_gain;
+    uint8_t afcs_pitch_accel_max;
+    uint8_t afcs_pitch_accel_min;
     uint16_t afcs_yaw_damping_filter_freq;
     uint16_t afcs_yaw_stability_gain;
 #endif
@@ -563,6 +566,7 @@ typedef struct pidRuntime_s {
 #ifdef USE_AIRPLANE_FCS
     pt1Filter_t afcsPitchDampingLowpass;
     pt1Filter_t afcsYawDampingLowpass;
+    float afcsAccelError;
 #endif
 } pidRuntime_t;