1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00

Added lift coef forcast in AoA limiter

This commit is contained in:
demvlad 2025-04-24 09:42:13 +03:00
parent e64c8e0da6
commit cb401d1ed9
5 changed files with 21 additions and 5 deletions

View file

@ -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) },

View file

@ -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"

View file

@ -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);

View file

@ -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

View file

@ -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;