diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 3156e87514..006ae6521c 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1007,9 +1007,9 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_D_MIN - { "d_min_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_roll) }, - { "d_min_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_pitch) }, - { "d_min_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_yaw) }, + { "d_min_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min[FD_ROLL]) }, + { "d_min_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min[FD_PITCH]) }, + { "d_min_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min[FD_YAW]) }, { "d_min_boost_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_gain) }, { "d_min_advance", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, d_min_advance) }, #endif diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 0b5816953d..1e5fc4811c 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -328,6 +328,9 @@ static uint16_t cmsx_itermAcceleratorGain; static uint16_t cmsx_itermThrottleThreshold; static uint8_t cmsx_motorOutputLimit; static int8_t cmsx_autoProfileCellCount; +#ifdef USE_D_MIN +static uint8_t cmsx_d_min[XYZ_AXIS_COUNT]; +#endif static long cmsx_profileOtherOnEnter(void) { @@ -348,6 +351,12 @@ static long cmsx_profileOtherOnEnter(void) cmsx_motorOutputLimit = pidProfile->motor_output_limit; cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count; +#ifdef USE_D_MIN + for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) { + cmsx_d_min[i] = pidProfile->d_min[i]; + } +#endif + return 0; } @@ -370,6 +379,12 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) pidProfile->motor_output_limit = cmsx_motorOutputLimit; pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount; +#ifdef USE_D_MIN + for (unsigned i = 0; i < XYZ_AXIS_COUNT; i++) { + pidProfile->d_min[i] = cmsx_d_min[i]; + } +#endif + return 0; } @@ -392,6 +407,12 @@ static OSD_Entry cmsx_menuProfileOtherEntries[] = { { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1}, 0 }, +#ifdef USE_D_MIN + { "D_MIN_ROLL", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_ROLL], 0, 100, 1 }, 0 }, + { "D_MIN_PITCH", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_PITCH], 0, 100, 1 }, 0 }, + { "D_MIN_YAW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min[FD_YAW], 0, 100, 1 }, 0 }, +#endif + { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } }; @@ -560,11 +581,6 @@ static uint16_t cmsx_dterm_lowpass_hz; static uint16_t cmsx_dterm_lowpass2_hz; static uint16_t cmsx_dterm_notch_hz; static uint16_t cmsx_dterm_notch_cutoff; -#ifdef USE_D_MIN -static uint8_t cmsx_d_min_roll; -static uint8_t cmsx_d_min_pitch; -static uint8_t cmsx_d_min_yaw; -#endif static uint16_t cmsx_yaw_lowpass_hz; static long cmsx_FilterPerProfileRead(void) @@ -575,11 +591,6 @@ static long cmsx_FilterPerProfileRead(void) cmsx_dterm_lowpass2_hz = pidProfile->dterm_lowpass2_hz; cmsx_dterm_notch_hz = pidProfile->dterm_notch_hz; cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff; -#ifdef USE_D_MIN - cmsx_d_min_roll = pidProfile->d_min_roll; - cmsx_d_min_pitch = pidProfile->d_min_pitch; - cmsx_d_min_yaw = pidProfile->d_min_yaw; -#endif cmsx_yaw_lowpass_hz = pidProfile->yaw_lowpass_hz; return 0; @@ -595,11 +606,6 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) pidProfile->dterm_lowpass2_hz = cmsx_dterm_lowpass2_hz; pidProfile->dterm_notch_hz = cmsx_dterm_notch_hz; pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff; -#ifdef USE_D_MIN - pidProfile->d_min_roll = cmsx_d_min_roll; - pidProfile->d_min_pitch = cmsx_d_min_pitch; - pidProfile->d_min_yaw = cmsx_d_min_yaw; -#endif pidProfile->yaw_lowpass_hz = cmsx_yaw_lowpass_hz; return 0; @@ -613,11 +619,6 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz, 0, 500, 1 }, 0 }, { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, -#ifdef USE_D_MIN - { "D_MIN_ROLL", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_roll, 0, 100, 1 }, 0 }, - { "D_MIN_PITCH", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_pitch, 0, 100, 1 }, 0 }, - { "D_MIN_YAW", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_d_min_yaw, 0, 100, 1 }, 0 }, -#endif { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz, 0, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f9fa998405..4813d862d4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -191,9 +191,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .use_integrated_yaw = false, .integrated_yaw_relax = 200, .thrustLinearization = 0, - .d_min_roll = 20, - .d_min_pitch = 22, - .d_min_yaw = 0, + .d_min = { 20, 22, 0 }, // roll, pitch, yaw .d_min_gain = 20, .d_min_advance = 20, .motor_output_limit = 100, @@ -277,7 +275,6 @@ static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT]; #endif #if defined(USE_D_MIN) -static FAST_RAM_ZERO_INIT uint8_t dMin[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT biquadFilter_t dMinRange[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT pt1Filter_t dMinLowpass[XYZ_AXIS_COUNT]; #endif @@ -408,11 +405,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } #endif #if defined(USE_D_MIN) - dMin[FD_ROLL] = pidProfile->d_min_roll; - dMin[FD_PITCH] = pidProfile->d_min_pitch; - dMin[FD_YAW] = pidProfile->d_min_yaw; - // Initialize the filters for all axis even if the dMin[axis] value is 0 + // Initialize the filters for all axis even if the d_min[axis] value is 0 // Otherwise if the pidProfile->d_min_xxx parameters are ever added to // in-flight adjustments and transition from 0 to > 0 in flight the feature // won't work because the filter wasn't initialized. @@ -669,8 +663,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #if defined(USE_D_MIN) for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { - if ((dMin[axis] > 0) && (dMin[axis] < pidProfile->pid[axis].D)) { - dMinPercent[axis] = dMin[axis] / (float)(pidProfile->pid[axis].D); + const uint8_t dMin = pidProfile->d_min[axis]; + if ((dMin > 0) && (dMin < pidProfile->pid[axis].D)) { + dMinPercent[axis] = dMin / (float)(pidProfile->pid[axis].D); } else { dMinPercent[axis] = 0; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f0887ed983..b5d6677464 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -160,9 +160,7 @@ typedef struct pidProfile_s { uint8_t use_integrated_yaw; // Selects whether the yaw pidsum should integrated uint8_t integrated_yaw_relax; // Specifies how much integrated yaw should be reduced to offset the drag based yaw component uint8_t thrustLinearization; // Compensation factor for pid linearization - uint8_t d_min_roll; // Minimum D value on roll axis - uint8_t d_min_pitch; // Minimum D value on pitch axis - uint8_t d_min_yaw; // Minimum D value on yaw axis + uint8_t d_min[XYZ_AXIS_COUNT]; // Minimum D value on each axis uint8_t d_min_gain; // Gain factor for amount of gyro / setpoint activity required to boost D uint8_t d_min_advance; // Percentage multiplier for setpoint input to boost algorithm uint8_t motor_output_limit; // Upper limit of the motor output (percent)