From d16fa16572463c43dfcacaa5d292ea09fa4b9e68 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 1/4] first build --- src/main/navigation/navigation.c | 44 ++++++-------------- src/main/navigation/navigation_fixedwing.c | 7 ++-- src/main/navigation/navigation_multicopter.c | 2 +- src/main/navigation/navigation_private.h | 4 +- 4 files changed, 18 insertions(+), 39 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a3302f3ab5..8d8ca4f283 100644 --- a/src/main/navigation/navigation.c +++ b/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) || - (posControl.activeWaypointIndex >= (posControl.waypointCount - 1)); - - if (isLastWaypoint) { - // Last waypoint reached + if (isLastMissionWaypoint()) { // 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); } -/** - * 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) +/* Checks if active waypoint is last in mission */ +bool isLastMissionWaypoint(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; - } + 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 00439e6d51..1f4795775a 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 cf8491c2af..0335e50188 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 c20b0fbef6..7d7007b232 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); From 207d72c16182cbe46468b2544fb3c5cd502e2787 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 29 Nov 2021 23:52:07 +0000 Subject: [PATCH 2/4] Update navigation.c --- src/main/navigation/navigation.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 8d8ca4f283..ed30063a71 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3068,8 +3068,8 @@ bool isApproachingHoldPosition(void) 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); + // RTH spiral climb and Home positions and POSHOLD position + return FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_POSHOLD_MODE); } float getActiveWaypointSpeed(void) From e7a894b2942828c5a1599f7ebf0a2f496be5762f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 3 Dec 2021 15:48:28 +0000 Subject: [PATCH 3/4] Fix slowDownForTurning bug --- src/main/navigation/navigation_multicopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 0335e50188..a8e71f7448 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 && - !isLastMissionWaypoint() && + !isApproachingHoldPosition() && newVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning ) || newVelTotal > maxSpeed From ff3cfdf70453518a01f977bc3d670fa7f218af41 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 4 Dec 2021 12:06:46 +0000 Subject: [PATCH 4/4] improve function definition --- src/main/navigation/navigation.c | 4 ++-- src/main/navigation/navigation_fixedwing.c | 2 +- src/main/navigation/navigation_multicopter.c | 2 +- src/main/navigation/navigation_private.h | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ed30063a71..ef432c42dc 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3062,8 +3062,8 @@ bool isLastMissionWaypoint(void) (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)); } -/* Checks if approaching hold position requiring fixed wing circling loiter */ -bool isApproachingHoldPosition(void) +/* Checks if Nav hold position is active */ +bool isNavHoldPositionActive(void) { 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; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 1f4795775a..ea142fb06b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -277,7 +277,7 @@ 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 = isApproachingHoldPosition() && + bool needToCalculateCircularLoiter = isNavHoldPositionActive() && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a8e71f7448..f7798894e1 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 && - !isApproachingHoldPosition() && + !isNavHoldPositionActive() && newVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning ) || newVelTotal > maxSpeed diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 7d7007b232..3cac06a17b 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -418,7 +418,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome); bool isWaypointMissed(const navWaypointPosition_t * waypoint); -bool isApproachingHoldPosition(void); +bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); float getActiveWaypointSpeed(void);