Added roll to yaw cross link control to improve roll rotation on high angle of attack

src/main/cli/settings.c

@@ -1425,6 +1425,9 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8 | PROFILE_VALUE, .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) },
     { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) },
+    { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_start) },
+    { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_stop) },
+    { PARAM_NAME_AFCS_ROLL_TO_YAW_LINK, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_to_yaw_link) },
 #endif
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY

src/main/fc/parameter_names.h

@@ -296,4 +296,7 @@
 #define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit"
 #define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain"
 #define PARAM_NAME_AFCS_SERVO_TIME "afcs_servo_time"
+#define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START "afcs_roll_yaw_clift_start"
+#define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP "afcs_roll_yaw_clift_stop"
+#define PARAM_NAME_AFCS_ROLL_TO_YAW_LINK "afcs_roll_to_yaw_link"
 #endif

src/main/flight/airplane_fcs.c

@@ -83,7 +83,7 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float
         float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f);
         servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
         pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT;
-        
+
         // limit integrator output
         float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition;
         if ( output > 100.0f) {
@@ -130,6 +130,21 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif
     return isLimitAoA;
 }
 
+// Roll to yaw control cross link to improve roll rotation at high angle of attack
+static float rollToYawCrossLinkControl(const pidProfile_t *pidProfile, float rollPilotControl, float liftCoef)
+{
+    float crossYawControl = 0.0f,
+          roll_yaw_clift_start = 0.1f * pidProfile->afcs_roll_yaw_clift_start,
+          roll_yaw_clift_stop = 0.1f * pidProfile->afcs_roll_yaw_clift_stop;
+
+    if (pidProfile->afcs_roll_to_yaw_link && liftCoef > roll_yaw_clift_start) {
+        float k = (liftCoef - roll_yaw_clift_start) / (roll_yaw_clift_stop - roll_yaw_clift_start);
+        k = constrainf(k, 0.0f, 1.0f);
+        crossYawControl = k * rollPilotControl * (pidProfile->afcs_roll_to_yaw_link * 0.1f);
+    }
+
+    return crossYawControl;
+}
 
 void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
 {
@@ -158,9 +173,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
 
     // Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack
     float liftCoef;
-    bool isValidLiftC = computeLiftCoefficient(pidProfile, accelZ, &liftCoef);
+    bool isValidLiftCoef = computeLiftCoefficient(pidProfile, accelZ, &liftCoef);
     bool isLimitAoA = false;
-    if (isValidLiftC) {
+    if (isValidLiftCoef) {
         isLimitAoA = updateAngleOfAttackLimiter(pidProfile, liftCoef);
     }
     if (isLimitAoA == false) {
@@ -196,13 +211,18 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
     }
     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;
+    float rollToYawCrossControl = 0.0f;
+    if (isValidLiftCoef) {
+        rollToYawCrossControl = rollToYawCrossLinkControl(pidProfile, rollPilotCtrl, liftCoef);
+    }
+    pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl + rollToYawCrossControl;
     pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
 
     // Save control components instead of PID to get logging without additional variables
     pidData[FD_YAW].F = 10.0f * yawPilotCtrl;
     pidData[FD_YAW].D = 10.0f * yawDampingCtrl;
     pidData[FD_YAW].P = 10.0f * yawStabilityCtrl;
+    pidData[FD_YAW].S = 10.0f * rollToYawCrossControl;
 
     DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl * 10.0f));
     DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f));

src/main/flight/pid.c

@@ -282,9 +282,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .afcs_pitch_accel_min = 60,            // maximal negative Z accel value *10
         .afcs_wing_load = 560,                 // wing load (mass / WingArea) g/decimeter^2 * 10. 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_lift_c_limit = 15,               // Limit aerodinamics lift force coefficient value *10
         .afcs_aoa_limiter_gain = 250,          // elevator speed for 0.1 lift force coef difference in %/sec *10
         .afcs_servo_time = 90,                 // minimal time of servo movement from neutrale to maximum, ms
+        .afcs_roll_yaw_clift_start = 8,        // Aerodynamics lift force coef to start yaw control for roll rotation  *10
+        .afcs_roll_yaw_clift_stop = 15,        // Aerodynamics lift force coef to maximum yaw control for roll rotation  *10
+        .afcs_roll_to_yaw_link = 0,            // The maximal yaw control value to support roll rotation, % *10
 #endif
     );
 }

src/main/flight/pid.h

@@ -348,6 +348,9 @@ typedef struct pidProfile_s {
     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
     uint16_t afcs_servo_time;                   // minimal time of servo movement from neutrale to maximum, ms
+    uint8_t afcs_roll_yaw_clift_start;          // Aerodynamics lift force coef to start yaw control for roll rotation  *10
+    uint8_t afcs_roll_yaw_clift_stop;           // Aerodynamics lift force coef to maximum yaw control for roll rotation  *10
+    uint8_t afcs_roll_to_yaw_link;               // The maximal yaw control value to support roll rotation, % *10
 #endif
 } pidProfile_t;