Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 23:05:17 +03:00
Code Issues Wiki Activity

Merge pull request #5367 from giacomo892/navigation_cleanup_giacomo892

Browse source 
Initial NAV_WP cleanup
This commit is contained in:
giacomo892 2020-02-22 17:32:50 +01:00 • committed by GitHub
commit 7ebf950b9a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 14 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/utils.h
View file

@ -109,4 +109,6 @@ void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("me
#define FALLTHROUGH do {} while(0) #define FALLTHROUGH do {} while(0)
#endif #endif
#define UNREACHABLE() __builtin_unreachable()
#define ALIGNED(x) __attribute__ ((aligned(x))) #define ALIGNED(x) __attribute__ ((aligned(x)))

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

@ -1367,7 +1367,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
/* A helper function to do waypoint-specific action */ /* A helper function to do waypoint-specific action */
UNUSED(previousState); UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
@ -1375,12 +1375,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
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
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
default:
posControl.rthState.rthInitialDistance = posControl.homeDistance; posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
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
}; };
UNREACHABLE();
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
@ -1389,9 +1390,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately // If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
default:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) { if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
@ -1426,13 +1426,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
else { else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
} }
UNREACHABLE();
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1440,10 +1445,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else { else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
} }
default:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
} }
UNREACHABLE();
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)