diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1d2702f66d0..2603185e518 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3712,8 +3712,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu randNumOld = randNum; } - DEBUG_SET(DEBUG_ALWAYS, 0, counter); - DEBUG_SET(DEBUG_ALWAYS, 1, randNum); DEBUG_SET(DEBUG_ALWAYS, 5, gpsSol.llh.alt); gpsSol.llh.alt += (20 * signChanger * randNum * counter); // in cm DEBUG_SET(DEBUG_ALWAYS, 6, gpsSol.llh.alt); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c94f0798d4d..1907c8dd7a9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2060,13 +2060,13 @@ groups: # CR47 - name: nav_mc_vel_xy_dterm_attenuation_start description: "Horizontal velocity where nav_mc_vel_xy_dterm_attenuation begins [m/s]" - default_value: 10 + default_value: 5 field: navVelXyDtermAttenuationStart min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation_end description: "Horizontal velocity where nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]" - default_value: 20 + default_value: 10 field: navVelXyDtermAttenuationEnd min: 0 max: 100 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d3dc6750fc8..5aa3395ac70 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4407,7 +4407,7 @@ void navigationUsePIDs(void) * Set coefficients used in MC VEL_XY */ // CR47 - multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation * 100.0f; + multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart * 100.0f; multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd * 100.0f; // multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a2a1c19c603..77f84182ad2 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -570,19 +570,21 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - float value, // CR47 - // const float maxValue, // CR47 + // CR47 + float value, + // const float maxValue, const float attenuationFactor, - const float attenuationStart, - const float attenuationEnd + const float attenuationStartVel, + const float attenuationEndVel + // CR47 ) { // CR47 - value -= attenuationStart; + value -= attenuationStartVel; if (value <= 0.0f) { return 0.0f; } - const float normalized = computeNormalizedVelocity(value, attenuationEnd); + const float normalized = computeNormalizedVelocity(value, attenuationEndVel); float scale = scaleRangef(normalized, 0.0f, 1.0f, 0.0f, attenuationFactor); return constrainf(scale, 0.0f, attenuationFactor);