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

Added angle of attack limiter by using of aerodynamics lift force coefficient

Browse source
This commit is contained in:
demvlad 2025-04-17 13:28:52 +03:00
parent 72dfefc1f8
commit 01728927d8
5 changed files with 81 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1408,7 +1408,7 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) },
{ PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) },
{ PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) },
{ PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) },
{ PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) },
{ PARAM_NAME_AFCS_PITCH_ACCEL_MAX, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_max) },
{ PARAM_NAME_AFCS_PITCH_ACCEL_MIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_min) },
@ -1419,6 +1419,11 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) },
{ 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_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) },
#endif
// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY

4
src/main/fc/parameter_names.h
View file

@ -291,4 +291,8 @@
#define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain"
#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ "afcs_yaw_damping_filter_freq"
#define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain"
#define PARAM_NAME_AFCS_WING_LOAD "afcs_wing_load"
#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"
#endif

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

@ -3,6 +3,7 @@
#include "fc/rc.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "io/gps.h"
#include <math.h>
#include "build/debug.h"
@ -10,7 +11,21 @@ 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));
pidRuntime.afcsAccelError = 0.0f;
pidRuntime.afcsPitchControlErrorSum = 0.0f;
}
static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ)
{
float liftC = 0.0f;
const float speedThreshold = 2.0f; //gps speed thresold
const float limitLiftC = 2.0f;
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 = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal AoA
}
return liftC;
}
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@ -38,25 +53,49 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl;
pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f);
if (pidProfile->afcs_pitch_accel_i_gain != 0) {
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 delta;
if (liftCoef > 0.5f) {
delta = limitLiftC - liftCoef;
if (delta < 0.0f) {
isLimitAoA = true;
pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
}
} else if (liftCoef < -0.5f) {
delta = -limitLiftC - liftCoef;
if (delta > 0.0f) {
isLimitAoA = true;
pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
}
}
}
if (isLimitAoA == false && pidProfile->afcs_pitch_accel_i_gain != 0) {
float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f
: (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f;
float accelDelta = accelReq - accelZ;
pidRuntime.afcsAccelError += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT;
float output = pidData[FD_PITCH].Sum + pidRuntime.afcsAccelError;
pidRuntime.afcsPitchControlErrorSum += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT;
float output = pidData[FD_PITCH].Sum + pidRuntime.afcsPitchControlErrorSum;
if ( output > 100.0f) {
pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum;
pidRuntime.afcsPitchControlErrorSum = 100.0f - pidData[FD_PITCH].Sum;
} else if (output < -100.0f) {
pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum;
pidRuntime.afcsPitchControlErrorSum = -100.0f - pidData[FD_PITCH].Sum;
}
pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError;
DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError));
DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsPitchControlErrorSum));
DEBUG_SET(DEBUG_AFCS, 4, lrintf(pidData[FD_PITCH].Sum));
DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelReq * 10.0f));
DEBUG_SET(DEBUG_AFCS, 6, lrintf(accelZ * 10.0f));
DEBUG_SET(DEBUG_AFCS, 7, lrintf(accelDelta * 10.0f));
}
pidData[FD_PITCH].Sum += pidRuntime.afcsPitchControlErrorSum;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f;
// Save control components instead of PID to get logging without additional variables

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

@ -275,11 +275,15 @@ void resetPidProfile(pidProfile_t *pidProfile)
.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_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_stability_gain = 0, // percent control by 1g accel change *100
.afcs_pitch_accel_i_gain = 0, // elevator speed for 1g 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_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz *100
.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
.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_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
#endif
);
}

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

@ -334,15 +334,19 @@ typedef struct pidProfile_s {
uint8_t chirp_time_seconds; // excitation time
#ifdef USE_AIRPLANE_FCS
uint8_t afcs_stick_gain[XYZ_AXIS_COUNT];
uint16_t afcs_damping_gain[XYZ_AXIS_COUNT];
uint16_t afcs_pitch_damping_filter_freq;
uint16_t afcs_pitch_stability_gain;
uint8_t afcs_pitch_accel_i_gain;
uint8_t afcs_pitch_accel_max;
uint8_t afcs_pitch_accel_min;
uint16_t afcs_yaw_damping_filter_freq;
uint16_t afcs_yaw_stability_gain;
uint8_t afcs_stick_gain[XYZ_AXIS_COUNT]; // Percent control output
uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; // percent control range addition by 1 degree per second angle rate * 1000
uint16_t afcs_pitch_damping_filter_freq; // pitch damping filter cut freq Hz * 100
uint16_t afcs_pitch_stability_gain; // percent control range addition by 1g accel z change *100
uint16_t afcs_pitch_accel_i_gain; // elevator speed for 1g Z accel difference in %/sec *10
uint8_t afcs_pitch_accel_max; // maximal positive Z accel value *10
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_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
#endif
} pidProfile_t;
@ -566,7 +570,7 @@ typedef struct pidRuntime_s {
#ifdef USE_AIRPLANE_FCS
pt1Filter_t afcsPitchDampingLowpass;
pt1Filter_t afcsYawDampingLowpass;
float afcsAccelError;
float afcsPitchControlErrorSum;
#endif
} pidRuntime_t;