From 73b8a06dc573e7b43a88bdccf48605a246109f6d Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 27 Oct 2023 22:45:16 +0100 Subject: [PATCH 1/7] first cut --- docs/Settings.md | 10 ++++++++++ src/main/cms/cms_menu_imu.c | 1 + src/main/fc/settings.yaml | 12 +++++++++--- src/main/flight/pid.c | 3 ++- src/main/flight/pid.h | 1 + src/main/navigation/navigation_multicopter.c | 11 +++++++++-- 6 files changed, 32 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 17384f92732..876bcec63b9 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3572,6 +3572,16 @@ P gain of altitude PID controller (Multirotor) --- +### nav_mc_vel_xy_accel_attenuate + +Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed. + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 10 | 100 | + +--- + ### nav_mc_vel_xy_d D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 7612ec44181..46c655f4a58 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -437,6 +437,7 @@ static const OSD_Entry cmsx_menuMechanicsEntries[] = OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX), OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF), OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ), + OSD_SETTING_ENTRY("ACCEL ATTENUATION", SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1011638959d..06b02bab2e5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -192,7 +192,7 @@ tables: enum: gpsBaudRate_e - name: nav_mc_althold_throttle values: ["STICK", "MID_STICK", "HOVER"] - enum: navMcAltHoldThrottle_e + enum: navMcAltHoldThrottle_e - name: led_pin_pwm_mode values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] enum: led_pin_pwm_mode_e @@ -1186,7 +1186,7 @@ groups: min: 0 max: 200 - + - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t @@ -2040,6 +2040,12 @@ groups: min: 0 max: 255 default_value: 40 + - name: nav_mc_vel_xy_accel_attenuate + description: "Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed." + field: mc_vel_xy_accel_attenuate + default_value: 100 + min: 10 + max: 100 - name: nav_mc_heading_p description: "P gain of Heading Hold controller (Multirotor)" default_value: 60 @@ -3586,7 +3592,7 @@ groups: min: 1000 max: 5000 default_value: 1500 - + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c3a47fa529c..f1ce98fe8d8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -278,6 +278,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT, .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, + .mc_vel_xy_accel_attenuate = SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE_DEFAULT, #ifdef USE_D_BOOST .dBoostMin = SETTING_D_BOOST_MIN_DEFAULT, @@ -838,7 +839,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); - + if (pidProfile()->pidItermLimitPercent != 0){ float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 48f1352eae7..3abee2c9709 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -132,6 +132,7 @@ typedef struct pidProfile_s { uint8_t navVelXyDtermAttenuationEnd; // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input + uint8_t mc_vel_xy_accel_attenuate; #ifdef USE_D_BOOST float dBoostMin; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2264d842014..63234f9d7e9 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -632,6 +632,13 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA //Choose smaller attenuation factor and convert from attenuation to scale const float dtermScale = 1.0f - MIN(setpointScale, measurementScale); + // Apply accel attenuation factor + const float speedError = setpointXY != 0.0f ? fabsf(1.0f - (posControl.actualState.velXY / setpointXY)) : 1.0f; + float gainScale = 1.0f; + if (speedError < 0.25f) { + gainScale = (scaleRangef(speedError, 0.0f, 0.25f, pidProfile()->mc_vel_xy_accel_attenuate, 100.0f)) / 100.0f; + } + // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration @@ -643,7 +650,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA accelLimitXMin, accelLimitXMax, 0, // Flags - 1.0f, // Total gain scale + gainScale, // Total gain scale dtermScale // Additional dTerm scale ); float newAccelY = navPidApply3( @@ -654,7 +661,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA accelLimitYMin, accelLimitYMax, 0, // Flags - 1.0f, // Total gain scale + gainScale, // Total gain scale dtermScale // Additional dTerm scale ); From 7c72762e572383aab9e89168645d9cf32c69f346 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 21 Apr 2024 09:39:52 +0100 Subject: [PATCH 2/7] Changed control logic --- docs/Settings.md | 8 +++---- src/main/cms/cms_menu_imu.c | 2 +- src/main/fc/settings.yaml | 8 +++---- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation_multicopter.c | 24 ++++++++++---------- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 876bcec63b9..e00de8a91e0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3604,21 +3604,21 @@ Maximum D-term attenution percentage for horizontal velocity PID controller (Mul ### nav_mc_vel_xy_dterm_attenuation_end -A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum +Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s] | Default | Min | Max | | --- | --- | --- | -| 60 | 0 | 100 | +| 10 | 0 | 100 | --- ### nav_mc_vel_xy_dterm_attenuation_start -A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins +Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation begins [m/s] | Default | Min | Max | | --- | --- | --- | -| 10 | 0 | 100 | +| 5 | 0 | 100 | --- diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 46c655f4a58..ac5d60f16eb 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -437,7 +437,7 @@ static const OSD_Entry cmsx_menuMechanicsEntries[] = OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX), OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF), OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ), - OSD_SETTING_ENTRY("ACCEL ATTENUATION", SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE), + OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 06b02bab2e5..aa770342a39 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2064,14 +2064,14 @@ groups: max: 100 default_value: 90 - name: nav_mc_vel_xy_dterm_attenuation_start - description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins" - default_value: 10 + description: "Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation begins [m/s]" + default_value: 5 field: navVelXyDtermAttenuationStart min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation_end - description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" - default_value: 60 + description: "Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]" + default_value: 10 field: navVelXyDtermAttenuationEnd min: 0 max: 100 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cb12579b83d..a50f39db966 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4274,8 +4274,8 @@ void navigationUsePIDs(void) * Set coefficients used in MC VEL_XY */ multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; - multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f; - multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f; + multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart * 100.0f; + multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd * 100.0f; #ifdef USE_MR_BRAKING_MODE multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 63234f9d7e9..daec5cae4c5 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -552,20 +552,22 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - const float value, - const float maxValue, + float activeSpeed, const float attenuationFactor, - const float attenuationStart, - const float attenuationEnd + const float attenuationStartVel, + const float attenuationEndVel ) { - const float normalized = computeNormalizedVelocity(value, maxValue); - - float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor); - return constrainf(scale, 0, attenuationFactor); + activeSpeed -= attenuationStartVel; + if (activeSpeed <= 0.0f) { + return 0.0f; + } + const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel); + float scale = scaleRangef(normalized, 0.0f, 1.0f, 0.0f, attenuationFactor); + return constrainf(scale, 0.0f, attenuationFactor); } -static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) +static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit) { const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; @@ -616,14 +618,12 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA */ const float setpointScale = computeVelocityScale( setpointXY, - maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd ); const float measurementScale = computeVelocityScale( posControl.actualState.velXY, - maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd @@ -742,7 +742,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) // Get max speed for current NAV mode float maxSpeed = getActiveSpeed(); updatePositionVelocityController_MC(maxSpeed); - updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); + updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX); navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); From 3a063e2211338d14c7448d33c0d07756a5272f02 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 21 Apr 2024 23:00:59 +0100 Subject: [PATCH 3/7] remove old setting --- docs/Settings.md | 10 ---------- src/main/fc/settings.yaml | 6 ------ 2 files changed, 16 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index e88e126c15f..78d93a6c11f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3592,16 +3592,6 @@ P gain of altitude PID controller (Multirotor) --- -### nav_mc_vel_xy_accel_attenuate - -Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed. - -| Default | Min | Max | -| --- | --- | --- | -| 100 | 10 | 100 | - ---- - ### nav_mc_vel_xy_d D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b3a9e26bb0d..54b280c2e91 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2021,12 +2021,6 @@ groups: min: 0 max: 255 default_value: 40 - - name: nav_mc_vel_xy_accel_attenuate - description: "Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed." - field: mc_vel_xy_accel_attenuate - default_value: 100 - min: 10 - max: 100 - name: nav_mc_heading_p description: "P gain of Heading Hold controller (Multirotor)" default_value: 60 From 6d66851362f416f11278d03ea16b0ace40983c15 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 21 Apr 2024 23:09:28 +0100 Subject: [PATCH 4/7] remove old logic --- src/main/flight/pid.c | 1 - src/main/flight/pid.h | 1 - src/main/navigation/navigation_multicopter.c | 11 ++--------- 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 049a0d33d85..2a9a3407842 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -277,7 +277,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT, .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, - .mc_vel_xy_accel_attenuate = SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE_DEFAULT, #ifdef USE_D_BOOST .dBoostMin = SETTING_D_BOOST_MIN_DEFAULT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6f5c01afcd5..e088ccffadf 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -130,7 +130,6 @@ typedef struct pidProfile_s { uint8_t navVelXyDtermAttenuationEnd; // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input - uint8_t mc_vel_xy_accel_attenuate; #ifdef USE_D_BOOST float dBoostMin; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index f65364c952e..ed6ab002b37 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -631,13 +631,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA //Choose smaller attenuation factor and convert from attenuation to scale const float dtermScale = 1.0f - MIN(setpointScale, measurementScale); - // Apply accel attenuation factor - const float speedError = setpointXY != 0.0f ? fabsf(1.0f - (posControl.actualState.velXY / setpointXY)) : 1.0f; - float gainScale = 1.0f; - if (speedError < 0.25f) { - gainScale = (scaleRangef(speedError, 0.0f, 0.25f, pidProfile()->mc_vel_xy_accel_attenuate, 100.0f)) / 100.0f; - } - // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration @@ -649,7 +642,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA accelLimitXMin, accelLimitXMax, 0, // Flags - gainScale, // Total gain scale + 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); float newAccelY = navPidApply3( @@ -660,7 +653,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA accelLimitYMin, accelLimitYMax, 0, // Flags - gainScale, // Total gain scale + 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); From 642d0755310d4f5e80d1a59d7aeb9c4ee78322bd Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 22 Apr 2024 11:27:32 +0100 Subject: [PATCH 5/7] change setting name --- src/main/cms/cms_menu_imu.c | 3 ++- src/main/fc/settings.yaml | 8 ++++---- src/main/flight/pid.c | 4 ++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a63653ebb70..72dd090f0ee 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -460,7 +460,8 @@ static const OSD_Entry cmsx_menuMechanicsEntries[] = OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX), OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF), OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ), - OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START), + OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_SPEED), + OSD_SETTING_ENTRY("MC DTERM ATT END", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_SPEED), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 54b280c2e91..47b8dcb1775 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2039,14 +2039,14 @@ groups: min: 0 max: 100 default_value: 90 - - name: nav_mc_vel_xy_dterm_attenuation_start - description: "Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation begins [m/s]" + - name: nav_mc_vel_xy_dterm_attenuation_start_speed + description: "Horizontal speed at which nav_mc_vel_xy_dterm_attenuation begins [m/s]" default_value: 5 field: navVelXyDtermAttenuationStart min: 0 max: 100 - - name: nav_mc_vel_xy_dterm_attenuation_end - description: "Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]" + - name: nav_mc_vel_xy_dterm_attenuation_end_speed + description: "Horizontal speed at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]" default_value: 10 field: navVelXyDtermAttenuationEnd min: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2a9a3407842..2dd34c6ef07 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -273,8 +273,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT, .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT, - .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT, - .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT, + .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_SPEED_DEFAULT, + .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_SPEED_DEFAULT, .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, From a393edc752a767d7826ba3d8c55ff6fea386ffa2 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 22 Apr 2024 11:29:13 +0100 Subject: [PATCH 6/7] Update Settings.md --- docs/Settings.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 78d93a6c11f..4b3b9c5e982 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3612,9 +3612,9 @@ Maximum D-term attenution percentage for horizontal velocity PID controller (Mul --- -### nav_mc_vel_xy_dterm_attenuation_end +### nav_mc_vel_xy_dterm_attenuation_end_speed -Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s] +Horizontal speed at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s] | Default | Min | Max | | --- | --- | --- | @@ -3622,9 +3622,9 @@ Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/ --- -### nav_mc_vel_xy_dterm_attenuation_start +### nav_mc_vel_xy_dterm_attenuation_start_speed -Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation begins [m/s] +Horizontal speed at which nav_mc_vel_xy_dterm_attenuation begins [m/s] | Default | Min | Max | | --- | --- | --- | From bd5f74fc49399648577bb1eb154c49e97f95412b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 22 Apr 2024 23:17:06 +0100 Subject: [PATCH 7/7] Update navigation_multicopter.c --- src/main/navigation/navigation_multicopter.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index ed6ab002b37..9c9a180c5c1 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -561,7 +561,8 @@ static float computeVelocityScale( if (activeSpeed <= 0.0f) { return 0.0f; } - const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel); + + const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel - attenuationStartVel); float scale = scaleRangef(normalized, 0.0f, 1.0f, 0.0f, attenuationFactor); return constrainf(scale, 0.0f, attenuationFactor); }