diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index c246566c7c..85acc79eac 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -236,7 +236,9 @@ static uint8_t cmsx_simplified_roll_pitch_ratio; static uint8_t cmsx_simplified_i_gain; static uint8_t cmsx_simplified_pd_ratio; static uint8_t cmsx_simplified_pd_gain; +#ifdef USE_D_MIN static uint8_t cmsx_simplified_dmin_ratio; +#endif static uint8_t cmsx_simplified_feedforward_gain; static uint8_t cmsx_simplified_dterm_filter; @@ -256,7 +258,9 @@ static const void *cmsx_simplifiedTuningOnEnter(displayPort_t *pDisp) cmsx_simplified_i_gain = pidProfile->simplified_i_gain; cmsx_simplified_pd_ratio = pidProfile->simplified_pd_ratio; cmsx_simplified_pd_gain = pidProfile->simplified_pd_gain; +#ifdef USE_D_MIN cmsx_simplified_dmin_ratio = pidProfile->simplified_dmin_ratio; +#endif cmsx_simplified_feedforward_gain = pidProfile->simplified_feedforward_gain; cmsx_simplified_dterm_filter = pidProfile->simplified_dterm_filter; @@ -280,7 +284,9 @@ static const void *cmsx_simplifiedTuningOnExit(displayPort_t *pDisp, const OSD_E pidProfile->simplified_i_gain = cmsx_simplified_i_gain; pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio; pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain; +#ifdef USE_D_MIN pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio; +#endif pidProfile->simplified_feedforward_gain = cmsx_simplified_feedforward_gain; pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter; @@ -304,7 +310,9 @@ static const void *cmsx_applySimplifiedTuning(displayPort_t *pDisp, const void * pidProfile->simplified_i_gain = cmsx_simplified_i_gain; pidProfile->simplified_pd_ratio = cmsx_simplified_pd_ratio; pidProfile->simplified_pd_gain = cmsx_simplified_pd_gain; +#ifdef USE_D_MIN pidProfile->simplified_dmin_ratio = cmsx_simplified_dmin_ratio; +#endif pidProfile->simplified_feedforward_gain = cmsx_simplified_feedforward_gain; pidProfile->simplified_dterm_filter = cmsx_simplified_dterm_filter; @@ -326,7 +334,9 @@ static const OSD_Entry cmsx_menuSimplifiedTuningEntries[] = { "I GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_i_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "PD RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "PD GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_pd_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, +#ifdef USE_D_MIN { "DMIN RATIO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_dmin_ratio, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, +#endif { "FF GAIN", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_simplified_feedforward_gain, SIMPLIFIED_TUNING_MIN, SIMPLIFIED_TUNING_MAX, 5, 10 }, 0 }, { "-- SIMPLIFIED FILTER --", OME_Label, NULL, NULL, 0}, diff --git a/src/main/config/simplified_tuning.c b/src/main/config/simplified_tuning.c index 4f03964bbf..3ee3ed065d 100644 --- a/src/main/config/simplified_tuning.c +++ b/src/main/config/simplified_tuning.c @@ -37,7 +37,10 @@ static void calculateNewPidValues(pidProfile_t *pidProfile) [PID_PITCH] = PID_PITCH_DEFAULT, [PID_YAW] = PID_YAW_DEFAULT, }; + +#ifdef USE_D_MIN const int dMinDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = D_MIN_DEFAULT; +#endif const float masterMultiplier = pidProfile->simplified_master_multiplier / 100.0f; const float feedforwardGain = pidProfile->simplified_feedforward_gain / 100.0f; @@ -47,16 +50,19 @@ static void calculateNewPidValues(pidProfile_t *pidProfile) for (int axis = FD_ROLL; axis <= pidProfile->simplified_pids_mode; ++axis) { const float rpRatio = (axis == FD_PITCH) ? pidProfile->simplified_roll_pitch_ratio / 100.0f : 1.0f; - const float dminRatio = 1.0f + (((float)pidDefaults[axis].D / dMinDefaults[axis] - 1.0f) * (pidProfile->simplified_dmin_ratio / 100.0f - 1.0f)); - pidProfile->pid[axis].P = constrain(pidDefaults[axis].P * masterMultiplier * pdGain * pdRatio * rpRatio, 0, PID_GAIN_MAX); pidProfile->pid[axis].I = constrain(pidDefaults[axis].I * masterMultiplier * iGain * rpRatio, 0, PID_GAIN_MAX); pidProfile->pid[axis].D = constrain(pidDefaults[axis].D * masterMultiplier * pdGain * rpRatio, 0, PID_GAIN_MAX); + +#ifdef USE_D_MIN + const float dminRatio = 1.0f + (((float)pidDefaults[axis].D / dMinDefaults[axis] - 1.0f) * (pidProfile->simplified_dmin_ratio / 100.0f - 1.0f)); + if (pidProfile->simplified_dmin_ratio == SIMPLIFIED_TUNING_MAX) { pidProfile->d_min[axis] = 0; } else { pidProfile->d_min[axis] = constrain(dMinDefaults[axis] * masterMultiplier * pdGain * rpRatio * dminRatio, 0, D_MIN_GAIN_MAX); } +#endif pidProfile->pid[axis].F = constrain(pidDefaults[axis].F * masterMultiplier * feedforwardGain * rpRatio, 0, F_GAIN_MAX); } } diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 232684dbe8..65b8ffa245 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -2143,7 +2143,11 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ sbufWriteU8(dst, currentPidProfile->simplified_i_gain); sbufWriteU8(dst, currentPidProfile->simplified_pd_ratio); sbufWriteU8(dst, currentPidProfile->simplified_pd_gain); +#ifdef USE_D_MIN sbufWriteU8(dst, currentPidProfile->simplified_dmin_ratio); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU8(dst, currentPidProfile->simplified_feedforward_gain); sbufWriteU8(dst, currentPidProfile->simplified_dterm_filter); @@ -3116,7 +3120,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, currentPidProfile->simplified_i_gain = sbufReadU8(src); currentPidProfile->simplified_pd_ratio = sbufReadU8(src); currentPidProfile->simplified_pd_gain = sbufReadU8(src); +#ifdef USE_D_MIN currentPidProfile->simplified_dmin_ratio = sbufReadU8(src); +#else + sbufReadU8(src); +#endif currentPidProfile->simplified_feedforward_gain = sbufReadU8(src); currentPidProfile->simplified_dterm_filter = sbufReadU8(src);