diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e164b33ab5..51d415a960 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1286,7 +1286,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n if (STATE(FIXED_WING_LEGACY)) { if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) { float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_CONSTANT); } else { tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); @@ -1396,7 +1396,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { resetLandingDetector(); // force reset landing detector just in case - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); @@ -1424,7 +1424,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, tmpHomePos->z, ROC_TO_ALT_TARGET); } } else { @@ -1471,7 +1471,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); } - updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT); return NAV_FSM_EVENT_NONE; } @@ -1494,7 +1494,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } // Prevent I-terms growing when already landed @@ -1704,7 +1704,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi // Adjust altitude to waypoint setting if (STATE(AIRPLANE)) { int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; - updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET); } else { setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z); } @@ -2786,7 +2786,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller posControl.desiredState.pos.z = pos->z; } @@ -2877,28 +2877,34 @@ bool isFlightDetected(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode) +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) { +#define MIN_TARGET_CLIMB_RATE 100.0f + static timeUs_t lastUpdateTimeUs; timeUs_t currentTimeUs = micros(); // Terrain following uses different altitude measurement const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; - if (mode == ROC_TO_ALT_RESET) { - lastUpdateTimeUs = currentTimeUs; - posControl.desiredState.pos.z = altitudeToUse; - } - else { // ROC_TO_ALT_NORMAL + if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { // ROC_TO_ALT_CONSTANT & ROC_TO_ALT_TARGET + + /* */ + if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { + int8_t direction = desiredClimbRate > 0 ? 1 : -1; + float absClimbRate = fabsf(desiredClimbRate); + uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate; + float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, + direction * -500.0f, direction * -maxRateCutoffAlt, MIN_TARGET_CLIMB_RATE, absClimbRate); + + desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); + } /* * 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 - ) { + if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { desiredClimbRate = 0; } @@ -2924,9 +2930,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti // 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); } - - lastUpdateTimeUs = currentTimeUs; + } else { // ROC_TO_ALT_RESET or zero desired climbrate + posControl.desiredState.pos.z = altitudeToUse; } + + lastUpdateTimeUs = currentTimeUs; } static void resetAltitudeController(bool useTerrainFollowing) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4ca3d2fc3d..075176c79e 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); - updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; } @@ -760,7 +760,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; if (posControl.flags.estAltStatus >= EST_USABLE) { - updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET); applyFixedWingAltitudeAndThrottleController(currentTimeUs); int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 2c5eaf4a34..23a71c5c68 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -133,11 +133,11 @@ 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, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); posControl.desiredState.pos.z = altTarget; } else { - updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); } // In surface tracking we always indicate that we're adjusting altitude @@ -159,14 +159,14 @@ bool adjustMulticopterAltitudeFromRCInput(void) rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband); } - updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true; } else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); } return false; @@ -842,7 +842,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 0598467d28..fdfa14e9b4 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -61,7 +61,8 @@ typedef enum { typedef enum { ROC_TO_ALT_RESET, - ROC_TO_ALT_NORMAL + ROC_TO_ALT_CONSTANT, + ROC_TO_ALT_TARGET } climbRateToAltitudeControllerMode_e; typedef enum { @@ -447,7 +448,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED -void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode); +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode); bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void);