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 filters settings are changed from delay time to cut freq

Browse source
This commit is contained in:
demvlad 2025-04-12 15:09:32 +03:00
parent 5513c359a6
commit 5af1941ad8
5 changed files with 29 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1404,18 +1404,18 @@ 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_stick_gain[FD_PITCH]) },
{ PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT8 | 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_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_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_STICK_GAIN, VAR_UINT8 | 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_stick_gain[FD_YAW]) },
{ PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT8 | 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) },
{ 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) },
#endif
// PG_TELEMETRY_CONFIG
#ifdef USE_TELEMETRY

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

@ -280,12 +280,12 @@
#ifdef USE_AIRPLANE_FCS
#define PARAM_NAME_AFCS_PITCH_STICK_GAIN "afcs_pitch_stick_gain"
#define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain"
#define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME "afcs_pitch_damping_filter_time"
#define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ "afcs_pitch_damping_filter_freq"
#define PARAM_NAME_AFCS_PITCH_STABILITY_GAIN "afcs_pitch_stability_gain"
#define PARAM_NAME_AFCS_ROLL_STICK_GAIN "afcs_roll_stick_gain"
#define PARAM_NAME_AFCS_ROLL_DAMPING_GAIN "afcs_roll_damping_gain"
#define PARAM_NAME_AFCS_YAW_STICK_GAIN "afcs_yaw_stick_gain"
#define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain"
#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME "afcs_yaw_damping_filter_time"
#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ "afcs_yaw_damping_filter_freq"
#define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain"
#endif

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

@ -5,8 +5,8 @@
void afcsInit(const pidProfile_t *pidProfile)
{
pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_pitch_damping_filter_time * 0.001f, pidRuntime.dT));
pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_yaw_damping_filter_time * 0.001f, pidRuntime.dT));
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));
}
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
@ -32,9 +32,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
// Save control components instead of PID to get logging without additional variables
pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl;
pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl;
pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl;
pidData[FD_PITCH].F = 5.0f * (pitchPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_PITCH]);
pidData[FD_PITCH].D = 5.0f * pitchDampingCtrl;
pidData[FD_PITCH].P = 5.0f * pitchStabilityCtrl;
// Roll channel
float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]);
@ -43,8 +43,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
// Save control components instead of PID to get logging without additional variables
pidData[FD_ROLL].F = 10.0f * rollPilotCtrl;
pidData[FD_ROLL].D = 10.0f * rollDampingCtrl;
pidData[FD_ROLL].F = 5.0f * (rollPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_ROLL]);
pidData[FD_ROLL].D = 5.0f * rollDampingCtrl;
// Yaw channel
float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]);
@ -57,8 +57,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs
pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f;
// Save control components instead of PID to get logging without additional variables
pidData[FD_YAW].F = 10.0f * yawPilotCtrl;
pidData[FD_YAW].D = 10.0f * yawDampingCtrl;
pidData[FD_YAW].P = 10.0f * yawStabilityCtrl;
pidData[FD_YAW].F = 5.0f * (yawPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_YAW]);
pidData[FD_YAW].D = 5.0f * yawDampingCtrl;
pidData[FD_YAW].P = 5.0f * yawStabilityCtrl;
}
#endif

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

@ -271,12 +271,12 @@ void resetPidProfile(pidProfile_t *pidProfile)
.chirp_frequency_end_deci_hz = 6000,
.chirp_time_seconds = 20,
#ifdef USE_AIRPLANE_FCS
.afcs_stick_gain = { 100, 100, 100 },
.afcs_damping_gain = { 20, 25, 500 },
.afcs_pitch_damping_filter_time = 100,
.afcs_pitch_stability_gain = 0,
.afcs_yaw_damping_filter_time = 3000,
.afcs_yaw_stability_gain = 0,
.afcs_stick_gain = { 100, 100, 100 }, // Percent control output with full stick
.afcs_damping_gain = { 20, 30, 100 }, // 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 range by 1g accel y change *100
#endif
);
}

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

@ -334,15 +334,11 @@ typedef struct pidProfile_s {
uint8_t chirp_time_seconds; // excitation time
#ifdef USE_AIRPLANE_FCS
uint16_t afcs_pitch_stick_gain;
uint16_t afcs_pitch_damping_gain;
uint16_t afcs_pitch_damping_filter_time;
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;
uint16_t afcs_roll_stick_gain;
uint16_t afcs_roll_damping_gain;
uint16_t afcs_yaw_stick_gain;
uint16_t afcs_yaw_damping_gain;
uint16_t afcs_yaw_damping_filter_time;
uint16_t afcs_yaw_damping_filter_freq;
uint16_t afcs_yaw_stability_gain;
#endif
} pidProfile_t;