diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a54275cafd..00231b6b0d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1408,6 +1408,9 @@ 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_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) }, { 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]) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 0d38f52afc..22aa90f72b 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -282,6 +282,9 @@ #define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain" #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_PITCH_ACCEL_I_GAIN "afcs_pitch_accel_i_gain" +#define PARAM_NAME_AFCS_PITCH_ACCEL_MAX "afcs_pitch_accel_max" +#define PARAM_NAME_AFCS_PITCH_ACCEL_MIN "afcs_pitch_accel_min" #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" diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 9674200c15..49542c2ef7 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -10,6 +10,7 @@ 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; } void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) @@ -27,12 +28,36 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs // Pitch channel 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_damping_gain[FD_PITCH] * 0.001f); - float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f); + if (pidProfile->afcs_pitch_damping_filter_freq != 0) { + float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch); + gyroPitch -= gyroPitchLow; // Damping the pitch gyro high freq part only + } + float pitchDampingCtrl = -1.0f * gyroPitch * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f); + float accelZ = acc.accADC.z * acc.dev.acc_1G_rec; + float pitchStabilityCtrl = (accelZ - 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; + pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); + if (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; + if ( output > 100.0f) { + pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum; + } else if (output < -100.0f) { + pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum; + } + pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError; + DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError)); + 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 = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; @@ -52,10 +77,12 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs // Yaw channel 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_damping_gain[FD_YAW] * 0.001f); - float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f); + if (pidProfile->afcs_yaw_damping_filter_freq != 0) { + float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw); + gyroYaw -= gyroYawLow; // Damping the yaw gyro high freq part only + } + float yawDampingCtrl = gyroYaw * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); + float yawStabilityCtrl = acc.accADC.y * acc.dev.acc_1G_rec * (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; @@ -63,5 +90,10 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_YAW].F = 10.0f * yawPilotCtrl; pidData[FD_YAW].D = 10.0f * yawDampingCtrl; pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; + + DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl)); + DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl)); + DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl)); + } #endif \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index babcb4265a..0b54797ab8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -271,12 +271,15 @@ 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 }, // 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 + .afcs_stick_gain = { 100, 100, 100 }, // Percent control output + .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 #endif ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e82c1df0e3..f7d5d6d37d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -338,6 +338,9 @@ typedef struct pidProfile_s { 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; #endif @@ -563,6 +566,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; + float afcsAccelError; #endif } pidRuntime_t;