diff --git a/docs/Settings.md b/docs/Settings.md index 8375448f59..511299a63f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3282,13 +3282,13 @@ Sets a reference distance to the waypoint course line that affects the accuracy --- -### nav_fw_wp_turn_smoothing_dist +### nav_fw_wp_turn_smoothing -Provides a means of smoothing waypoint turns by reaching waypoints earlier as turns become tighter. Applied as an increasing multiple of nav_fw_wp_turn_smoothing_dist [cm] starting at 1 for turns exceeding 45 degrees to a maximum of 10 for turns of 90 degrees and over, e.g. a 70 degree turn is reached when 6 x nav_fw_wp_turn_smoothing_dist from waypoint. 200 is a good starting point. Set to 0 to disable turn smoothing. +Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 2000 | +| OFF | OFF | ON | --- diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 47ffb6eed6..b582ab7f87 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -199,7 +199,7 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] = OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX), #endif OSD_SETTING_ENTRY("MISSION RESTART", SETTING_NAV_WP_MISSION_RESTART), - OSD_SETTING_ENTRY("TURN SMOOTHING DIST", SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST), + OSD_SETTING_ENTRY("WP TURN SMOOTHING", SETTING_NAV_FW_WP_TURN_SMOOTHING), OSD_SETTING_ENTRY("WP TRACKING ACCURACY", SETTING_NAV_FW_WP_TRACKING_ACCURACY), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f2c0aa6752..a926f1e7ef 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2426,12 +2426,11 @@ groups: field: fw.waypoint_tracking_accuracy min: 0 max: 100 - - name: nav_fw_wp_turn_smoothing_dist - description: "Provides a means of smoothing waypoint turns by reaching waypoints earlier as turns become tighter. Applied as an increasing multiple of nav_fw_wp_turn_smoothing_dist [cm] starting at 1 for turns exceeding 45 degrees to a maximum of 10 for turns of 90 degrees and over, e.g. a 70 degree turn is reached when 6 x nav_fw_wp_turn_smoothing_dist from waypoint. 200 is a good starting point. Set to 0 to disable turn smoothing." - default_value: 0 - field: fw.wp_turn_smoothing_dist - min: 0 - max: 2000 + - name: nav_fw_wp_turn_smoothing + description: "Smooths turns during WP missions by switching to a loiter path when within the nav_fw_loiter_radius from the waypoint. The loiter is centered relative to the waypoint such that the craft passes through the waypoint during the loiter at which point the waypoint is considered to have been reached." + default_value: OFF + field: fw.waypoint_turn_smoothing + type: bool - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" default_value: 300 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cfec8f88d2..de33e9ecfd 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -209,8 +209,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT, .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled .auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled - .waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0 m - .wp_turn_smoothing_dist = SETTING_NAV_FW_WP_TURN_SMOOTHING_DIST_DEFAULT, // 0 cm + .waypoint_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0 m, improves course tracking during FW WP missions + .waypoint_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // OFF, smooths turns during FW WP mode missions } ); @@ -1203,6 +1203,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) { updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference posControl.flags.rthTrackbackActive = true; + posControl.wpReached = false; calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK; } @@ -1569,6 +1570,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; + posControl.wpReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS // We use p3 as the volatile jump counter (p2 is the static value) @@ -2213,7 +2215,7 @@ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3 return true; } -static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) +bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) { // Only for WP Mode not Trackback. Ignore non geo waypoints except RTH and JUMP. if (FLIGHT_MODE(NAV_WP_MODE) && !isLastMissionWaypoint()) { @@ -2255,25 +2257,16 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w return true; } - uint16_t turnEarlyDistance = 0; - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { + if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) { // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypointYaw - if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) { + // Also check if WP reached when turn smoothing used + if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000 || posControl.wpReached) { + posControl.wpReached = false; return true; } - - // fixed wing option to reach waypoint earlier to help smooth tighter turns to next waypoint. - // factor applied from 1 to 10 for turns > 60 to 105 degs and over - if (navConfig()->fw.wp_turn_smoothing_dist && STATE(AIRPLANE) && posControl.activeWaypointIndex > 0) { - fpVector3_t nextWpPos; - if (getLocalPosNextWaypoint(&nextWpPos)) { - int32_t bearingToNextWP = ABS(wrap_18000(calculateBearingBetweenLocalPositions(waypointPos, &nextWpPos) - *waypointYaw)); - turnEarlyDistance = navConfig()->fw.wp_turn_smoothing_dist * constrain((bearingToNextWP - 4000) / 500, 0, 10); - } - } } - return posControl.wpDistance <= (navConfig()->general.waypoint_radius + turnEarlyDistance); + return posControl.wpDistance <= (navConfig()->general.waypoint_radius); } bool isWaypointAltitudeReached(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 356ee6723a..766c9a6241 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -308,7 +308,7 @@ typedef struct navConfig_s { uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg) uint16_t auto_disarm_delay; // fixed wing disarm delay for landing detector uint8_t waypoint_tracking_accuracy; // fixed wing tracking distance accuracy [m] - uint16_t wp_turn_smoothing_dist; // distance increment to smooth turns during fixed wing waypoint missions [cm] + uint8_t waypoint_turn_smoothing; // Smooths turns during WP missions } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index cd61757202..76c8555c63 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -275,20 +275,48 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); uint32_t navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius); + fpVector3_t loiterCenterPos = posControl.desiredState.pos; + int8_t turnDirection = loiterDirection(); - // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target + // Detemine if a circular loiter is required. + // For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target #define TAN_15DEG 0.26795f needToCalculateCircularLoiter = isNavHoldPositionActive() && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); - // Calculate virtual position for straight movement - if (needToCalculateCircularLoiter) { - // We are closing in on a waypoint, calculate circular loiter - float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f); + /* WP turn smoothing option - switch to loiter path when distance to waypoint < navLoiterRadius. + * Loiter centered on point inside turn at navLoiterRadius distance from waypoint and + * on a bearing midway between current and next waypoint course bearings */ + if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter) { + if (posControl.wpDistance < navLoiterRadius) { + fpVector3_t posNextWp; + if (getLocalPosNextWaypoint(&posNextWp)) { + int32_t bearingNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, &posNextWp); + int32_t loiterCenterBearing = wrap_36000(((wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw - 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; - float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle); - float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle); + // turn direction to next waypoint + turnDirection = wrap_18000(bearingNextWp - posControl.activeWaypoint.yaw) > 0 ? 1 : -1; // 1 = right + + // if waypoint not reached based on distance from waypoint then waypoint considered + // reached if difference between actual heading and waypoint bearing exceeds 90 degs + posControl.wpReached = ABS(wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw)) > 9000; + + needToCalculateCircularLoiter = true; + } + } + } + + // We are closing in on a waypoint, calculate circular loiter if required + if (needToCalculateCircularLoiter) { + float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(turnDirection * 45.0f); + float loiterTargetX = loiterCenterPos.x + navLoiterRadius * cos_approx(loiterAngle); + float loiterTargetY = loiterCenterPos.y + navLoiterRadius * sin_approx(loiterAngle); // We have temporary loiter target. Recalculate distance and position error posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x; @@ -358,8 +386,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta // We have virtual position target, calculate heading error int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); - /* If waypoint tracking enabled force craft toward waypoint course line - * and hold within set accuracy distance from line */ + /* If waypoint tracking enabled quickly force craft toward waypoint course line and track along it */ if (navConfig()->fw.waypoint_tracking_accuracy && isWaypointNavTrackingRoute() && !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) { @@ -370,16 +397,20 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.yaw)); float distToCourseLine = calculateDistanceToDestination(&virtualCoursePoint); + // courseVirtualCorrection initially used to determine current position relative to course line for later use int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing); - int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw); + // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy float courseCorrectionFactor = constrainf(distToCourseLine / (100.0f * navConfig()->fw.waypoint_tracking_accuracy), 0.0f, 1.0f); courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor; + int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw); float courseHeadingFactor = constrainf(sq(courseHeadingError / 18000.0f), 0.0f, 1.0f); courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); + + // final courseVirtualCorrection using max 80 deg heading adjustment toward course line courseVirtualCorrection = 8000 * courseCorrectionFactor; virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 7de65e6ec5..753f2b5cea 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -401,6 +401,7 @@ typedef struct { float wpInitialAltitude; // Altitude at start of WP float wpInitialDistance; // Distance when starting flight to WP float wpDistance; // Distance to active WP + bool wpReached; // WP reached when using turn smoothing timeMs_t wpReachedTime; // Time the waypoint was reached bool wpAltitudeReached; // WP altitude achieved @@ -454,6 +455,8 @@ bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); float getActiveWaypointSpeed(void); bool isWaypointNavTrackingRoute(void); +bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos); +int32_t calculateBearingBetweenLocalPositions(const fpVector3_t * startPos, const fpVector3_t * endPos); void updateActualHeading(bool headingValid, int32_t newHeading); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);