diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8a8a509bd6..a54275cafd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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 diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d8d7df625..0d38f52afc 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -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 \ No newline at end of file diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 8badac577c..5bea01578f 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -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 \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fa28ecea2e..babcb4265a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index feffb72f16..e82c1df0e3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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;