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:
parent
e64c8e0da6
commit
cb401d1ed9
5 changed files with 21 additions and 5 deletions
|
@ -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) },
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue