Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Update navigation_fixedwing.c

Browse source
This commit is contained in:
breadoven 2022-11-02 20:45:01 +00:00
parent 235e4d90c3
commit 5cdaab4c28
1 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -400,12 +400,12 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing); int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); 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 // 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) { 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);
// captureFactor adjusts distance/heading sensitivity balance when approaching course line. // captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
// Approach distance threashold based on speed and an assumed 1 second response time. // 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; 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 // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy