diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c566c94bfad..57a6ba5c066 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2927,8 +2927,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller - posControl.desiredState.pos.z = pos->z; + updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, pos->z, ROC_TO_ALT_TARGET); } // Heading @@ -3036,66 +3035,68 @@ bool isProbablyStillFlying(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +static bool isMaxAltitudeLimitExceeded(void) { -#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s + return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude; +} - static timeUs_t lastUpdateTimeUs; - timeUs_t currentTimeUs = micros(); +int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) +{ + if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) { + targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude); + } - // Terrain following uses different altitude measurement - const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); - if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { - /* ROC_TO_ALT_CONSTANT - constant climb rate - * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached - * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */ + float maxClimbRate = navConfig()->general.max_auto_climb_rate; + if (emergLandingIsActive) { + maxClimbRate = navConfig()->general.emerg_descent_rate; + } else if (posControl.flags.isAdjustingAltitude) { + maxClimbRate = navConfig()->general.max_manual_climb_rate; + } + const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z; + float targetVel = 0.0f; - if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { - const int8_t direction = desiredClimbRate > 0 ? 1 : -1; - const float absClimbRate = fabsf(desiredClimbRate); - const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; - const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, - 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + if (STATE(MULTIROTOR)) { + targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros); + } else { + targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate); + } - desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); - } + if (emergLandingIsActive && targetAltitudeError > -50) { + return -100; + } else { + return constrainf(targetVel, -maxClimbRate, maxClimbRate); + } +} - /* - * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 - * In other words, when altitude is reached, allow it only to shrink - */ - if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { - desiredClimbRate = 0; - } +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +{ + /* ROC_TO_ALT_CONSTANT - constant climb rate. desiredClimbRate direction (+ or -) is required + * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached + * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC. + * desiredClimbRate direction isn't required, set by target altitude direction instead + * NAV_IMPOSSIBLE_ALTITUDE_TARGET simply uses posControl.desiredState.pos.z to set a constant climb rate */ - if (STATE(FIXED_WING_LEGACY)) { - // Fixed wing climb rate controller is open-loop. We simply move the known altitude target - float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); - static bool targetHoldActive = false; - - if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) { - // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target - if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) { - posControl.desiredState.pos.z += desiredClimbRate * timeDelta; - targetHoldActive = false; - } else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up - posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate; - targetHoldActive = true; - } - } else { - targetHoldActive = false; - } - } - else { - // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD - posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP); - } - } else { // ROC_TO_ALT_RESET or zero desired climbrate + // Terrain following uses different altitude measurement + const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; + + if (mode == ROC_TO_ALT_RESET) { posControl.desiredState.pos.z = altitudeToUse; + } else if (mode == ROC_TO_ALT_TARGET) { + posControl.desiredState.pos.z = targetAltitude; + } else { // ROC_TO_ALT_CONSTANT + posControl.desiredState.pos.z = NAV_IMPOSSIBLE_ALTITUDE_TARGET; + } + + /* + * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve + */ + if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) { + posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude); } - lastUpdateTimeUs = currentTimeUs; + posControl.desiredState.vel.z = desiredClimbRate; } static void resetAltitudeController(bool useTerrainFollowing) @@ -4317,9 +4318,9 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f, + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, 0.0f, NAV_DTERM_CUT_HZ, 0.0f diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e6084e0972e..c5dde6dba48 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET); } return false; } @@ -134,46 +134,34 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) { static pt1Filter_t velzFilterState; - // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to - // velocity error. We use PID controller on altitude error and calculate desired pitch angle + float desiredClimbRate = posControl.desiredState.vel.z; - // Update energies - const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS; - const float demSKE = 0.0f; - - const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS; - const float estSKE = 0.0f; - - // speedWeight controls balance between potential and kinetic energy used for pitch controller - // speedWeight = 1.0 : pitch will only control airspeed and won't control altitude - // speedWeight = 0.5 : pitch will be used to control both airspeed and altitude - // speedWeight = 0.0 : pitch will only control altitude - const float speedWeight = 0.0f; // no speed sensing for now - - const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight; - const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight; + if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) { + desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); + } - // SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed - const float pitchGainInv = 1.0f / 1.0f; + // Reduce max allowed climb pitch if performing loiter (stall prevention) + if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) { + desiredClimbRate *= 0.67f; + } // Here we use negative values for dive for better clarity - float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // Reduce max allowed climb pitch if performing loiter (stall prevention) - if (needToCalculateCircularLoiter) { - maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; - } - - // 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, 1.0f); + // PID controller to translate desired climb rate error into pitch angle [decideg] + float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z; + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); - // Reconstrain pitch angle ( >0 - climb, <0 - dive) + // Reconstrain pitch angle (> 0 - climb, < 0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); posControl.rcAdjustment[PITCH] = targetPitchAngle; + + posControl.desiredState.vel.z = desiredClimbRate; + navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767); } void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2264d842014..935665ee60e 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -63,28 +63,26 @@ static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; -// Position to velocity controller for Z axis -static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) { - float targetVel = sqrtControllerApply( - &alt_hold_sqrt_controller, - posControl.desiredState.pos.z, - navGetCurrentActualPositionAndVelocity()->pos.z, - US2S(deltaMicros) + return sqrtControllerApply( + &alt_hold_sqrt_controller, + targetAltitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + US2S(deltaMicros) ); +} - // hard limit desired target velocity to max_climb_rate - float vel_max_z = 0.0f; +// Position to velocity controller for Z axis +static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +{ + float targetVel = posControl.desiredState.vel.z; - if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->general.max_manual_climb_rate; - } else { - vel_max_z = navConfig()->general.max_auto_climb_rate; + if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) { + targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); } - targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); - - posControl.pids.pos[Z].output_constrained = targetVel; + posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info // Limit max up/down acceleration target const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f); @@ -132,8 +130,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) // In terrain follow mode we apply different logic for terrain control if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { // We have solid terrain sensor signal - directly map throttle to altitude - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); - posControl.desiredState.pos.z = altTarget; + updateClimbRateToAltitudeController(navConfig()->general.max_manual_climb_rate, altTarget, ROC_TO_ALT_TARGET); } else { updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); @@ -165,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET); } return false; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c408f109c9b..35e5492c6f1 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -47,6 +47,8 @@ #define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points +#define NAV_IMPOSSIBLE_ALTITUDE_TARGET 10000000 // 100km in cm. Serves as a flag for vertical velocity control + #define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); @@ -481,6 +483,7 @@ bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError); +int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros); bool checkForPositionSensorTimeout(void); @@ -499,10 +502,9 @@ bool adjustMulticopterHeadingFromRCInput(void); bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment); void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); - bool isMulticopterLandingDetected(void); - void calculateMulticopterInitialHoldPosition(fpVector3_t * pos); +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros); /* Fixed-wing specific functions */ void setupFixedWingAltitudeController(void);