diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 08d19abf78..3b55a2414b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -257,7 +257,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 bearing, int32_t distance); -void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance); static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing); bool isWaypointAltitudeReached(void); static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); @@ -1059,7 +1058,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 @@ -1068,19 +1067,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.course = posControl.actualState.cog; // Store the course to follow posControl.cruise.previousCourse = posControl.cruise.course; posControl.cruise.lastCourseAdjustmentTime = 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()) { @@ -1097,30 +1096,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.lastCourseAdjustmentTime; - 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.course = wrap_36000(posControl.cruise.course - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); posControl.cruise.lastCourseAdjustmentTime = currentTimeMs; - } - - if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) + } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) { posControl.cruise.previousCourse = posControl.cruise.course; - - 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.course, 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.course, distance); - DEBUG_SET(DEBUG_CRUISE, 2, 2); } - setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.course, NAV_POS_UPDATE_XY); + setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); return NAV_FSM_EVENT_NONE; } @@ -1139,7 +1125,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) @@ -2812,13 +2798,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z; } -void calculateNewCruiseTarget(fpVector3_t * origin, int32_t course, int32_t distance) -{ - origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(course)); - origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(course)); - origin->z = origin->z; -} - /*----------------------------------------------------------- * NAV land detector *-----------------------------------------------------------*/ @@ -3755,13 +3734,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_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 40340e511e..99fccb1e10 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -267,6 +267,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,9 +395,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta static int32_t previousHeadingError; static bool errorIsDecreasing; static bool forceTurnDirection = false; + int32_t virtualTargetBearing; - // 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 + virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); + } /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index dd99ce12b5..44fc1796fd 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -323,7 +323,6 @@ typedef struct { } rthSanityChecker_t; typedef struct { - fpVector3_t targetPos; int32_t course; int32_t previousCourse; timeMs_t lastCourseAdjustmentTime;