Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Added checking angle of attack limiter valid work conditions

Browse source
This commit is contained in:
demvlad 2025-04-18 08:54:05 +03:00
parent ff5cf0673d
commit 048ccc0045
4 changed files with 65 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/cli/settings.c
View file

@ -1420,11 +1420,11 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) },
{ PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) },
{ PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) },
{ PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 0, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) },
{ PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) },
{ PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 1500 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) },
{ 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_SERVO_TIME, VAR_UINT16, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) },
{ PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) },
#endif
// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY

58
src/main/flight/airplane_fcs.c
View file

@ -1,6 +1,7 @@
#ifdef USE_AIRPLANE_FCS
#include "fc/rc.h"
#include "fc/runtime_config.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "io/gps.h"
@ -21,7 +22,7 @@ static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed,
const float speedThreshold = 2.5f; //gps speed thresold
if (speed > speedThreshold) {
const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f;
liftC = accelZ * (0.001f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure;
liftC = accelZ * (0.01f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure;
liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal values during plane launch
} else {
liftC = limitLiftC;
@ -49,22 +50,41 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float
pidRuntime.afcsElevatorAddition = -100.0f - pidData[FD_PITCH].Sum;
}
DEBUG_SET(DEBUG_AFCS, 1, lrintf(accelReq * 10.0f));
DEBUG_SET(DEBUG_AFCS, 2, lrintf(accelDelta * 10.0f));
DEBUG_SET(DEBUG_AFCS, 4, lrintf(accelReq * 10.0f));
DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelDelta * 10.0f));
}
}
// The angle of attack limiter. The aerodynamics lift force coefficient depends by angle of attack. Therefore it possible to use this coef instead of AoA value.
static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float accelZ)
{
static bool isLimiterEnabled = false;
static float validLiftCoefTime = 0.0f;
const float timeForValid = 3.0f;
bool isLimitAoA = false;
if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) {
float speed = 0.01f * gpsSol.speed3d;
float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ);
float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit;
float liftCoefDiff;
if (pidProfile->afcs_aoa_limiter_gain != 0
&& ARMING_FLAG(ARMED)
&& gpsSol.numSat > 5) {
float speed = 0.01f * gpsSol.speed3d,
liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ),
limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit;
// Enable AoA limiter after 2s flight with good lift coef. It prevents issues during plane launch
if (isLimiterEnabled == false) {
if (liftCoef < limitLiftC && liftCoef > -limitLiftC) {
validLiftCoefTime += pidRuntime.dT;
if (validLiftCoefTime > timeForValid) {
isLimiterEnabled = true;
}
}
}
if (isLimiterEnabled) {
const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s
float servoVelocity;
float liftCoefDiff = 0.0f,
servoVelocity = 0.0f;
if (liftCoef > 0.5f) {
liftCoefDiff = limitLiftC - liftCoef;
if (liftCoefDiff < 0.0f) {
@ -82,8 +102,13 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float acc
pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT;
}
}
DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoef * 100.0f));
DEBUG_SET(DEBUG_AFCS, 4, lrintf(liftCoefDiff * 100.0f));
DEBUG_SET(DEBUG_AFCS, 7, lrintf(liftCoefDiff * 100.0f));
}
DEBUG_SET(DEBUG_AFCS, 6, lrintf(liftCoef * 100.0f));
} else if (isLimiterEnabled) {
isLimiterEnabled = false;
validLiftCoefTime = 0.0f;
}
return isLimitAoA;
}
@ -114,19 +139,21 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl;
pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f);
// Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack
bool isLimitAoA = updateAngleOfAttackLimiter(pidProfile, accelZ);
if (isLimitAoA == false) {
updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ);
}
pidData[FD_PITCH].Sum += pidRuntime.afcsElevatorAddition;
DEBUG_SET(DEBUG_AFCS, 0, lrintf(pidRuntime.afcsElevatorAddition * 10.0f));
pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f;
// Save control components instead of PID to get logging without additional variables
// Do not use I, because it needs init to zero after switch to other flight modes from current AFCS mode
pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl;
pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl;
pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl;
pidData[FD_PITCH].S = 10.0f * pidRuntime.afcsElevatorAddition;
// Roll channel
float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]);
@ -155,8 +182,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
pidData[FD_YAW].D = 10.0f * yawDampingCtrl;
pidData[FD_YAW].P = 10.0f * yawStabilityCtrl;
DEBUG_SET(DEBUG_AFCS, 5, lrintf(pitchPilotCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 6, lrintf(pitchDampingCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 7, lrintf(pitchStabilityCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl * 10.0f));
DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsElevatorAddition * 10.0f));
}
#endif

2
src/main/flight/pid.c
View file

@ -280,7 +280,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.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
.afcs_pitch_accel_min = 60, // maximal negative Z accel value *10
.afcs_wing_load = 5600, // wing load (mass / WingArea) g/decimeter^2 * 100. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think
.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_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10

2
src/main/flight/pid.h
View file

@ -343,7 +343,7 @@ typedef struct pidProfile_s {
uint8_t afcs_pitch_accel_min; // maximal negative Z accel value *10
uint16_t afcs_yaw_damping_filter_freq; // yaw damping filter cut freq Hz *100
uint16_t afcs_yaw_stability_gain; // percent control by 1g Y accel change *100
uint16_t afcs_wing_load; // wing load (mass / WingArea) g/decimeter^2 * 100.
uint16_t afcs_wing_load; // wing load (mass / WingArea) g/decimeter^2 * 10.
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