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

fix wp reached logic

Browse source
This commit is contained in:
breadoven 2022-07-20 11:59:42 +01:00
parent 81fb470f4e
commit fd125bb017
3 changed files with 8 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -289,6 +289,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
* Loiter centered on point inside turn at navLoiterRadius distance from waypoint and
* on a bearing midway between current and next waypoint course bearings
* Only for turns > 30 degs, navLoiterRadius factored down between 30 to 60 degs to align with course line */
posControl.flags.wpTurnSmoothingActive = false;
if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter &&
posControl.activeWaypoint.bearingToNextWp != -1) {
int32_t turnAngle = wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw);
@ -301,14 +302,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
// if waypoint not reached based on distance from waypoint then waypoint considered
// reached if difference between actual heading and waypoint bearing exceeds 90 degs
posControl.wpReached = ABS(wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw)) > 9000;
// turn direction to next waypoint
turnDirection = turnAngle > 0 ? 1 : -1; // 1 = right
needToCalculateCircularLoiter = true;
needToCalculateCircularLoiter = posControl.flags.wpTurnSmoothingActive = true;
}
}