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

AFCS code refactoring

Browse source
This commit is contained in:
demvlad 2025-04-11 20:23:10 +03:00
parent 125cb1f4cc
commit 5513c359a6
3 changed files with 15 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1404,16 +1404,16 @@ const clivalue_t valueTable[] = {
#endif
#ifdef USE_AIRPLANE_FCS
{ PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stick_gain) },
{ PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_gain) },
{ PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_PITCH]) },
{ 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_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_time) },
{ 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_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_stick_gain) },
{ PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_damping_gain) },
{ PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) },
{ PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_ROLL]) },
{ PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stick_gain) },
{ PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_gain) },
{ PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_YAW]) },
{ 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_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_time) },
{ PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) },
#endif

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

@ -22,11 +22,11 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
}
// Pitch channel
float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_pitch_stick_gain;
float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH];
float gyroPitch = gyro.gyroADCf[FD_PITCH];
float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch);
float gyroPitchHigh = gyroPitch - gyroPitchLow;
float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_pitch_damping_gain * 0.001f);
float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f);
float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f);
pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl;
pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
@ -37,8 +37,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl;
// Roll channel
float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_roll_stick_gain);
float rollDampingCtrl = -1.0f * gyro.gyroADCf[FD_ROLL] * (pidProfile->afcs_roll_damping_gain * 0.001f);
float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]);
float rollDampingCtrl = -1.0f * gyro.gyroADCf[FD_ROLL] * (pidProfile->afcs_damping_gain[FD_ROLL] * 0.001f);
pidData[FD_ROLL].Sum = rollPilotCtrl + rollDampingCtrl;
pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
@ -47,11 +47,11 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_ROLL].D = 10.0f * rollDampingCtrl;
// Yaw channel
float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_yaw_stick_gain);
float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]);
float gyroYaw = gyro.gyroADCf[FD_YAW];
float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw);
float gyroYawHigh = gyroYaw - gyroYawLow;
float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_yaw_damping_gain * 0.001f);
float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f);
float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f);
pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl;
pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;

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

@ -271,14 +271,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
.chirp_frequency_end_deci_hz = 6000,
.chirp_time_seconds = 20,
#ifdef USE_AIRPLANE_FCS
.afcs_pitch_stick_gain = 100,
.afcs_pitch_damping_gain = 20,
.afcs_stick_gain = { 100, 100, 100 },
.afcs_damping_gain = { 20, 25, 500 },
.afcs_pitch_damping_filter_time = 100,
.afcs_pitch_stability_gain = 0,
.afcs_roll_stick_gain = 100,
.afcs_roll_damping_gain = 25,
.afcs_yaw_stick_gain = 100,
.afcs_yaw_damping_gain = 500,
.afcs_yaw_damping_filter_time = 3000,
.afcs_yaw_stability_gain = 0,
#endif