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] 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);