diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d0ab6d67ee..ab2b676c34 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1650,6 +1650,24 @@ groups: field: navVelXyDTermLpfHz 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 - name: nav_fw_pos_z_p description: "P gain of altitude PID controller (Fixedwing)" default_value: "50" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6cdf4e41e0..732bc26520 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -260,6 +260,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .loiter_direction = NAV_LOITER_RIGHT, .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, + .navVelXyDtermAttenuation = 90, + .navVelXyDtermAttenuationStart = 10, + .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 67b47cd83b..c686266150 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -135,7 +135,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; - + 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 a5de607a30..63b5f35f95 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -175,6 +175,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; +EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; #if defined(NAV_BLACKBOX) int16_t navCurrentState; @@ -1874,8 +1875,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; @@ -1899,11 +1909,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; } @@ -1943,7 +1955,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) @@ -3400,6 +3412,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_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 7af61ae6ce..389d934b57 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -146,7 +146,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); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 90d425dfaa..964cb980fe 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -51,7 +51,6 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" - /*----------------------------------------------------------- * Altitude controller for multicopter aircraft *-----------------------------------------------------------*/ @@ -438,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; @@ -447,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) { @@ -473,12 +469,37 @@ static void updatePositionVelocityController_MC(void) #endif } -static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit) +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; + const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; + + 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; @@ -509,18 +530,61 @@ 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 + */ + 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); + // 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); #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( @@ -529,10 +593,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 @@ -590,8 +654,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 7feaa50b1f..fcae1b7ded 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -368,17 +368,35 @@ 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); 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);