Skip to content

Commit

Permalink
Merge pull request #8519 from breadoven/abo_wp_tracking_fixes
Browse files Browse the repository at this point in the history 
Waypoint turn smoothing and tracking accuracy improvements
  • Loading branch information
@DzikuVx
DzikuVx authored Nov 19, 2022
2 parents 456a945 + 5cdaab4 commit 9f6c3f7
Show file tree
Hide file tree
Showing 2 changed files with 42 additions and 32 deletions.
5 changes: 5 additions & 0 deletions src/main/navigation/navigation.c
View file
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
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

0 comments on commit 9f6c3f7

Please sign in to comment.