Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-18 22:05:15 +03:00
Code Issues Wiki Activity

Update navigation_fixedwing.c

Browse source
This commit is contained in:
breadoven 2022-11-02 19:53:04 +00:00
parent 1b3013b570
commit 235e4d90c3
1 changed files with 22 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/navigation/navigation_fixedwing.c
View file

@ -396,34 +396,33 @@ 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 waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// apply course tracking correction if target bearing error < 90 degs or when close to waypoint (within 10m) // courseVirtualCorrection initially used to determine current position relative to course line for later use
if (ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) { int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
// courseVirtualCorrection initially used to determine current position relative to course line for later use float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); // tracking only active when target bearing error < 90 degs, close to waypoint (within 10m) and > 2m from course line
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); int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
// only apply tracking correction when > 2m from waypoint course line // captureFactor adjusts distance/heading sensitivity balance when approaching course line.
if (distToCourseLine > 200) { // Approach distance threashold based on speed and an assumed 1 second response time.
// captureFactor adjusts distance/heading sensitivity balance when approaching course line. float captureFactor = distToCourseLine < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
// approach 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(captureFactor * distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// course heading alignment factor // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f); // initial courseCorrectionFactor based on distance to course line
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; float courseCorrectionFactor = constrainf(captureFactor * distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// final courseCorrectionFactor combining distance and heading factors // course heading alignment factor
courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f);
courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor;
// final courseVirtualCorrection using max 80 deg heading adjustment toward course line // final courseCorrectionFactor combining distance and heading factors
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f);
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
} // final courseVirtualCorrection value
courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor;
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.yaw - courseVirtualCorrection);
} }
} }