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

Fix last waypoint issue

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-30 10:49:21 +10:00
parent 936df953eb
commit 43de844aca
4 changed files with 35 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

53
src/main/flight/navigation_rewrite.c
View file

@ -755,18 +755,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
{ {
UNUSED(previousState); UNUSED(previousState);
// Prepare controllers if (posControl.waypointCount == 0 || !posControl.waypointListValid) {
resetPositionController(); return NAV_FSM_EVENT_ERROR;
resetAltitudeController();
setupAltitudeController();
if (posControl.waypointCount == 0) {
// No waypoints defined, enter PH and end sequence
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} }
else { else {
// Prepare controllers
resetPositionController();
resetAltitudeController();
setupAltitudeController();
posControl.activeWaypointIndex = 0; posControl.activeWaypointIndex = 0;
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]); setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
@ -796,14 +795,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
{ {
UNUSED(previousState); UNUSED(previousState);
// Waypoint reached, do something and move on to next waypoint bool isLastWaypoint = posControl.waypointList[posControl.activeWaypointIndex].flags.isLastWaypoint ||
posControl.activeWaypointIndex++; (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (posControl.activeWaypointIndex >= posControl.waypointCount) { if (isLastWaypoint) {
// This is the last waypoint - finalize // This is the last waypoint - finalize
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} }
else { else {
// Waypoint reached, do something and move on to next waypoint
posControl.activeWaypointIndex++;
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]); setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
@ -1454,9 +1455,14 @@ void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * w
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount) { if (wpNumber <= posControl.waypointCount) {
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.waypointList[wpNumber - 1].pos, &wpLLH); geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.waypointList[wpNumber - 1].pos, &wpLLH);
*isLastWaypoint = posControl.waypointList[wpNumber - 1].flags.isLastWaypoint;
}
else {
wpLLH.lat = 0;
wpLLH.lon = 0;
wpLLH.alt = 0;
*isLastWaypoint = false;
} }
*isLastWaypoint = (wpNumber >= posControl.waypointCount);
} }
*wpLat = wpLLH.lat; *wpLat = wpLLH.lat;
@ -1464,7 +1470,7 @@ void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * w
*wpAlt = wpLLH.alt; *wpAlt = wpLLH.alt;
} }
void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt) void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt, bool isLastWaypoint)
{ {
gpsLocation_t wpLLH; gpsLocation_t wpLLH;
navWaypointPosition_t wpPos; navWaypointPosition_t wpPos;
@ -1508,13 +1514,14 @@ void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt)
} }
// WP #1 - #15 - common waypoints - pre-programmed mission // WP #1 - #15 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
uint8_t wpIndex = wpNumber - 1;
/* Sanity check - can set waypoints only sequentially - one by one */ /* Sanity check - can set waypoints only sequentially - one by one */
if (wpIndex <= posControl.waypointCount) { // condition is true if wpIndex is at most 1 record ahead the waypointCount if (wpNumber <= (posControl.waypointCount + 1)) { // condition is true if wpIndex is at most 1 record ahead the waypointCount
wpPos.flags.isHomeWaypoint = false; wpPos.flags.isHomeWaypoint = false;
posControl.waypointList[wpIndex] = wpPos; wpPos.flags.isLastWaypoint = isLastWaypoint;
posControl.waypointCount = wpIndex + 1;
posControl.waypointList[wpNumber - 1] = wpPos;
posControl.waypointCount = wpNumber;
posControl.waypointListValid = isLastWaypoint;
} }
} }
} }
@ -1539,7 +1546,8 @@ bool isApproachingLastWaypoint(void)
/* No waypoints, holding current position */ /* No waypoints, holding current position */
return true; return true;
} }
else if (posControl.activeWaypointIndex == (posControl.waypointCount - 1)) { else if (posControl.activeWaypointIndex == (posControl.waypointCount - 1) ||
posControl.waypointList[posControl.activeWaypointIndex].flags.isLastWaypoint) {
return true; return true;
} }
else { else {
@ -1693,7 +1701,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
} }
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) { if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if (canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) if (canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid)
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
} }
@ -1873,6 +1881,7 @@ void navigationInit(navConfig_t *initialnavConfig,
posControl.flags.forcedRTHActivated = 0; posControl.flags.forcedRTHActivated = 0;
posControl.waypointCount = 0; posControl.waypointCount = 0;
posControl.activeWaypointIndex = 0; posControl.activeWaypointIndex = 0;
posControl.waypointListValid = false;
/* Set initial surface invalid */ /* Set initial surface invalid */
posControl.actualState.surface = -1.0f; posControl.actualState.surface = -1.0f;

3
src/main/flight/navigation_rewrite.h
View file

@ -133,6 +133,7 @@ typedef struct gpsOrigin_s {
typedef struct { typedef struct {
struct { struct {
bool isHomeWaypoint; // for home waypoint yaw is set to "launch heading", for common waypoints - to "initial bearing" bool isHomeWaypoint; // for home waypoint yaw is set to "launch heading", for common waypoints - to "initial bearing"
bool isLastWaypoint;
} flags; } flags;
t_fp_vector pos; t_fp_vector pos;
int32_t yaw; // deg * 100 int32_t yaw; // deg * 100
@ -168,7 +169,7 @@ float getEstimatedActualVelocity(int axis);
float getEstimatedActualPosition(int axis); float getEstimatedActualPosition(int axis);
void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * wpAlt, bool * isLastWaypoint); void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * wpAlt, bool * isLastWaypoint);
void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt); void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt, bool isLastWaypoint);
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos); void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos);
void geoConvertLocalToGeodetic(gpsOrigin_s * origin, t_fp_vector * pos, gpsLocation_t * llh); void geoConvertLocalToGeodetic(gpsOrigin_s * origin, t_fp_vector * pos, gpsLocation_t * llh);

1
src/main/flight/navigation_rewrite_private.h
View file

@ -238,6 +238,7 @@ typedef struct {
navWaypointPosition_t waypointList[NAV_MAX_WAYPOINTS]; navWaypointPosition_t waypointList[NAV_MAX_WAYPOINTS];
int8_t waypointCount; int8_t waypointCount;
int8_t activeWaypointIndex; int8_t activeWaypointIndex;
bool waypointListValid;
/* Internals */ /* Internals */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];

2
src/main/io/serial_msp.c
View file

@ -1569,7 +1569,7 @@ static bool processInCommand(void)
msp_wp.p3 = read16(); // P3 msp_wp.p3 = read16(); // P3
msp_wp.flag = read8(); // future: to set nav flag msp_wp.flag = read8(); // future: to set nav flag
if (msp_wp.action == 1) { // support only WAYPOINT types if (msp_wp.action == 1) { // support only WAYPOINT types
setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt); setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt, (msp_wp.flag == 0xA5));
} }
break; break;
#endif #endif