From 2b2d7171f20ffc63e4f401ce9453cb4079a0bb38 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 29 Nov 2021 23:30:08 +0000
Subject: [PATCH] first build

---
 src/main/navigation/navigation.c             | 46 ++++++++--------------------
 src/main/navigation/navigation_fixedwing.c   |  7 ++---
 src/main/navigation/navigation_multicopter.c |  2 +-
 src/main/navigation/navigation_private.h     |  4 +--
 4 files changed, 19 insertions(+), 40 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 02eca8d09..a2bc4c993 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1646,11 +1646,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio
 {
     UNUSED(previousState);
 
-    const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
-                          (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
-
-    if (isLastWaypoint) {
-        // Last waypoint reached
+    if (isLastMissionWaypoint()) {      // Last waypoint reached
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
     }
     else {
@@ -3079,37 +3075,21 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
     calculateAndSetActiveWaypointToLocalPosition(&localPos);
 }
 
-/**
- * Returns TRUE if we are in WP mode and executing last waypoint on the list, or in RTH mode, or in PH mode
- *  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) {
-        if (posControl.waypointCount == 0) {
-            /* 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;
-    }
+/* Checks if active waypoint is last in mission */
+bool isLastMissionWaypoint(void)
+{
+    return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
+            (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
 }
 
-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)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 27c5a4792..48a3dd20e 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/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())
-                                            && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
-                                            && (distanceToActualTarget > 50.0f)
-                                            && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
+    bool needToCalculateCircularLoiter = isApproachingHoldPosition() &&
+                                            (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
+                                            (distanceToActualTarget > 50.0f);
 
     // Calculate virtual position for straight movement
     if (needToCalculateCircularLoiter) {
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 2f5028716..a5238afe9 100755
--- a/src/main/navigation/navigation_multicopter.c
+++ b/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
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 23e61927c..8243fe7fd 100644
--- a/src/main/navigation/navigation_private.h
+++ b/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 isApproachingLastWaypoint(void);
+bool isApproachingHoldPosition(void);
+bool isLastMissionWaypoint(void);
 float getActiveWaypointSpeed(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading);
-- 
2.11.4.GIT