From e4846f2ecc4a0df2e25522287e86d84556a43ead Mon Sep 17 00:00:00 2001 From: supiiik Date: Tue, 22 May 2018 19:59:47 +0200 Subject: [PATCH 1/6] New calculation for DTERM setpoint weight I'm flying race with betaflight and on all my race quads I have setpoint on maximum value 2.54. But I feel, that it's not enough. This calculation of setpoint is almost same as old method up to number 2.0 (previously 2.54) but numbers above 2.0 have more aggresive impact on Dterm RC stick commands. With this calculation i found, that value 2.3 is fine for me. 2.3 is equivalent to 6,35 with old calculation method (which was not possible of course, because there is 8bit limit). With higher values have quad much sharper responses and feel more "locked in". I think, that most freestyle pilots have setpoint at values around 1, so there is almost no change and race pilots using higher values and for those, who are limited by the value 2.54 (like me and my friends) should be solution new setpoint calculation method. This is my first pull request to betaflight, so I hope, that I did everything well according your rules --- src/main/flight/pid.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 10859b5dcc..55e3d99cc1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -317,7 +317,10 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D; } - dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; + dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f; + if (dtermSetpointWeight > 2.0f) { + dtermSetpointWeight = 10 * (dtermSetpointWeight - 2.0f) + 2.0f; + } if (pidProfile->setpointRelaxRatio == 0) { relaxFactor = 0; } else { From 6c15904a772030f352a1e374d0e36a4eb8a98fd0 Mon Sep 17 00:00:00 2001 From: supiiik Date: Tue, 22 May 2018 20:01:25 +0200 Subject: [PATCH 2/6] Update pid.c --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 55e3d99cc1..325d9e4149 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -318,7 +318,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) } dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f; - if (dtermSetpointWeight > 2.0f) { + if (dtermSetpointWeight > 2.0f) { dtermSetpointWeight = 10 * (dtermSetpointWeight - 2.0f) + 2.0f; } if (pidProfile->setpointRelaxRatio == 0) { From 76e3e7aded92cd432493d4ad044563519324b22d Mon Sep 17 00:00:00 2001 From: supiiik Date: Wed, 23 May 2018 08:39:45 +0200 Subject: [PATCH 3/6] dterm_setpoint_weight to uint16_t dterm_setpoint_weight changet to uint16 with limit 0-2000 --- src/main/cms/cms_menu_imu.c | 4 ++-- src/main/fc/rc_adjustments.c | 4 ++-- src/main/flight/pid.c | 5 +---- src/main/flight/pid.h | 2 +- src/main/interface/msp.c | 8 ++++---- src/main/interface/settings.c | 2 +- 6 files changed, 11 insertions(+), 14 deletions(-) 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) }, From 4312513a77c27e8bd4eddf9b07bdd5d9b7108b70 Mon Sep 17 00:00:00 2001 From: supiiik Date: Wed, 23 May 2018 13:33:43 +0200 Subject: [PATCH 4/6] MSP changed Changed MSP, all new data are at the end --- src/main/cms/cms_menu_imu.c | 2 +- src/main/flight/pid.c | 2 +- src/main/interface/msp.c | 12 ++++++++---- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 0fce8bb347..aacc0f91b1 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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, 2000, 1, 10 }, 0 }, + { "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_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/flight/pid.c b/src/main/flight/pid.c index 10859b5dcc..3f057b6cff 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -88,7 +88,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, ); #endif -PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3); void resetPidProfile(pidProfile_t *pidProfile) { diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index f9a7fc2bed..ec4877653e 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); - sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); -// sbufWriteU8(dst, 0); // reserved + sbufWriteU8(dst, currentPidProfile->dtermSetpointWeight); + sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU16(dst, currentPidProfile->rateAccelLimit); @@ -1234,6 +1234,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); + sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); break; case MSP_SENSOR_CONFIG: @@ -1680,8 +1681,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) sbufReadU8(src); // reserved currentPidProfile->vbatPidCompensation = sbufReadU8(src); currentPidProfile->setpointRelaxRatio = sbufReadU8(src); - currentPidProfile->dtermSetpointWeight = sbufReadU16(src); // not sure, if this is OK. Low bit is written 1st, so it should be compatible -// sbufReadU8(src); // reserved + currentPidProfile->dtermSetpointWeight = sbufReadU8(src); + sbufReadU8(src); // reserved sbufReadU8(src); // reserved sbufReadU8(src); // reserved currentPidProfile->rateAccelLimit = sbufReadU16(src); @@ -1694,6 +1695,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->itermThrottleThreshold = sbufReadU16(src); currentPidProfile->itermAcceleratorGain = sbufReadU16(src); } + if (sbufBytesRemaining(src) >= 2) { + currentPidProfile->dtermSetpointWeight = sbufReadU16(src); + } pidInitConfig(currentPidProfile); break; From 8a1c5e5e74112833a4dc88b52ad8ee8e49a80d63 Mon Sep 17 00:00:00 2001 From: supiiik Date: Wed, 23 May 2018 14:36:23 +0200 Subject: [PATCH 5/6] small fixes, MIN for 8bit MSP --- src/main/cms/cms_menu_imu.c | 2 +- src/main/interface/msp.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index aacc0f91b1..4e30f3e55d 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1, 10 }, 0 }, + { "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 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/interface/msp.c b/src/main/interface/msp.c index ec4877653e..4a4c354263 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1224,7 +1224,7 @@ 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, MIN(currentPidProfile->dtermSetpointWeight, 255)); sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, 0); // reserved From cc12dddedbc5ecf4407010bd99bfeea674474305 Mon Sep 17 00:00:00 2001 From: supiiik Date: Wed, 23 May 2018 15:55:07 +0200 Subject: [PATCH 6/6] Indentations repaired --- src/main/cms/cms_menu_imu.c | 2 +- src/main/interface/msp.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 4e30f3e55d..801a9d5d78 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 0 }, + { "D SETPT WT", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight, 0, 2000, 1 }, 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/interface/msp.c b/src/main/interface/msp.c index 4a4c354263..13fa3a2164 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1234,7 +1234,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold); sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain); - sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); + sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight); break; case MSP_SENSOR_CONFIG: @@ -1695,9 +1695,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->itermThrottleThreshold = sbufReadU16(src); currentPidProfile->itermAcceleratorGain = sbufReadU16(src); } - if (sbufBytesRemaining(src) >= 2) { - currentPidProfile->dtermSetpointWeight = sbufReadU16(src); - } + if (sbufBytesRemaining(src) >= 2) { + currentPidProfile->dtermSetpointWeight = sbufReadU16(src); + } pidInitConfig(currentPidProfile); break;