Added angle of attack limiter by using of aerodynamics lift force coefficient

src/main/cli/settings.c

@@ -1408,7 +1408,7 @@ 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_I_GAIN,         VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, 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) },
 
@@ -1419,6 +1419,11 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_AFCS_YAW_DAMPING_GAIN,         VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) },
     { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ,  VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) },
     { PARAM_NAME_AFCS_YAW_STABILITY_GAIN,       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) },
+
+    { PARAM_NAME_AFCS_WING_LOAD,   VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) },
+    { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 0, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) },
+    { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) },
+    { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) },
 #endif
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY

src/main/fc/parameter_names.h

@@ -291,4 +291,8 @@
 #define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain"
 #define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ "afcs_yaw_damping_filter_freq"
 #define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain"
+#define PARAM_NAME_AFCS_WING_LOAD "afcs_wing_load"
+#define PARAM_NAME_AFCS_AIR_DENSITY "afcs_air_density"
+#define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit"
+#define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain"
 #endif

src/main/flight/airplane_fcs.c

@@ -3,6 +3,7 @@
 #include "fc/rc.h"
 #include "sensors/acceleration.h"
 #include "sensors/gyro.h"
+#include "io/gps.h"
 #include <math.h>
 #include "build/debug.h"
 
@@ -10,7 +11,21 @@ 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;
+    pidRuntime.afcsPitchControlErrorSum = 0.0f;
+}
+
+static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ)
+{
+    float liftC = 0.0f;
+    const float speedThreshold = 2.0f;    //gps speed thresold
+    const float limitLiftC = 2.0f;
+    if (speed > speedThreshold) {
+        const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f;
+        liftC = accelZ * (0.001f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure;
+        liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal AoA
+    }
+
+    return liftC;
 }
 
 void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@@ -38,25 +53,49 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
 
     pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl;
     pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f);
-    if (pidProfile->afcs_pitch_accel_i_gain != 0) {
+
+    bool isLimitAoA = false;
+    if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) {
+        float speed = 0.01f * gpsSol.speed3d;
+        float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ);
+        float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit;
+        float delta;
+        if (liftCoef > 0.5f) {
+            delta = limitLiftC - liftCoef;
+            if (delta < 0.0f) {
+                isLimitAoA = true;
+                pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
+            }
+        } else if (liftCoef < -0.5f) {
+            delta = -limitLiftC - liftCoef;
+            if (delta > 0.0f) {
+                isLimitAoA = true;
+                pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
+            }
+        }
+    }
+
+    if (isLimitAoA == false && 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;
+        pidRuntime.afcsPitchControlErrorSum += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT;
-        float output = pidData[FD_PITCH].Sum + pidRuntime.afcsAccelError;
+        float output = pidData[FD_PITCH].Sum + pidRuntime.afcsPitchControlErrorSum;
         if ( output > 100.0f) {
-            pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum;
+            pidRuntime.afcsPitchControlErrorSum = 100.0f - pidData[FD_PITCH].Sum;
         } else if (output < -100.0f) {
-            pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum;
+            pidRuntime.afcsPitchControlErrorSum = -100.0f - pidData[FD_PITCH].Sum;
         }
-        pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError;
+
-        DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError));
+        DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsPitchControlErrorSum));
         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 += pidRuntime.afcsPitchControlErrorSum;
+    
     pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f;
 
     // Save control components instead of PID to get logging without additional variables

src/main/flight/pid.c

@@ -275,11 +275,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .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_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 by 1g accel change *100
+        .afcs_yaw_stability_gain = 0,          // percent control by 1g Y accel change *100
-        .afcs_pitch_accel_i_gain = 0,          // elevator speed for 1g accel difference in %/sec *10
+        .afcs_pitch_accel_i_gain = 250,        // elevator speed for 1g Z accel difference in %/sec *10
-        .afcs_pitch_accel_max = 80,            // maximal positive z accel value *10
+        .afcs_pitch_accel_max = 80,            // maximal positive Z accel value *10
-        .afcs_pitch_accel_min = 60,            // maximal negative z accel value *10
+        .afcs_pitch_accel_min = 60,            // maximal negative Z accel value *10
+        .afcs_wing_load = 5600,                // wing load (mass / WingArea) g/decimeter^2 * 100. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think
+        .afcs_air_density = 1225,              // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data
+        .afcs_lift_c_limit = 12,               // Limit aerodinamics lift force coefficient value *10
+        .afcs_aoa_limiter_gain = 250,          // elevator speed for 0.1 lift force coef difference in %/sec *10
 #endif
     );
 }

src/main/flight/pid.h

@@ -334,15 +334,19 @@ typedef struct pidProfile_s {
     uint8_t chirp_time_seconds;             // excitation time
 
 #ifdef USE_AIRPLANE_FCS
-    uint8_t afcs_stick_gain[XYZ_AXIS_COUNT];
+    uint8_t afcs_stick_gain[XYZ_AXIS_COUNT];    // Percent control output
-    uint16_t afcs_damping_gain[XYZ_AXIS_COUNT];
+    uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; // percent control range addition by 1 degree per second angle rate * 1000
-    uint16_t afcs_pitch_damping_filter_freq;
+    uint16_t afcs_pitch_damping_filter_freq;    // pitch damping filter cut freq Hz * 100
-    uint16_t afcs_pitch_stability_gain;
+    uint16_t afcs_pitch_stability_gain;         // percent control range addition by 1g accel z change *100
-    uint8_t afcs_pitch_accel_i_gain;
+    uint16_t afcs_pitch_accel_i_gain;            // elevator speed for 1g Z accel difference in %/sec *10
-    uint8_t afcs_pitch_accel_max;
+    uint8_t afcs_pitch_accel_max;               // maximal positive Z accel value *10
-    uint8_t afcs_pitch_accel_min;
+    uint8_t afcs_pitch_accel_min;               // maximal negative Z accel value *10
-    uint16_t afcs_yaw_damping_filter_freq;
+    uint16_t afcs_yaw_damping_filter_freq;      // yaw damping filter cut freq Hz *100
-    uint16_t afcs_yaw_stability_gain;
+    uint16_t afcs_yaw_stability_gain;           // percent control by 1g Y accel change *100
+    uint16_t afcs_wing_load;                    // wing load (mass / WingArea) g/decimeter^2 * 100. 
+    uint16_t afcs_air_density;                  // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data
+    uint8_t afcs_lift_c_limit;                  // Limit aerodinamics lift force coefficient value *10
+    uint16_t afcs_aoa_limiter_gain;             // elevator speed for 0.1 lift force coef difference in %/sec *10
 #endif
 } pidProfile_t;
 
@@ -566,7 +570,7 @@ typedef struct pidRuntime_s {
 #ifdef USE_AIRPLANE_FCS
     pt1Filter_t afcsPitchDampingLowpass;
     pt1Filter_t afcsYawDampingLowpass;
-    float afcsAccelError;
+    float afcsPitchControlErrorSum;
 #endif
 } pidRuntime_t;