first build

src/main/navigation/navigation.c

@@ -1642,11 +1642,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio
 {
     UNUSED(previousState);
 
-    const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
+    if (isLastMissionWaypoint()) {      // Last waypoint reached
-                          (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
-
-    if (isLastWaypoint) {
-        // Last waypoint reached
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
     }
     else {
@@ -3059,37 +3055,21 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
     calculateAndSetActiveWaypointToLocalPosition(&localPos);
 }
 
-/**
+/* Checks if active waypoint is last in mission */
- * Returns TRUE if we are in WP mode and executing last waypoint on the list, or in RTH mode, or in PH mode
+bool isLastMissionWaypoint(void)
- *  In RTH mode our only and last waypoint is home
- *  In PH mode our waypoint is hold position */
-bool isApproachingLastWaypoint(void)
 {
-    if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
+    return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
-        if (posControl.waypointCount == 0) {
+            (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
-            /* No waypoints */
-            return true;
-        }
-        else if ((posControl.activeWaypointIndex == (posControl.waypointCount - 1)) ||
-                 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)) {
-            return true;
-        }
-        else {
-            return false;
-        }
-    }
-    else if (navGetStateFlags(posControl.navState) & NAV_CTL_POS) {
-        // If POS controller is active we are in Poshold or RTH mode - assume last waypoint
-        return true;
-    }
-    else {
-        return false;
-    }
 }
 
-bool isWaypointWait(void)
+/* Checks if approaching hold position requiring fixed wing circling loiter */
+bool isApproachingHoldPosition(void)
 {
-    return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
+    if (FLIGHT_MODE(NAV_WP_MODE)) {     // WP mode last WP hold and Timed hold positions
+        return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
+    }
+    // RTH spiral climb and Home positions and POSHOLD (Course Hold excluded, no loiter required)
+    return (navGetCurrentStateFlags() & NAV_CTL_POS) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
 }
 
 float getActiveWaypointSpeed(void)

src/main/navigation/navigation_fixedwing.c

@@ -277,10 +277,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 
     // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
     #define TAN_15DEG    0.26795f
-    bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
+    bool needToCalculateCircularLoiter = isApproachingHoldPosition() &&
-                                            && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
+                                            (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
-                                            && (distanceToActualTarget > 50.0f)
+                                            (distanceToActualTarget > 50.0f);
-                                            && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
 
     // Calculate virtual position for straight movement
     if (needToCalculateCircularLoiter) {

src/main/navigation/navigation_multicopter.c

@@ -446,7 +446,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
      */
     if (
         (navGetCurrentStateFlags() & NAV_AUTO_WP &&
-        !isApproachingLastWaypoint() &&
+        !isLastMissionWaypoint() &&
         newVelTotal < maxSpeed &&
         !navConfig()->mc.slowDownForTurning
         ) || newVelTotal > maxSpeed

src/main/navigation/navigation_private.h

@@ -418,8 +418,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
 
 bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
 bool isWaypointMissed(const navWaypointPosition_t * waypoint);
-bool isWaypointWait(void);
+bool isApproachingHoldPosition(void);
-bool isApproachingLastWaypoint(void);
+bool isLastMissionWaypoint(void);
 float getActiveWaypointSpeed(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading);