From f30a21237829fcb6d6d5950d5398b38be8710ca4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 19 Jan 2020 20:50:39 +0100 Subject: [PATCH 1/4] Attenuate VEL_XY when UAV is traveling at high speed --- src/main/fc/settings.yaml | 12 ++++ src/main/flight/pid.c | 3 + src/main/flight/pid.h | 4 +- src/main/navigation/navigation.c | 19 +++-- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/navigation/navigation_multicopter.c | 73 +++++++++++++++++--- src/main/navigation/navigation_private.h | 12 +++- 7 files changed, 107 insertions(+), 18 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 26b1fe49f7..27a2a28efa 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1203,6 +1203,18 @@ groups: field: navVelXyDTermLpfHz min: 0 max: 100 + - name: nav_mc_vel_xy_dterm_dyn_scale + field: navVelXyDtermDynScale + min: 0 + max: 1 + - name: nav_mc_vel_xy_dterm_dyn_scale_min_at + field: navVelXyDtermDynScaleMinAt + min: 0 + max: 1 + - name: nav_mc_vel_xy_dterm_dyn_scale_max_at + field: navVelXyDtermDynScaleMaxAt + min: 0 + max: 1 - name: nav_fw_pos_z_p field: bank_fw.pid[PID_POS_Z].P condition: USE_NAV diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ce7e657982..862ec61897 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -246,6 +246,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, + .navVelXyDtermDynScale = 0.2f, + .navVelXyDtermDynScaleMinAt = 0.6f, + .navVelXyDtermDynScaleMaxAt = 0.2f, .iterm_relax_type = ITERM_RELAX_SETPOINT, .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax = ITERM_RELAX_RP, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 343ecf21e5..0ce8b3e94f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -137,7 +137,9 @@ typedef struct pidProfile_s { uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) float navVelXyDTermLpfHz; - + float navVelXyDtermDynScale; // VEL_XY dynamic Dterm scale: Dterm will be attenuated by this value when velocity is at navVelXyDtermMaxAt and 1.0 when velocity is at navVelXyDtermMinAt + float navVelXyDtermDynScaleMinAt; // VEL_XY dynamic Dterm scale: Dterm will be scaled down fully to navVelXyDtermDynScale when velocity in setpoint and measurement is above this value. It is a factor of max. velocity processed by the controller + float navVelXyDtermDynScaleMaxAt; // VEL_XY dynamic Dterm scale: Dterm will be not scaled when velocity in setpoint and measurement is below this value. It is a factor of max. velocity processed by the controller uint8_t iterm_relax_type; // Specifies type of relax algorithm 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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3cfe2459ef..2956b6e962 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1711,8 +1711,17 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) // Implementation of PID with back-calculation I-term anti-windup // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228) // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf -float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler) -{ +float navPidApply3( + pidController_t *pid, + const float setpoint, + const float measurement, + const float dt, + const float outMin, + const float outMax, + const pidControllerFlags_e pidFlags, + const float gainScaler, + const float dTermScaler +) { float newProportional, newDerivative, newFeedForward; float error = setpoint - measurement; @@ -1736,11 +1745,13 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu } if (pid->dTermLpfHz > 0) { - newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler; + newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt); } else { newDerivative = pid->param.kD * newDerivative; } + newDerivative = newDerivative * gainScaler * dTermScaler; + if (pidFlags & PID_ZERO_INTEGRATOR) { pid->integrator = 0.0f; } @@ -1780,7 +1791,7 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags) { - return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f); + return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f); } void navPidReset(pidController_t *pid) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f7d0891761..6b4bfe8f08 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -134,7 +134,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); // PID controller to translate energy balance error [J] into pitch angle [decideg] - float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv); + float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f); targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); // Reconstrain pitch angle ( >0 - climb, <0 - dive) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 596b47f646..aee57bffca 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -437,7 +437,7 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) return 1.0f - posControl.posResponseExpo * (1.0f - (velScale * velScale)); // x^3 expo factor } -static void updatePositionVelocityController_MC(void) +static void updatePositionVelocityController_MC(const float maxSpeed) { const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; @@ -446,9 +446,6 @@ static void updatePositionVelocityController_MC(void) float newVelX = posErrorX * posControl.pids.pos[X].param.kP; float newVelY = posErrorY * posControl.pids.pos[Y].param.kP; - // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); - // Scale velocity to respect max_speed float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); if (newVelTotal > maxSpeed) { @@ -472,12 +469,20 @@ static void updatePositionVelocityController_MC(void) #endif } -static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit) +static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) { + const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; + const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; + const float measurementXY = sqrtf(powf(measurementX, 2)+powf(measurementY, 2)); + + const float setpointX = posControl.desiredState.vel.x; + const float setpointY = posControl.desiredState.vel.y; + const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2)); + // Calculate velocity error - const float velErrorX = posControl.desiredState.vel.x - navGetCurrentActualPositionAndVelocity()->vel.x; - const float velErrorY = posControl.desiredState.vel.y - navGetCurrentActualPositionAndVelocity()->vel.y; + const float velErrorX = setpointX - measurementX; + const float velErrorY = setpointY - measurementY; // Calculate XY-acceleration limit according to velocity error limit float accelLimitX, accelLimitY; @@ -508,11 +513,55 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // TODO: Verify if we need jerk limiting after all + /* + * This PID controller has dynamic dTerm scale. It's less active when controller + * is tracking setpoint at high speed. Full dTerm is required only for position hold, + * acceleration and deceleration + * Scale down dTerm with 2D speed + */ + + //Normalize the setpoint and measurement between 0.0f and 1.0f where 1.0f is currently used max speed + const float setpointNormalized = constrainf(scaleRangef(fabsf(setpointXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); + const float measurementNormalized = constrainf(scaleRangef(fabsf(measurementXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); + + /* + * Map normalized speed of setpoint and measurement between 1.0f and nav_mc_vel_xy_dterm_dyn_scale + * 1.0f means that Dterm is not attenuated + * navVelXyDtermDynScale means full allowed attenuation + * + * Dterm can be attenuated when both setpoint and measurement is high - UAV is moving close to desired velocity + */ + const float setpointScale = constrainf(scaleRangef(setpointNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); + const float measurementScale = constrainf(scaleRangef(measurementNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); + + //Use the higher scaling factor from the two above + const float dtermScale = MAX(setpointScale, measurementScale); + // 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 - float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0); - float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0); + float newAccelX = navPidApply3( + &posControl.pids.vel[X], + setpointX, + measurementX, + US2S(deltaMicros), + accelLimitXMin, + accelLimitXMax, + 0, // Flags + 1.0f, // Total gain scale + dtermScale // Additional dTerm scale + ); + float newAccelY = navPidApply3( + &posControl.pids.vel[Y], + setpointY, + measurementY, + US2S(deltaMicros), + accelLimitYMin, + accelLimitYMax, + 0, // Flags + 1.0f, // Total gain scale + dtermScale // Additional dTerm scale + ); int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle); @@ -589,8 +638,10 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) if (!bypassPositionController) { // Update position controller if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { - updatePositionVelocityController_MC(); - updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX); + // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s + const float maxSpeed = getActiveWaypointSpeed(); + updatePositionVelocityController_MC(maxSpeed); + updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); } else { resetMulticopterPositionController(); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index a11e9a0f62..f56048e8ab 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -373,7 +373,17 @@ extern navigationPosControl_t posControl; const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); -float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler); +float navPidApply3( + pidController_t *pid, + const float setpoint, + const float measurement, + const float dt, + const float outMin, + const float outMax, + const pidControllerFlags_e pidFlags, + const float gainScaler, + const float dTermScaler +); void navPidReset(pidController_t *pid); void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); From 9538e65dc9598e548114af67bfa7e2f6214e0b35 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 25 Jan 2020 18:08:57 +0100 Subject: [PATCH 2/4] Add basic debug --- src/main/navigation/navigation_multicopter.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index aee57bffca..13eea54dfe 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -524,6 +524,9 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float setpointNormalized = constrainf(scaleRangef(fabsf(setpointXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); const float measurementNormalized = constrainf(scaleRangef(fabsf(measurementXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); + DEBUG_SET(DEBUG_ALWAYS, 0, setpointNormalized * 100); + DEBUG_SET(DEBUG_ALWAYS, 1, measurementNormalized * 100); + /* * Map normalized speed of setpoint and measurement between 1.0f and nav_mc_vel_xy_dterm_dyn_scale * 1.0f means that Dterm is not attenuated @@ -534,8 +537,12 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA const float setpointScale = constrainf(scaleRangef(setpointNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); const float measurementScale = constrainf(scaleRangef(measurementNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); + DEBUG_SET(DEBUG_ALWAYS, 2, setpointScale * 100); + DEBUG_SET(DEBUG_ALWAYS, 3, measurementScale * 100); + //Use the higher scaling factor from the two above const float dtermScale = MAX(setpointScale, measurementScale); + DEBUG_SET(DEBUG_ALWAYS, 4, dtermScale * 100); // 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 From 0ae860faf64ceaad7e18f88ef607107b9aeeaa00 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 6 Mar 2020 15:27:59 +0100 Subject: [PATCH 3/4] Refactor Dterm atenuation --- src/main/fc/settings.yaml | 18 ++++++------ src/main/flight/pid.c | 6 ++-- src/main/flight/pid.h | 6 ++-- src/main/navigation/navigation.c | 13 +++++++-- src/main/navigation/navigation_multicopter.c | 29 ++++++++------------ src/main/navigation/navigation_private.h | 8 ++++++ 6 files changed, 46 insertions(+), 34 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2484bd4f54..e59925478d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1184,18 +1184,18 @@ groups: field: navVelXyDTermLpfHz min: 0 max: 100 - - name: nav_mc_vel_xy_dterm_dyn_scale - field: navVelXyDtermDynScale + - name: nav_mc_vel_xy_dterm_attenuation + field: navVelXyDtermAttenuation min: 0 - max: 1 - - name: nav_mc_vel_xy_dterm_dyn_scale_min_at - field: navVelXyDtermDynScaleMinAt + max: 100 + - name: nav_mc_vel_xy_dterm_attenuation_start + field: navVelXyDtermAttenuationStart min: 0 - max: 1 - - name: nav_mc_vel_xy_dterm_dyn_scale_max_at - field: navVelXyDtermDynScaleMaxAt + max: 100 + - name: nav_mc_vel_xy_dterm_attenuation_end + field: navVelXyDtermAttenuationEnd min: 0 - max: 1 + max: 100 - name: nav_fw_pos_z_p field: bank_fw.pid[PID_POS_Z].P condition: USE_NAV diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 07734efdac..3897ecac33 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -257,9 +257,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, - .navVelXyDtermDynScale = 0.2f, - .navVelXyDtermDynScaleMinAt = 0.6f, - .navVelXyDtermDynScaleMaxAt = 0.2f, + .navVelXyDtermAttenuation = 80, + .navVelXyDtermAttenuationStart = 20, + .navVelXyDtermAttenuationEnd = 60, .iterm_relax_type = ITERM_RELAX_SETPOINT, .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax = ITERM_RELAX_RP, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cb53cc220c..51e6469d9d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -138,9 +138,9 @@ typedef struct pidProfile_s { uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) float navVelXyDTermLpfHz; - float navVelXyDtermDynScale; // VEL_XY dynamic Dterm scale: Dterm will be attenuated by this value when velocity is at navVelXyDtermMaxAt and 1.0 when velocity is at navVelXyDtermMinAt - float navVelXyDtermDynScaleMinAt; // VEL_XY dynamic Dterm scale: Dterm will be scaled down fully to navVelXyDtermDynScale when velocity in setpoint and measurement is above this value. It is a factor of max. velocity processed by the controller - float navVelXyDtermDynScaleMaxAt; // VEL_XY dynamic Dterm scale: Dterm will be not scaled when velocity in setpoint and measurement is below this value. It is a factor of max. velocity processed by the controller + uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity + uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity + uint8_t navVelXyDtermAttenuationEnd; // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity uint8_t iterm_relax_type; // Specifies type of relax algorithm 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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 6400c95c9c..634aae2c04 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -171,8 +171,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, } ); -navigationPosControl_t posControl; -navSystemStatus_t NAV_Status; +EXTENDED_FASTRAM navigationPosControl_t posControl; +EXTENDED_FASTRAM navSystemStatus_t NAV_Status; +EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; #if defined(NAV_BLACKBOX) int16_t navCurrentState; @@ -3231,6 +3232,14 @@ 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.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f; + // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z navPidInit( &posControl.pids.pos[Z], diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 13eea54dfe..b0130f3a11 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -50,7 +50,6 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" - /*----------------------------------------------------------- * Altitude controller for multicopter aircraft *-----------------------------------------------------------*/ @@ -470,11 +469,9 @@ static void updatePositionVelocityController_MC(const float maxSpeed) } static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) -{ - +{ const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; - const float measurementXY = sqrtf(powf(measurementX, 2)+powf(measurementY, 2)); const float setpointX = posControl.desiredState.vel.x; const float setpointY = posControl.desiredState.vel.y; @@ -522,26 +519,25 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA //Normalize the setpoint and measurement between 0.0f and 1.0f where 1.0f is currently used max speed const float setpointNormalized = constrainf(scaleRangef(fabsf(setpointXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); - const float measurementNormalized = constrainf(scaleRangef(fabsf(measurementXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); + const float measurementNormalized = constrainf(scaleRangef(fabsf(posControl.actualState.velXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); DEBUG_SET(DEBUG_ALWAYS, 0, setpointNormalized * 100); DEBUG_SET(DEBUG_ALWAYS, 1, measurementNormalized * 100); /* - * Map normalized speed of setpoint and measurement between 1.0f and nav_mc_vel_xy_dterm_dyn_scale - * 1.0f means that Dterm is not attenuated - * navVelXyDtermDynScale means full allowed attenuation - * * Dterm can be attenuated when both setpoint and measurement is high - UAV is moving close to desired velocity */ - const float setpointScale = constrainf(scaleRangef(setpointNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); - const float measurementScale = constrainf(scaleRangef(measurementNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f); + float setpointScale = scaleRangef(setpointNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation); + setpointScale = constrainf(setpointScale, 0, multicopterPosXyCoefficients.dTermAttenuation); + + float measurementScale = scaleRangef(measurementNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation); + measurementScale = constrainf(measurementScale, 0, multicopterPosXyCoefficients.dTermAttenuation); DEBUG_SET(DEBUG_ALWAYS, 2, setpointScale * 100); DEBUG_SET(DEBUG_ALWAYS, 3, measurementScale * 100); - //Use the higher scaling factor from the two above - const float dtermScale = MAX(setpointScale, measurementScale); + //Choose smaller attenuation factor and convert from attenuation to scale + const float dtermScale = 1.0f - MIN(setpointScale, measurementScale); DEBUG_SET(DEBUG_ALWAYS, 4, dtermScale * 100); // Apply PID with output limiting and I-term anti-windup @@ -574,8 +570,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA #ifdef USE_MR_BRAKING_MODE //Boost required accelerations - if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0) { - const float rawBoostFactor = (float)navConfig()->mc.braking_boost_factor / 100.0f; + if (STATE(NAV_CRUISE_BRAKING_BOOST) && multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) { //Scale boost factor according to speed const float boostFactor = constrainf( @@ -584,10 +579,10 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA navConfig()->mc.braking_boost_speed_threshold, navConfig()->general.max_manual_speed, 0.0f, - rawBoostFactor + multicopterPosXyCoefficients.breakingBoostFactor ), 0.0f, - rawBoostFactor + multicopterPosXyCoefficients.breakingBoostFactor ); //Boost required acceleration for harder braking diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f56048e8ab..e97d76311d 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -363,11 +363,19 @@ typedef struct { float totalTripDistance; } navigationPosControl_t; +typedef struct { + float dTermAttenuation; + float dTermAttenuationStart; + float dTermAttenuationEnd; + float breakingBoostFactor; +} multicopterPosXyCoefficients_t; + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList); #endif extern navigationPosControl_t posControl; +extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients; /* Internally used functions */ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); From abafe9b258ad1594e27d29972c37b559dc3ecdbd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Jul 2020 22:37:54 +0200 Subject: [PATCH 4/4] Refactor attenuation factor computation --- src/main/fc/settings.yaml | 6 +++ src/main/flight/pid.c | 4 +- src/main/navigation/navigation_multicopter.c | 53 ++++++++++++-------- 3 files changed, 41 insertions(+), 22 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5215060725..8c2ddc4639 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1651,14 +1651,20 @@ groups: min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation + description: "Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating." + default_value: "90" field: navVelXyDtermAttenuation min: 0 max: 100 - 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" 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" field: navVelXyDtermAttenuationEnd min: 0 max: 100 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4687ce4e4c..d48118bd37 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -258,8 +258,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, - .navVelXyDtermAttenuation = 80, - .navVelXyDtermAttenuationStart = 20, + .navVelXyDtermAttenuation = 90, + .navVelXyDtermAttenuationStart = 10, .navVelXyDtermAttenuationEnd = 60, .iterm_relax_type = ITERM_RELAX_SETPOINT, .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index b7bfa2e2d7..964cb980fe 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -469,6 +469,25 @@ static void updatePositionVelocityController_MC(const float maxSpeed) #endif } +static float computeNormalizedVelocity(const float value, const float maxValue) +{ + return constrainf(scaleRangef(fabsf(value), 0, maxValue, 0.0f, 1.0f), 0.0f, 1.0f); +} + +static float computeVelocityScale( + const float value, + const float maxValue, + const float attenuationFactor, + const float attenuationStart, + const float attenuationEnd +) +{ + const float normalized = computeNormalizedVelocity(value, maxValue); + + float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor); + return constrainf(scale, 0, attenuationFactor); +} + static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) { const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; @@ -517,29 +536,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA * acceleration and deceleration * Scale down dTerm with 2D speed */ - - //Normalize the setpoint and measurement between 0.0f and 1.0f where 1.0f is currently used max speed - const float setpointNormalized = constrainf(scaleRangef(fabsf(setpointXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); - const float measurementNormalized = constrainf(scaleRangef(fabsf(posControl.actualState.velXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f); - - DEBUG_SET(DEBUG_ALWAYS, 0, setpointNormalized * 100); - DEBUG_SET(DEBUG_ALWAYS, 1, measurementNormalized * 100); - - /* - * Dterm can be attenuated when both setpoint and measurement is high - UAV is moving close to desired velocity - */ - float setpointScale = scaleRangef(setpointNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation); - setpointScale = constrainf(setpointScale, 0, multicopterPosXyCoefficients.dTermAttenuation); - - float measurementScale = scaleRangef(measurementNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation); - measurementScale = constrainf(measurementScale, 0, multicopterPosXyCoefficients.dTermAttenuation); - - DEBUG_SET(DEBUG_ALWAYS, 2, setpointScale * 100); - DEBUG_SET(DEBUG_ALWAYS, 3, measurementScale * 100); + 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 + ); //Choose smaller attenuation factor and convert from attenuation to scale const float dtermScale = 1.0f - MIN(setpointScale, measurementScale); - DEBUG_SET(DEBUG_ALWAYS, 4, dtermScale * 100); // 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