diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a821942500..12d10ca462 100644 --- a/src/main/cli/settings.c +++ b/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 diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index de4a9eec21..dbed54f5b4 100644 --- a/src/main/fc/parameter_names.h +++ b/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 diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index efd7449c9c..b4f7f5824f 100644 --- a/src/main/flight/airplane_fcs.c +++ b/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)); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b208c22d4b..73b667ffa7 100644 --- a/src/main/flight/pid.c +++ b/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 ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index febda0ad07..4c921d9df0 100644 --- a/src/main/flight/pid.h +++ b/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;