From 35c267b15cc74867bb073f39423730d15d097063 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 24 Nov 2022 16:09:25 +0000 Subject: [PATCH 1/5] changes --- src/main/navigation/navigation.c | 41 ++++++++---------------- src/main/navigation/navigation_private.h | 1 - 2 files changed, 14 insertions(+), 28 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 05f4c4756c..b7a6472096 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1057,7 +1057,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { - const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + UNUSED(previousState); if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now @@ -1066,19 +1066,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration. - if (!(prevFlags & NAV_CTL_POS)) { - resetPositionController(); - } + resetPositionController(); posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow posControl.cruise.previousYaw = posControl.cruise.yaw; posControl.cruise.lastYawAdjustmentTime = 0; - return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state + return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState) { + UNUSED(previousState); + const timeMs_t currentTimeMs = millis(); if (checkForPositionSensorTimeout()) { @@ -1095,30 +1095,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile if (posControl.flags.isAdjustingHeading) { timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime; - if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. + if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); - float centidegsPerIteration = rateTarget * timeDifference * 0.001f; + float centidegsPerIteration = rateTarget * MS2S(timeDifference); posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw)); posControl.cruise.lastYawAdjustmentTime = currentTimeMs; - } - - if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) + } else if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) { posControl.cruise.previousYaw = posControl.cruise.yaw; - - uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm] - - if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING) - || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING) - || posControl.flags.isAdjustingHeading) { - calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); - DEBUG_SET(DEBUG_CRUISE, 2, 1); - } else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin - calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); - DEBUG_SET(DEBUG_CRUISE, 2, 2); } - setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY); + setDesiredPosition(NULL, posControl.cruise.yaw, NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } @@ -1137,7 +1124,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n resetPositionController(); - return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state + return NAV_FSM_EVENT_SUCCESS; // go back to NAV_STATE_COURSE_HOLD_IN_PROGRESS state } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) @@ -3744,13 +3731,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // PH has priority over COURSE_HOLD // CRUISE has priority on AH if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { - - if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; + } - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) { return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD; - + } } if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 26cc246b86..424de508ab 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -322,7 +322,6 @@ typedef struct { } rthSanityChecker_t; typedef struct { - fpVector3_t targetPos; int32_t yaw; int32_t previousYaw; timeMs_t lastYawAdjustmentTime; From 1015e19c816e332055db0870175e8984db21bbb4 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 29 Nov 2022 14:08:05 +0000 Subject: [PATCH 2/5] Add missing code --- src/main/navigation/navigation_fixedwing.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 64964a86e8..5c7c1b08e5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -394,6 +394,10 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // We have virtual position target, calculate heading error int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + virtualTargetBearing = posControl.desiredState.yaw; + } + /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { // courseVirtualCorrection initially used to determine current position relative to course line for later use From e7f9f6e1356b6dc9bec7d5187762d7f7cfdf4a7b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 19 Dec 2022 00:16:16 +0000 Subject: [PATCH 3/5] remove redundant function + update --- src/main/navigation/navigation.c | 8 -------- src/main/navigation/navigation_fixedwing.c | 10 +++++++--- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b7a6472096..803b04b014 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -255,7 +255,6 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint); 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 isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointYaw); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); @@ -2798,13 +2797,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; } -void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance) -{ - origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw)); - origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw)); - origin->z = origin->z; -} - /*----------------------------------------------------------- * NAV land detector *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 5c7c1b08e5..26f523c895 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -266,6 +266,10 @@ static int8_t loiterDirection(void) { static void calculateVirtualPositionTarget_FW(float trackingPeriod) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + return; + } + float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; @@ -391,11 +395,11 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta static bool errorIsDecreasing; static bool forceTurnDirection = false; - // We have virtual position target, calculate heading error - int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { virtualTargetBearing = posControl.desiredState.yaw; + } else { + // We have virtual position target, calculate heading error + int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ From d86ff58c94e456a693b001308a4256a892830fd3 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 19 Dec 2022 00:28:08 +0000 Subject: [PATCH 4/5] Update navigation_fixedwing.c --- src/main/navigation/navigation_fixedwing.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 26f523c895..7207a01cef 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -394,12 +394,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta static int32_t previousHeadingError; static bool errorIsDecreasing; static bool forceTurnDirection = false; + int32_t virtualTargetBearing; if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { virtualTargetBearing = posControl.desiredState.yaw; } else { // We have virtual position target, calculate heading error - int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); + virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ From 2181b9e69d9a0450ec53a3c952ad6e2e7303f694 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 13 Jan 2023 14:38:43 +0000 Subject: [PATCH 5/5] 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 19fccb2442..3b55a2414b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1102,11 +1102,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; - } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) + } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; } - setDesiredPosition(NULL, posControl.cruise.yaw, NAV_POS_UPDATE_HEADING); + setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; }