diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 34d963ee21..8a8a509bd6 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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 diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 122a358dbd..8badac577c 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 21eb66423c..fa28ecea2e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 @@ -1549,7 +1545,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } #ifdef USE_WING - // When PASSTHRU_MODE is active - reset all PIDs to zero so the aircraft won't snap out of control + // When PASSTHRU_MODE is active - reset all PIDs to zero so the aircraft won't snap out of control // because of accumulated PIDs once PASSTHRU_MODE gets disabled. bool isFixedWingAndPassthru = isFixedWing() && FLIGHT_MODE(PASSTHRU_MODE); #endif // USE_WING