diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 562f2621041..305ec50127c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2263,6 +2263,11 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w } if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) { + // If WP turn smoothing CUT option used WP is reached when start of turn is initiated + if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { + posControl.flags.wpTurnSmoothingActive = false; + return true; + } // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw // Same method for turn smoothing option but relative bearing set at 60 degrees uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e0e0bd36f8b..64964a86e88 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -285,38 +285,38 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); - /* WP turn smoothing option - switches to loiter path around waypoint using navLoiterRadius. + /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP + * Works for turns > 30 degs and < 160 degs. + * Option 1 switches to loiter path around waypoint using navLoiterRadius. * Loiter centered on point inside turn at required distance from waypoint and * on a bearing midway between current and next waypoint course bearings. - * Works for turns > 30 degs and < 120 degs. - * 2 options, 1 = pass through WP, 2 = cut inside turn missing WP */ + * Option 2 simply uses a normal turn once the turn initiation point is reached */ int32_t waypointTurnAngle = posControl.activeWaypoint.nextTurnAngle == -1 ? -1 : ABS(posControl.activeWaypoint.nextTurnAngle); posControl.flags.wpTurnSmoothingActive = false; - if (waypointTurnAngle > 3000 && waypointTurnAngle < 12000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // turnFactor adjusts start of loiter based on turn angle - float turnFactor = 0.0f; + if (waypointTurnAngle > 3000 && waypointTurnAngle < 16000 && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { + // turnStartFactor adjusts start of loiter based on turn angle + float turnStartFactor; if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { // passes through WP - turnFactor = waypointTurnAngle / 6000.0f; - } else { - turnFactor = tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); // cut inside turn missing WP + turnStartFactor = waypointTurnAngle / 6000.0f; + } else { // // cut inside turn missing WP + turnStartFactor = constrainf(tan_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)), 1.0f, 2.0f); } - constrainf(turnFactor, 0.5f, 2.0f); - if (posControl.wpDistance < navLoiterRadius * turnFactor) { - int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000); - float distToTurnCentre = navLoiterRadius; - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { - distToTurnCentre = navLoiterRadius / cos_approx(CENTIDEGREES_TO_RADIANS(waypointTurnAngle / 2.0f)); - } - loiterCenterPos.x = posControl.activeWaypoint.pos.x + distToTurnCentre * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - loiterCenterPos.y = posControl.activeWaypoint.pos.y + distToTurnCentre * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); + // velXY provides additional turn initiation distance based on an assumed 1 second delayed turn response time + if (posControl.wpDistance < (posControl.actualState.velXY + navLoiterRadius * turnStartFactor)) { + if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_ON) { + int32_t loiterCenterBearing = wrap_36000(((wrap_18000(posControl.activeWaypoint.nextTurnAngle - 18000)) / 2) + posControl.activeWaypoint.yaw + 18000); + loiterCenterPos.x = posControl.activeWaypoint.pos.x + navLoiterRadius * cos_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); + loiterCenterPos.y = posControl.activeWaypoint.pos.y + navLoiterRadius * sin_approx(CENTIDEGREES_TO_RADIANS(loiterCenterBearing)); - posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; - posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; + posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x; + posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; - // turn direction to next waypoint - loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right + // turn direction to next waypoint + loiterTurnDirection = posControl.activeWaypoint.nextTurnAngle > 0 ? 1 : -1; // 1 = right - needToCalculateCircularLoiter = posControl.flags.wpTurnSmoothingActive = true; + needToCalculateCircularLoiter = true; + } + posControl.flags.wpTurnSmoothingActive = true; } } @@ -396,26 +396,31 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // only apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m) - if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) { - // courseVirtualCorrection initially used to determine current position relative to course line for later use - int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing); - float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); + // courseVirtualCorrection initially used to determine current position relative to course line for later use + int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing); + float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); + + // tracking only active when certain distance and heading conditions are met + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && distToCourseLine > 200) { + int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw); + + // captureFactor adjusts distance/heading sensitivity balance when closing in on course line. + // Closing distance threashold based on speed and an assumed 1 second response time. + float captureFactor = distToCourseLine < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f; // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy // initial courseCorrectionFactor based on distance to course line - float courseCorrectionFactor = constrainf(distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f); + float courseCorrectionFactor = constrainf(captureFactor * distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f); courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor; // course heading alignment factor - int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw); - float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f); + float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f); courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; // final courseCorrectionFactor combining distance and heading factors courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); - // final courseVirtualCorrection using max 80 deg heading adjustment toward course line + // final courseVirtualCorrection value courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection); }