From 19faf75e75ae917fbed37ffeb2e0ddfa309a72e1 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 3 Jun 2019 21:41:23 +0200 Subject: [PATCH] [RTH] Refactor RTH code to respect initial/final/hover RTH altitudes --- src/main/flight/rth_estimator.c | 4 +- src/main/navigation/navigation.c | 271 ++++++++++++++--------- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_private.h | 23 +- 4 files changed, 188 insertions(+), 112 deletions(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index e0298a248e..fc2e7706fc 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -138,7 +138,7 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan // output in Watt static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) { const float timeToHome = distanceToHome / speedToHome; // seconds - const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - calculateRTHAltitude()) : 0); + const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - getFinalRTHAltitude()) : 0); const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle); return estimatePitchPower(pitchToHome) * timeToHome / 3600; } @@ -156,7 +156,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { )) return -1; - const float RTH_initial_altitude_change = MAX(0, (calculateRTHAltitude() - getEstimatedActualPosition(Z)) / 100); + const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100); float RTH_heading; // degrees #ifdef USE_WIND_ESTIMATOR diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cc6b2b35e8..1a97e7a085 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -185,6 +185,7 @@ uint16_t navEPV; int16_t navAccNEU[3]; #endif +static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode); static void updateDesiredRTHAltitude(void); static void resetAltitudeController(bool useTerrainFollowing); static void resetPositionController(void); @@ -197,6 +198,7 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); +static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); @@ -1112,28 +1114,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // If we have valid pos sensor OR we are configured to ignore GPS loss if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { const float rthAltitudeMargin = STATE(FIXED_WING) ? - MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)) : // Airplane - MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.homeWaypointAbove.pos.z - posControl.homePosition.pos.z)); // Copters + MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)) : // Airplane + MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // Copters - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homeWaypointAbove.pos.z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING)) { initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); } + // Save initial home distance for future use + posControl.rthState.rthInitialDistance = posControl.homeDistance; + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); } else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); } - // Save initial home distance for future use - posControl.rthInitialHomeDistance = posControl.homeDistance; - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME } else { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); + /* For multi-rotors execute sanity check during initial ascent as well */ if (!STATE(FIXED_WING)) { if (!validateRTHSanityChecker()) { @@ -1143,21 +1148,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING)) { - fpVector3_t pos = posControl.homeWaypointAbove.pos; - pos.z += FW_RTH_CLIMB_OVERSHOOT_CM; - - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); } else { // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // it in a reasonable time. Immediately after we finish this phase - target the original altitude. - fpVector3_t pos = posControl.homeWaypointAbove.pos; - pos.z += MR_RTH_CLIMB_OVERSHOOT_CM; + tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM; if (navConfig()->general.flags.rth_tail_first) { - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); } else { - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); } } @@ -1180,17 +1182,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { - if (isWaypointReached(&posControl.homeWaypointAbove, true)) { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + + if (isWaypointPositionReached(tmpHomePos, true)) { // Successfully reached position target - update XYZ-position - fpVector3_t pos = posControl.homeWaypointAbove.pos; - - // NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT changes the behavior of altitude controller and meaning of homeWaypointAbove.pos position - if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) { - pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude; - } - - setDesiredPosition(&pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING } else if (!validateRTHSanityChecker()) { @@ -1198,25 +1194,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) { - // Calculate required travel and actual travel distance - float rthTotalDistanceToTravel = posControl.rthInitialHomeDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0); - - // At this point `posControl.homeWaypointAbove.pos.z` is initial altitude at RTH start - // while (posControl.homePosition.pos.z + navConfig()->general.rth_altitude) is a target altitude above home point - // We will scale gradually between the two, but only if rthTotalDistanceToTravel is more than 1m (otherwise probably no point) - if (rthTotalDistanceToTravel >= 100) { - fpVector3_t pos; - - float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f); - - pos.z = (posControl.homeWaypointAbove.pos.z * ratioNotTravelled) + - ((posControl.homePosition.pos.z + navConfig()->general.rth_altitude) * (1.0f - ratioNotTravelled)); - - setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); - } - } - + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; } } @@ -1248,7 +1226,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; } else { - if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { + if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; @@ -1258,7 +1236,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } } @@ -1275,17 +1254,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout())) return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - if ((navConfig()->general.rth_home_altitude) && (posControl.desiredState.pos.z != navConfig()->general.rth_home_altitude)) { - int8_t altitudeChangeDirection = navConfig()->general.rth_home_altitude > posControl.homeWaypointAbove.pos.z ? 1 : -1; - float timeToReachHomeAltitude = altitudeChangeDirection * (navConfig()->general.rth_home_altitude - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); + + if (navConfig()->general.rth_home_altitude) { + float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate; + if (timeToReachHomeAltitude < 1) { // we almost reached the target home altitude so set the desired altitude to the desired home altitude - posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude; - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z); + 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); } } + else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } return NAV_FSM_EVENT_NONE; } @@ -1315,8 +1299,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); } else { + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt) + float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt) / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); @@ -1386,8 +1372,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_RTH: default: + posControl.rthState.rthInitialDistance = posControl.homeDistance; initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - calculateAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos); + calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS }; } @@ -1398,22 +1385,28 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na // If no position sensor available - land immediately if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { - const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH); - switch (posControl.waypointList[posControl.activeWaypointIndex].action) { case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_RTH: default: - if (isWaypointReached(&posControl.activeWaypoint, isDoingRTH) || isWaypointMissed(&posControl.activeWaypoint)) { - // Waypoint reached + if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) { return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED } else { - // Update XY-position target to active waypoint setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } break; + + case NAV_WP_ACTION_RTH: + if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { + return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED + } + else { + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z); + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms + } + break; } } /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ @@ -1659,6 +1652,49 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent) } } +static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) +{ + posControl.rthState.homeTmpWaypoint = posControl.rthState.homePosition.pos; + + switch (mode) { + case RTH_HOME_ENROUTE_INITIAL: + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; + break; + + case RTH_HOME_ENROUTE_PROPORTIONAL: + { + float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0); + if (rthTotalDistanceToTravel >= 100) { + float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f); + posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled)); + } + else { + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + } + } + break; + + case RTH_HOME_ENROUTE_FINAL: + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + break; + + case RTH_HOME_FINAL_HOVER: + if (navConfig()->general.rth_home_altitude) { + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; + } + else { + // If home altitude not defined - fall back to final ENROUTE altitude + posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; + } + break; + + case RTH_HOME_FINAL_LAND: + break; + } + + return &posControl.rthState.homeTmpWaypoint; +} + /*----------------------------------------------------------- * Float point PID-controller implementation *-----------------------------------------------------------*/ @@ -1907,22 +1943,22 @@ void updateActualHeading(bool headingValid, int32_t newHeading) */ navigationEstimateStatus_e newEstHeading = headingValid ? EST_TRUSTED : EST_NONE; if (newEstHeading >= EST_USABLE && posControl.flags.estHeadingStatus < EST_USABLE && - (posControl.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) && - (posControl.homeFlags & NAV_HOME_VALID_HEADING) == 0) { + (posControl.rthState.homeFlags & (NAV_HOME_VALID_XY | NAV_HOME_VALID_Z)) && + (posControl.rthState.homeFlags & NAV_HOME_VALID_HEADING) == 0) { // Home was stored using the fake heading (assuming boot as 0deg). Calculate // the offset from the fake to the actual yaw and apply the same rotation // to the home point. int32_t fakeToRealYawOffset = newHeading - posControl.actualState.yaw; - posControl.homePosition.yaw += fakeToRealYawOffset; - if (posControl.homePosition.yaw < 0) { - posControl.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360); + posControl.rthState.homePosition.yaw += fakeToRealYawOffset; + if (posControl.rthState.homePosition.yaw < 0) { + posControl.rthState.homePosition.yaw += DEGREES_TO_CENTIDEGREES(360); } - if (posControl.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) { - posControl.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360); + if (posControl.rthState.homePosition.yaw >= DEGREES_TO_CENTIDEGREES(360)) { + posControl.rthState.homePosition.yaw -= DEGREES_TO_CENTIDEGREES(360); } - posControl.homeFlags |= NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } posControl.actualState.yaw = newHeading; posControl.flags.estHeadingStatus = newEstHeading; @@ -2001,10 +2037,10 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint) return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees } -bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) +static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome) { // We consider waypoint reached if within specified radius - const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos); + const uint32_t wpDistance = calculateDistanceToDestination(pos); if (STATE(FIXED_WING) && isWaypointHome) { // Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check @@ -2016,28 +2052,22 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp } } +bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome) +{ + return isWaypointPositionReached(&waypoint->pos, isWaypointHome); +} + static void updateHomePositionCompatibility(void) { - geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.homePosition.pos); + geoConvertLocalToGeodetic(&GPS_home, &posControl.gpsOrigin, &posControl.rthState.homePosition.pos); GPS_distanceToHome = posControl.homeDistance / 100; GPS_directionToHome = posControl.homeDirection / 100; } -float calculateRTHAltitude(void) { - switch (navConfig()->general.flags.rth_alt_control_mode) { - case NAV_RTH_NO_ALT: - return(posControl.actualState.abs.pos.z); - case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin - return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude); - case NAV_RTH_MAX_ALT: - return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z)); - case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home - case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT: - return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z)); - case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home - default: - return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude); - } +// Backdoor for RTH estimator +float getFinalRTHAltitude(void) +{ + return posControl.rthState.rthFinalAltitude; } /*----------------------------------------------------------- @@ -2047,11 +2077,42 @@ static void updateDesiredRTHAltitude(void) { if (ARMING_FLAG(ARMED)) { if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) { - posControl.homeWaypointAbove.pos.z = calculateRTHAltitude(); + switch (navConfig()->general.flags.rth_alt_control_mode) { + case NAV_RTH_NO_ALT: + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; + break; + + case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude; + posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude; + break; + + case NAV_RTH_MAX_ALT: + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = MAX(posControl.rthState.rthFinalAltitude, posControl.actualState.abs.pos.z); + break; + + case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = MAX(posControl.rthState.rthFinalAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); + break; + + case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT: + posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.rthInitialAltitude + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z); + posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; + break; + + case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home + default: + posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; + posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude; + } } } else { - posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z; + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } } @@ -2064,7 +2125,7 @@ void initializeRTHSanityChecker(const fpVector3_t * pos) posControl.rthSanityChecker.lastCheckTime = currentTimeMs; posControl.rthSanityChecker.initialPosition = *pos; - posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); + posControl.rthSanityChecker.minimalDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); } bool validateRTHSanityChecker(void) @@ -2079,7 +2140,7 @@ bool validateRTHSanityChecker(void) // Check at 10Hz rate if ((currentTimeMs - posControl.rthSanityChecker.lastCheckTime) > 100) { - const float currentDistanceToHome = calculateDistanceToDestination(&posControl.homePosition.pos); + const float currentDistanceToHome = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); if (currentDistanceToHome < posControl.rthSanityChecker.minimalDistanceToHome) { posControl.rthSanityChecker.minimalDistanceToHome = currentDistanceToHome; @@ -2102,33 +2163,33 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t { // XY-position if ((useMask & NAV_POS_UPDATE_XY) != 0) { - posControl.homePosition.pos.x = pos->x; - posControl.homePosition.pos.y = pos->y; + posControl.rthState.homePosition.pos.x = pos->x; + posControl.rthState.homePosition.pos.y = pos->y; if (homeFlags & NAV_HOME_VALID_XY) { - posControl.homeFlags |= NAV_HOME_VALID_XY; + posControl.rthState.homeFlags |= NAV_HOME_VALID_XY; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_XY; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_XY; } } // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - posControl.homePosition.pos.z = pos->z; + posControl.rthState.homePosition.pos.z = pos->z; if (homeFlags & NAV_HOME_VALID_Z) { - posControl.homeFlags |= NAV_HOME_VALID_Z; + posControl.rthState.homeFlags |= NAV_HOME_VALID_Z; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_Z; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_Z; } } // Heading if ((useMask & NAV_POS_UPDATE_HEADING) != 0) { // Heading - posControl.homePosition.yaw = yaw; + posControl.rthState.homePosition.yaw = yaw; if (homeFlags & NAV_HOME_VALID_HEADING) { - posControl.homeFlags |= NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } else { - posControl.homeFlags &= ~NAV_HOME_VALID_HEADING; + posControl.rthState.homeFlags &= ~NAV_HOME_VALID_HEADING; } } @@ -2136,7 +2197,6 @@ void setHomePosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t posControl.homeDirection = 0; // Update target RTH altitude as a waypoint above home - posControl.homeWaypointAbove = posControl.homePosition; updateDesiredRTHAltitude(); updateHomePositionCompatibility(); @@ -2167,7 +2227,7 @@ void updateHomePosition(void) if (!ARMING_FLAG(ARMED)) { if (posControl.flags.estPosStatus >= EST_USABLE) { const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; - bool setHome = (posControl.homeFlags & validHomeFlags) != validHomeFlags; + bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { case NAV_RESET_NEVER: break; @@ -2200,8 +2260,9 @@ void updateHomePosition(void) // Update distance and direction to home if armed (home is not updated when armed) if (STATE(GPS_FIX_HOME)) { - posControl.homeDistance = calculateDistanceToDestination(&posControl.homePosition.pos); - posControl.homeDirection = calculateBearingToDestination(&posControl.homePosition.pos); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); + posControl.homeDistance = calculateDistanceToDestination(tmpHomePos); + posControl.homeDirection = calculateBearingToDestination(tmpHomePos); updateHomePositionCompatibility(); } } @@ -3239,7 +3300,7 @@ bool FAST_CODE isNavLaunchEnabled(void) int32_t navigationGetHomeHeading(void) { - return posControl.homePosition.yaw; + return posControl.rthState.homePosition.yaw; } // returns m/s diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 03f05c74be..d8e81ca16d 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -416,7 +416,7 @@ void resetWaypointList(void); bool loadNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void); -float calculateRTHAltitude(void); +float getFinalRTHAltitude(void); int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); /* Geodetic functions */ diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 8e57fdcaf0..14eaa21a3a 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -297,6 +297,23 @@ typedef struct { timeMs_t lastYawAdjustmentTime; } navCruise_t; +typedef struct { + navigationHomeFlags_t homeFlags; + navWaypointPosition_t homePosition; // Original home position and base altitude + float rthInitialAltitude; // Altitude at start of RTH + float rthFinalAltitude; // Altitude at end of RTH approach + float rthInitialDistance; // Distance when starting flight home + fpVector3_t homeTmpWaypoint; // Temporary storage for home target +} rthState_t; + +typedef enum { + RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach + RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach + RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach + RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set) + RTH_HOME_FINAL_LAND, // Home position and altitude +} rthTargetMode_e; + typedef struct { /* Flags and navigation system state */ navigationFSMState_t navState; @@ -321,11 +338,9 @@ typedef struct { /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */ rthSanityChecker_t rthSanityChecker; - navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched) - navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude - navigationHomeFlags_t homeFlags; - uint32_t rthInitialHomeDistance; // Distance to home after RTH has been initiated and the initial climb/descent is done + rthState_t rthState; + /* Home parameters */ uint32_t homeDistance; // cm int32_t homeDirection; // deg*100