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

rth trackback tweaks

Browse source
This commit is contained in:
breadoven 2024-05-14 11:42:13 +01:00
parent b96c3d4c1f
commit c9779c2dee
3 changed files with 11 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -301,10 +301,6 @@ bool validateRTHSanityChecker(void);
void updateHomePosition(void); void updateHomePosition(void);
bool abortLaunchAllowed(void); bool abortLaunchAllowed(void);
// static bool rthAltControlStickOverrideCheck(unsigned axis);
// static void updateRthTrackback(bool forceSaveTrackPoint);
// static fpVector3_t * rthGetTrackbackPos(void);
#ifdef USE_FW_AUTOLAND #ifdef USE_FW_AUTOLAND
static float getLandAltitude(void); static float getLandAltitude(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading); static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
@ -1449,7 +1445,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
} }
else { else {
// Switch to RTH trackback // Switch to RTH trackback
if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { if (rthTrackBackCanBeActivated() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference
posControl.flags.rthTrackbackActive = true; posControl.flags.rthTrackbackActive = true;
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@ -1568,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
if (rthTrackBackSetNewPosition()) { if (!rthTrackBackSetNewPosition()) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
} }

15
src/main/navigation/rth_trackback.c
View file

@ -37,9 +37,10 @@
rth_trackback_t rth_trackback; rth_trackback_t rth_trackback;
bool rthTrackBackIsActive(void) bool rthTrackBackCanBeActivated(void)
{ {
return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); return posControl.flags.estPosStatus >= EST_USABLE &&
(navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated));
} }
void rthTrackBackUpdate(bool forceSaveTrackPoint) void rthTrackBackUpdate(bool forceSaveTrackPoint)
@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint)
bool rthTrackBackSetNewPosition(void) bool rthTrackBackSetNewPosition(void)
{ {
if (posControl.flags.estPosStatus == EST_NONE) { if (posControl.flags.estPosStatus == EST_NONE) {
return false; return false; // will fall back to RTH initialize allowing full RTH to handle position loss correctly
} }
const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex])); const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void)
if (rth_trackback.activePointIndex < 0 || cancelTrackback) { if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
return true; // Procede to home after final trackback point return false; // No more trackback points to set, procede to home
} }
if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void)
setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
} }
return false; return true;
} }
fpVector3_t *getRthTrackBackPosition(void) fpVector3_t *getRthTrackBackPosition(void)
@ -174,9 +175,9 @@ fpVector3_t *getRthTrackBackPosition(void)
return &rth_trackback.pointsList[rth_trackback.activePointIndex]; return &rth_trackback.pointsList[rth_trackback.activePointIndex];
} }
void resetRthTrackBack(void) void resetRthTrackBack(void)
{ {
rth_trackback.activePointIndex = -1; rth_trackback.activePointIndex = -1;
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
rth_trackback.WrapAroundCounter = -1; rth_trackback.WrapAroundCounter = -1;
} }

2
src/main/navigation/rth_trackback.h
View file

@ -35,7 +35,7 @@ typedef struct
extern rth_trackback_t rth_trackback; extern rth_trackback_t rth_trackback;
bool rthTrackBackIsActive(void); bool rthTrackBackCanBeActivated(void);
bool rthTrackBackSetNewPosition(void); bool rthTrackBackSetNewPosition(void);
void rthTrackBackUpdate(bool forceSaveTrackPoint); void rthTrackBackUpdate(bool forceSaveTrackPoint);
fpVector3_t *getRthTrackBackPosition(void); fpVector3_t *getRthTrackBackPosition(void);