Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RTH trackback code cleanup #10042

Merged
merged 1 commit into from
May 15, 2024
Merged

RTH trackback code cleanup #10042

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 2 additions & 6 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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;
}

Expand Down
15 changes: 8 additions & 7 deletions src/main/navigation/rth_trackback.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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]));
Expand All @@ -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)) {
Expand All @@ -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)
Expand All @@ -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 changes: 1 addition & 1 deletion src/main/navigation/rth_trackback.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading