Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Added servo velocity limit

Browse source
This commit is contained in:
demvlad 2025-04-17 14:43:39 +03:00
parent a0fc8fa6dd
commit ff5cf0673d
6 changed files with 26 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1424,6 +1424,7 @@ const clivalue_t valueTable[] = {
{ 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) },
{ PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) },
#endif
// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY

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

@ -295,4 +295,5 @@
#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_SERVO_TIME "afcs_servo_time"
#endif

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

@ -16,25 +16,32 @@ void afcsInit(const pidProfile_t *pidProfile)
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;
float liftC = 0.0f;
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 = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal AoA
liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal values during plane launch
} else {
liftC = limitLiftC;
}
return liftC;
}
//The astatic accel Z controller by stick position
static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float pitchPilotCtrl, float accelZ)
{
if (pidProfile->afcs_pitch_accel_i_gain != 0) {
const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s
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.afcsElevatorAddition += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT;
float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f);
servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT;
float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition;
if ( output > 100.0f) {
pidRuntime.afcsElevatorAddition = 100.0f - pidData[FD_PITCH].Sum;
@ -56,17 +63,23 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float acc
float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ);
float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit;
float liftCoefDiff;
const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s
float servoVelocity;
if (liftCoef > 0.5f) {
liftCoefDiff = limitLiftC - liftCoef;
if (liftCoefDiff < 0.0f) {
isLimitAoA = true;
pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f);
servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT;
}
} else if (liftCoef < -0.5f) {
liftCoefDiff = -limitLiftC - liftCoef;
if (liftCoefDiff > 0.0f) {
isLimitAoA = true;
pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT;
servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f);
servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT;
}
}
DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoef * 100.0f));
@ -76,9 +89,8 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float acc
}
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
{
UNUSED(currentTimeUs);
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
pidData[axis].P = 0;
pidData[axis].I = 0;

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

@ -1,4 +1,4 @@
#pragma once
#include "pid.h"
void afcsInit(const pidProfile_t *pidProfile);
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs);
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile);

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

@ -284,6 +284,7 @@ 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 = 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
.afcs_servo_time = 90, // minimal time of servo movement from neutrale to maximum, ms
#endif
);
}
@ -1261,7 +1262,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#ifdef USE_AIRPLANE_FCS
bool isAFCS = isFixedWing() && FLIGHT_MODE(AIRPLANE_FCS_MODE);
if (isAFCS) {
afcsUpdate(pidProfile, currentTimeUs);
afcsUpdate(pidProfile);
return; // The airplanes FCS do not need PID controller
}
#endif

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

@ -347,6 +347,7 @@ 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
uint16_t afcs_servo_time; // minimal time of servo movement from neutrale to maximum, ms
#endif
} pidProfile_t;