diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 152913f189..0fce8bb347 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -237,7 +237,7 @@ static CMS_Menu cmsx_menuRateProfile = { .entries = cmsx_menuRateProfileEntries }; -static uint8_t cmsx_dtermSetpointWeight; +static uint16_t cmsx_dtermSetpointWeight; static uint8_t cmsx_setpointRelaxRatio; static uint8_t cmsx_angleStrength; static uint8_t cmsx_horizonStrength; @@ -285,7 +285,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) static OSD_Entry cmsx_menuProfileOtherEntries[] = { { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, - { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, + { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1, 10 }, 0 }, { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 8018f44577..261c4eb235 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -401,7 +401,7 @@ static int applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t a blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; case ADJUSTMENT_D_SETPOINT: - newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in cli.c + newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 2000); // FIXME magic numbers repeated in cli.c pidProfile->dtermSetpointWeight = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); break; @@ -550,7 +550,7 @@ static int applyAbsoluteAdjustment(controlRateConfig_t *controlRateConfig, adjus blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); break; case ADJUSTMENT_D_SETPOINT: - newValue = constrain(value, 0, 254); // FIXME magic numbers repeated in cli.c + newValue = constrain(value, 0, 2000); // FIXME magic numbers repeated in cli.c pidProfile->dtermSetpointWeight = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue); break; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 325d9e4149..10859b5dcc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -317,10 +317,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; } - dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f; - if (dtermSetpointWeight > 2.0f) { - dtermSetpointWeight = 10 * (dtermSetpointWeight - 2.0f) + 2.0f; - } + dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; if (pidProfile->setpointRelaxRatio == 0) { relaxFactor = 0; } else { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 37bba45d76..3f4cd89bfc 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -98,7 +98,7 @@ typedef struct pidProfile_s { uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint8_t setpointRelaxRatio; // Setpoint weight relaxation effect - uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) + uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 53021236f5..f9a7fc2bed 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1224,8 +1224,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio); - sbufWriteU8(dst, currentPidProfile->dtermSetpointWeight); - sbufWriteU8(dst, 0); // reserved + sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); +// sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU16(dst, currentPidProfile->rateAccelLimit); @@ -1680,8 +1680,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU8(src); // reserved currentPidProfile->vbatPidCompensation = sbufReadU8(src); currentPidProfile->setpointRelaxRatio = sbufReadU8(src); - currentPidProfile->dtermSetpointWeight = sbufReadU8(src); - sbufReadU8(src); // reserved + currentPidProfile->dtermSetpointWeight = sbufReadU16(src); // not sure, if this is OK. Low bit is written 1st, so it should be compatible +// sbufReadU8(src); // reserved sbufReadU8(src); // reserved sbufReadU8(src); // reserved currentPidProfile->rateAccelLimit = sbufReadU16(src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index a872320b73..2b8d53e25e 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -737,7 +737,7 @@ const clivalue_t valueTable[] = { { "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) }, + { "dterm_setpoint_weight", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "crash_dthreshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_dthreshold) },