Skip to content

Waypoint turn smoothing and tracking accuracy improvements #8519

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
DzikuVx merged 3 commits into from
Nov 19, 2022
Merged

Waypoint turn smoothing and tracking accuracy improvements #8519

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1b3013b
initial build
breadoven Nov 2, 2022
235e4d9
Update navigation_fixedwing.c
breadoven Nov 2, 2022
5cdaab4
Update navigation_fixedwing.c
breadoven Nov 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
69 changes: 37 additions & 32 deletions src/main/navigation/navigation_fixedwing.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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);
}
Expand Down