diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cd4f8224e9..015d35a1dd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1424,6 +1424,8 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 1200, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, { 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_AOA_LIMITER_FILTER_FREQ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_filter_freq) }, + { PARAM_NAME_AFCS_AOA_LIMITER_FORCAST_TIME, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_forcast_time) }, { 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) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index dbed54f5b4..96234ce1cd 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -295,6 +295,8 @@ #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" +#define PARAM_NAME_AFCS_AOA_LIMITER_FILTER_FREQ "afcs_aoa_limiter_filter_freq" +#define PARAM_NAME_AFCS_AOA_LIMITER_FORCAST_TIME "afcs_aoa_limiter_forcast_time" #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" diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 245a797198..97f62d5ad7 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -34,6 +34,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)); + pt1FilterInit(&pidRuntime.afcsLiftCoefLowpass, pt1FilterGain(pidProfile->afcs_aoa_limiter_filter_freq * 0.1f, pidRuntime.dT)); pidRuntime.afcsElevatorAddition = 0.0f; } @@ -101,14 +102,20 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float liftCoef) { bool isLimitAoA = false; + static float liftCoefLast = 0.0f; + float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef); + float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT; + liftCoefLast = liftCoefF; + liftCoefF += liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f); + if (pidProfile->afcs_aoa_limiter_gain != 0) { const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s float liftCoefDiff = 0.0f, servoVelocity = 0.0f; - if (liftCoef > 0.0f) { - liftCoefDiff = liftCoef - limitLiftC; + if (liftCoefF > 0.0f) { + liftCoefDiff = liftCoefF - limitLiftC; if (liftCoefDiff > 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); @@ -116,7 +123,7 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } else { - liftCoefDiff = liftCoef + limitLiftC; + liftCoefDiff = liftCoefF + limitLiftC; if (liftCoefDiff < 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 73b667ffa7..fd4cc7b807 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -273,9 +273,9 @@ void resetPidProfile(pidProfile_t *pidProfile) #ifdef USE_AIRPLANE_FCS .afcs_stick_gain = { 100, 100, 100 }, // Percent control output .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 1.6Hz (Tf=0.1s) .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 0.05Hz (Tf=3s) .afcs_yaw_stability_gain = 0, // percent control by 1g Y accel change *100 .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 @@ -284,6 +284,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .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 = 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_aoa_limiter_filter_freq = 30, // aoa limiter lift coef filter cut freq 3Hz * 10 + .afcs_aoa_limiter_forcast_time = 10, // aoa limiter lift coef forcast time, 1s *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 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4c921d9df0..8c8a4f533e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -347,6 +347,8 @@ typedef struct pidProfile_s { 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 + uint8_t afcs_aoa_limiter_filter_freq; // aoa limiter lift coef filter cut freq Hz * 10 + uint8_t afcs_aoa_limiter_forcast_time; // aoa limiter lift coef forcast time, s *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 @@ -574,6 +576,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; + pt1Filter_t afcsLiftCoefLowpass; float afcsElevatorAddition; #endif } pidRuntime_t;