Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 08:45:31 +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()) { if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference updateRthTrackback(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true; posControl.flags.rthTrackbackActive = true;
posControl.wpReached = false;
calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos()); calculateAndSetActiveWaypointToLocalPosition(rthGetTrackbackPos());
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK; 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.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false; posControl.wpAltitudeReached = false;
posControl.wpReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS 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) // 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) { if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
// Check if WP reached when turn smoothing used // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
// Otherwise 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
if (navConfig()->fw.waypoint_turn_smoothing && posControl.activeWaypoint.bearingToNextWp != -1) { uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 4500 : 10000;
return posControl.wpReached; if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > relativeBearing) {
} else if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointYaw)) > 10000) {
return true; 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 * Loiter centered on point inside turn at navLoiterRadius distance from waypoint and
* on a bearing midway between current and next waypoint course bearings * 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 */ * 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 && if (navConfig()->fw.waypoint_turn_smoothing && isWaypointNavTrackingRoute() && !needToCalculateCircularLoiter &&
posControl.activeWaypoint.bearingToNextWp != -1) { posControl.activeWaypoint.bearingToNextWp != -1) {
int32_t turnAngle = wrap_18000(posControl.activeWaypoint.bearingToNextWp - posControl.activeWaypoint.yaw); 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; posErrorX = loiterCenterPos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterCenterPos.y - navGetCurrentActualPositionAndVelocity()->pos.y; 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 // turn direction to next waypoint
turnDirection = turnAngle > 0 ? 1 : -1; // 1 = right 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 resetLandingDetector;
bool rthTrackbackActive; // Activation status of RTH trackback bool rthTrackbackActive; // Activation status of RTH trackback
bool wpTurnSmoothingActive; // Activation status WP turn smoothing
} navigationFlags_t; } navigationFlags_t;
typedef struct { typedef struct {
@ -401,7 +403,6 @@ typedef struct {
float wpInitialAltitude; // Altitude at start of WP float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP float wpDistance; // Distance to active WP
bool wpReached; // WP reached when using turn smoothing
timeMs_t wpReachedTime; // Time the waypoint was reached timeMs_t wpReachedTime; // Time the waypoint was reached
bool wpAltitudeReached; // WP altitude achieved bool wpAltitudeReached; // WP altitude achieved