diff --git a/docs/Settings.md b/docs/Settings.md index 22bd7623bab..4b3b9c5e982 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3612,23 +3612,23 @@ 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 -A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum +Horizontal speed 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 +### nav_mc_vel_xy_dterm_attenuation_start_speed -A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins +Horizontal speed 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 f8c75ab4801..72dd090f0ee 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -460,6 +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_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 30d4c096bed..47b8dcb1775 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2039,15 +2039,15 @@ groups: min: 0 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 + - 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: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" - default_value: 60 + - 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 max: 100 @@ -2295,7 +2295,7 @@ groups: field: w_z_surface_p min: 0 max: 100 - default_value: 3.5 + default_value: 3.5 - name: inav_w_z_surface_v description: "Weight of rangefinder measurements in estimated climb rate. Setting is used on both airplanes and multirotors when rangefinder is present and Surface mode enabled" field: w_z_surface_v @@ -4108,7 +4108,7 @@ groups: type: navFwAutolandConfig_t headers: ["navigation/navigation.h"] condition: USE_FW_AUTOLAND - members: + members: - name: nav_fw_land_approach_length description: "Length of the final approach" default_value: 35000 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, diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e7261b068cf..38b6c87237f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4641,8 +4641,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 54caccb52b5..9c9a180c5c1 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -551,20 +551,23 @@ 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); + activeSpeed -= attenuationStartVel; + if (activeSpeed <= 0.0f) { + return 0.0f; + } - float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor); - return constrainf(scale, 0, attenuationFactor); + const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel - attenuationStartVel); + 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; @@ -615,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 @@ -734,7 +735,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);