Skip to content

Commit

Permalink
Merge pull request #10278 from breadoven/abo_wp_tracking_improvements
Browse files Browse the repository at this point in the history 
Waypoint tracking improvements
  • Loading branch information
@breadoven
breadoven authored Sep 4, 2024
2 parents 6a27be8 + 570b301 commit 376b35d
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 33 deletions.
2 changes: 1 addition & 1 deletion docs/Settings.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3524,7 +3524,7 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within

### nav_fw_wp_tracking_accuracy

Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable.
Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy.

| Default | Min | Max |
| --- | --- | --- |
Expand Down
4 changes: 2 additions & 2 deletions src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ tables:
values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
"AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
"ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"]
- name: aux_operator
values: ["OR", "AND"]
Expand Down Expand Up @@ -2545,7 +2545,7 @@ groups:
min: 1
max: 9
- name: nav_fw_wp_tracking_accuracy
description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable."
description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy."
default_value: 0
field: fw.wp_tracking_accuracy
min: 0
Expand Down
74 changes: 44 additions & 30 deletions src/main/navigation/navigation_fixedwing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -399,38 +399,52 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
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) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
navCrossTrackError = 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.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog);

// 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 = navCrossTrackError < 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(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;

// course heading alignment factor
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 value
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection);
if (isWaypointNavTrackingActive()) {
/* Calculate cross track error */
posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);

fpVector3_t virtualCoursePoint;
virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);

/* If waypoint tracking enabled force craft toward and closely track along waypoint course line */
if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) {
static float crossTrackErrorRate;
static timeUs_t previousCrossTrackErrorUpdateTime;
static float previousCrossTrackError = 0.0f;
static pt1Filter_t fwCrossTrackErrorRateFilterState;

if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
}
crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
previousCrossTrackErrorUpdateTime = currentTimeUs;
previousCrossTrackError = navCrossTrackError;
}

uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);

if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);

/* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);

/* Calculate final adjusted virtualTargetBearing */
uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
}
}
}

/*
* Calculate NAV heading error
* Units are centidegrees
Expand Down

0 comments on commit 376b35d

Please sign in to comment.