Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +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

11
src/main/navigation/navigation.c
View file

@ -1203,7 +1203,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true;
posControl.wpReached = false;
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK;
}
@ -1570,7 +1569,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
posControl.wpReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
// We use p3 as the volatile jump counter (p2 is the static value)
@ -2258,11 +2256,10 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
}
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
// Check if WP reached when turn smoothing used
// Otherwise check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
if (navConfig()->fw.waypoint_turn_smoothing && posControl.activeWaypoint.bearingToNextWp != -1) {
return posControl.wpReached;
} else if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
// 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 45 degrees
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 4500 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > relativeBearing) {
return true;
}
}

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;
}
}

3
src/main/navigation/navigation_private.h
View file

@ -112,6 +112,8 @@ typedef struct navigationFlags_s {
bool resetLandingDetector;
bool rthTrackbackActive; // Activation status of RTH trackback
bool wpTurnSmoothingActive; // Activation status WP turn smoothing
} navigationFlags_t;
typedef struct {
@ -401,7 +403,6 @@ typedef struct {
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP
bool wpReached; // WP reached when using turn smoothing
timeMs_t wpReachedTime; // Time the waypoint was reached
bool wpAltitudeReached; // WP altitude achieved