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);
bool abortLaunchAllowed(void);
// static bool rthAltControlStickOverrideCheck(unsigned axis);
// static void updateRthTrackback(bool forceSaveTrackPoint);
// static fpVector3_t * rthGetTrackbackPos(void);
#ifdef USE_FW_AUTOLAND
static float getLandAltitude(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
@ -1449,7 +1445,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
}
else {
// 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
posControl.flags.rthTrackbackActive = true;
calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@ -1568,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (rthTrackBackSetNewPosition()) {
if (!rthTrackBackSetNewPosition()) {
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;
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)
@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint)
bool rthTrackBackSetNewPosition(void)
{
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]));
@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void)
if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
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)) {
@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void)
setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
return false;
return true;
}
fpVector3_t *getRthTrackBackPosition(void)
@ -174,9 +175,9 @@ fpVector3_t *getRthTrackBackPosition(void)
return &rth_trackback.pointsList[rth_trackback.activePointIndex];
}
void resetRthTrackBack(void)
void resetRthTrackBack(void)
{
rth_trackback.activePointIndex = -1;
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;
bool rthTrackBackIsActive(void);
bool rthTrackBackCanBeActivated(void);
bool rthTrackBackSetNewPosition(void);
void rthTrackBackUpdate(bool forceSaveTrackPoint);
fpVector3_t *getRthTrackBackPosition(void);