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

Waypoint storage reworked - store raw waypoint data (MWP-compliant), convert to local coordinates as required

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-01 10:35:51 +10:00
parent fd28138535
commit ce18dc94eb
4 changed files with 99 additions and 111 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -73,7 +73,7 @@ static void resetPositionController(void);
static void setupAltitudeController(void); static void setupAltitudeController(void);
void resetNavigation(void); void resetNavigation(void);
static void setDesiredPositionToWaypointAndUpdateInitialBearing(navWaypointPosition_t * waypoint); static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint);
void calculateInitialHoldPosition(t_fp_vector * pos); void calculateInitialHoldPosition(t_fp_vector * pos);
/*************************************************************************************************/ /*************************************************************************************************/
@ -829,7 +829,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
posControl.activeWaypointIndex = 0; posControl.activeWaypointIndex = 0;
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]); calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
} }
@ -843,13 +843,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
if (isWaypointReached(&posControl.waypointList[posControl.activeWaypointIndex]) || isWaypointMissed(&posControl.waypointList[posControl.activeWaypointIndex])) { if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
// Waypoint reached // Waypoint reached
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
else { else {
// Update XY-position target to active waypoint // Update XY-position target to active waypoint
setDesiredPosition(&posControl.waypointList[posControl.activeWaypointIndex].pos, posControl.waypointList[posControl.activeWaypointIndex].yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
@ -858,7 +858,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
{ {
UNUSED(previousState); UNUSED(previousState);
bool isLastWaypoint = posControl.waypointList[posControl.activeWaypointIndex].flags.isLastWaypoint || bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1)); (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) { if (isLastWaypoint) {
@ -868,7 +868,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else { else {
// Waypoint reached, do something and move on to next waypoint // Waypoint reached, do something and move on to next waypoint
posControl.activeWaypointIndex++; posControl.activeWaypointIndex++;
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]); calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
} }
@ -1168,20 +1168,14 @@ int32_t calculateBearingToDestination(t_fp_vector * destinationPos)
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Check if waypoint is/was reached * Check if waypoint is/was reached. Assume that waypoint-yaw stores initial bearing
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
bool isWaypointMissed(navWaypointPosition_t * waypoint) bool isWaypointMissed(navWaypointPosition_t * waypoint)
{ {
// We only can miss not home waypoint int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
if (waypoint->flags.isHomeWaypoint) { bearingError = wrap_18000(bearingError);
return false;
}
else {
int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
bearingError = wrap_18000(bearingError);
return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
}
} }
bool isWaypointReached(navWaypointPosition_t * waypoint) bool isWaypointReached(navWaypointPosition_t * waypoint)
@ -1242,7 +1236,6 @@ static void updateDesiredRTHAltitude(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void setHomePosition(t_fp_vector * pos, int32_t yaw) void setHomePosition(t_fp_vector * pos, int32_t yaw)
{ {
posControl.homePosition.flags.isHomeWaypoint = true;
posControl.homePosition.pos = *pos; posControl.homePosition.pos = *pos;
posControl.homePosition.yaw = yaw; posControl.homePosition.yaw = yaw;
@ -1503,76 +1496,73 @@ static void applyEmergencyLandingController(uint32_t currentTime)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* WP controller * WP controller
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * wpAlt, bool * isLastWaypoint) void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
{ {
gpsLocation_t wpLLH; /* Default waypoint to send */
wpData->action = NAV_WP_ACTION_RTH;
wpLLH.lat = 0.0f; wpData->lat = 0;
wpLLH.lon = 0.0f; wpData->lon = 0;
wpLLH.alt = 0.0f; wpData->alt = 0;
wpData->p1 = 0;
wpData->p2 = 0;
wpData->p3 = 0;
wpData->flag = NAV_WP_FLAG_LAST;
// WP #0 - special waypoint - HOME // WP #0 - special waypoint - HOME
if (wpNumber == 0) { if (wpNumber == 0) {
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {
wpLLH = GPS_home; wpData->lat = GPS_home.lat;
wpData->lon = GPS_home.lon;
wpData->alt = GPS_home.alt;
} }
*isLastWaypoint = true;
} }
// WP #255 - special waypoint - directly get actualPosition // WP #255 - special waypoint - directly get actualPosition
else if (wpNumber == 255) { else if (wpNumber == 255) {
gpsLocation_t wpLLH;
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH); geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH);
*isLastWaypoint = true;
wpData->lat = wpLLH.lat;
wpData->lon = wpLLH.lon;
wpData->alt = wpLLH.alt;
} }
// 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) && posControl.waypointListValid) {
if (wpNumber <= posControl.waypointCount) { if (wpNumber <= posControl.waypointCount) {
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.waypointList[wpNumber - 1].pos, &wpLLH); *wpData = posControl.waypointList[wpNumber - 1];
*isLastWaypoint = posControl.waypointList[wpNumber - 1].flags.isLastWaypoint;
}
else {
wpLLH.lat = 0;
wpLLH.lon = 0;
wpLLH.alt = 0;
*isLastWaypoint = false;
} }
} }
*wpLat = wpLLH.lat;
*wpLon = wpLLH.lon;
*wpAlt = wpLLH.alt;
} }
void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt, bool isLastWaypoint) void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
{ {
gpsLocation_t wpLLH; gpsLocation_t wpLLH;
navWaypointPosition_t wpPos; navWaypointPosition_t wpPos;
// Ignore mission updates if position estimator is not ready yet // Pre-fill structure to convert to local coordinates
if (posControl.flags.hasValidPositionSensor) wpLLH.lat = wpData->lat;
return; wpLLH.lon = wpData->lon;
wpLLH.alt = wpData->alt;
// Convert to local coordinates
wpLLH.lat = wpLat;
wpLLH.lon = wpLon;
wpLLH.alt = wpAlt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos);
wpPos.yaw = 0; // FIXME
// WP #0 - special waypoint - HOME // WP #0 - special waypoint - HOME
if ((wpNumber == 0) && ARMING_FLAG(ARMED)) { if ((wpNumber == 0) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid) {
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
setHomePosition(&wpPos.pos, wpPos.yaw); geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos);
setHomePosition(&wpPos.pos, 0);
} }
// WP #255 - special waypoint - directly set desiredPosition // WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode // Only valid when armed and in poshold mode
else if ((wpNumber == 255) && ARMING_FLAG(ARMED) && (posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { else if ((wpNumber == 255) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid &&
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
// Converto to local coordinates
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos);
// If close to actualPos, use heading, if far - use bearing // If close to actualPos, use heading, if far - use bearing
uint32_t wpDistance = calculateDistanceToDestination(&wpPos.pos); uint32_t wpDistance = calculateDistanceToDestination(&wpPos.pos);
navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY; navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
// If we received global altitude == 0, use current altitude // If we received global altitude == 0, use current altitude
if (wpAlt != 0) { if (wpData->alt != 0) {
waypointUpdateFlags |= NAV_POS_UPDATE_Z; waypointUpdateFlags |= NAV_POS_UPDATE_Z;
} }
@ -1587,25 +1577,27 @@ 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)) {
/* Sanity check - can set waypoints only sequentially - one by one */ posControl.waypointList[wpNumber - 1] = *wpData;
if (wpNumber <= (posControl.waypointCount + 1)) { // condition is true if wpIndex is at most 1 record ahead the waypointCount posControl.waypointCount = wpNumber;
wpPos.flags.isHomeWaypoint = false; posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
wpPos.flags.isLastWaypoint = isLastWaypoint;
posControl.waypointList[wpNumber - 1] = wpPos;
posControl.waypointCount = wpNumber;
posControl.waypointListValid = isLastWaypoint;
}
} }
} }
static void setDesiredPositionToWaypointAndUpdateInitialBearing(navWaypointPosition_t * waypoint) static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint)
{ {
gpsLocation_t wpLLH;
wpLLH.lat = waypoint->lat;
wpLLH.lon = waypoint->lon;
wpLLH.alt = waypoint->alt;
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &posControl.activeWaypoint.pos);
// Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints) // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
waypoint->yaw = calculateBearingToDestination(&waypoint->pos); posControl.activeWaypoint.yaw = calculateBearingToDestination(&posControl.activeWaypoint.pos);
// Set desired position to next waypoint (XYZ-controller) // Set desired position to next waypoint (XYZ-controller)
setDesiredPosition(&waypoint->pos, waypoint->yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
} }
/** /**
@ -1616,11 +1608,11 @@ bool isApproachingLastWaypoint(void)
{ {
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount == 0) { if (posControl.waypointCount == 0) {
/* No waypoints, holding current position */ /* No waypoints */
return true; return true;
} }
else if (posControl.activeWaypointIndex == (posControl.waypointCount - 1) || else if ((posControl.activeWaypointIndex == (posControl.waypointCount - 1)) ||
posControl.waypointList[posControl.activeWaypointIndex].flags.isLastWaypoint) { (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)) {
return true; return true;
} }
else { else {

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

@ -130,13 +130,22 @@ typedef struct gpsOrigin_s {
int32_t alt; // Altitude in centimeters (meters * 100) int32_t alt; // Altitude in centimeters (meters * 100)
} gpsOrigin_s; } gpsOrigin_s;
#define NAV_WP_FLAG_LAST 0xA5
#define NAV_WP_ACTION_WAYPOINT 0x01
#define NAV_WP_ACTION_RTH 0x04
typedef struct { typedef struct {
struct { uint8_t action;
bool isHomeWaypoint; // for home waypoint yaw is set to "launch heading", for common waypoints - to "initial bearing" int32_t lat;
bool isLastWaypoint; int32_t lon;
} flags; int32_t alt;
t_fp_vector pos; int16_t p1, p2, p3;
int32_t yaw; // deg * 100 uint8_t flag;
} navWaypoint_t;
typedef struct {
t_fp_vector pos;
int32_t yaw; // deg * 100
} navWaypointPosition_t; } navWaypointPosition_t;
#if defined(NAV) #if defined(NAV)
@ -168,8 +177,8 @@ bool naivationBlockArming(void);
float getEstimatedActualVelocity(int axis); 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, navWaypoint_t * wpData);
void setWaypoint(uint8_t wpNumber, int32_t wpLat, int32_t wpLon, int32_t wpAlt, bool isLastWaypoint); void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
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);

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

@ -242,10 +242,12 @@ typedef struct {
int32_t homeDirection; // deg*100 int32_t homeDirection; // deg*100
/* Waypoint list */ /* Waypoint list */
navWaypointPosition_t waypointList[NAV_MAX_WAYPOINTS]; navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
int8_t waypointCount;
int8_t activeWaypointIndex;
bool waypointListValid; bool waypointListValid;
int8_t waypointCount;
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex;
/* Internals */ /* Internals */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];

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

@ -716,16 +716,8 @@ static bool processOutCommand(uint8_t cmdMSP)
uint32_t i, tmp, junk; uint32_t i, tmp, junk;
#ifdef GPS #ifdef GPS
struct { int8_t msp_wp_no;
uint8_t wp_no; navWaypoint_t msp_wp;
uint8_t action;
int32_t lat;
int32_t lon;
int32_t alt;
int16_t p1, p2, p3;
uint8_t flag;
} msp_wp;
bool msp_wp_last;
#endif #endif
switch (cmdMSP) { switch (cmdMSP) {
@ -1083,18 +1075,18 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(GPS_update & 1); serialize8(GPS_update & 1);
break; break;
case MSP_WP: case MSP_WP:
msp_wp.wp_no = read8(); // get the wp number msp_wp_no = read8(); // get the wp number
getWaypoint(msp_wp.wp_no, &msp_wp.lat, &msp_wp.lon, &msp_wp.alt, &msp_wp_last); getWaypoint(msp_wp_no, &msp_wp);
headSerialReply(21); headSerialReply(21);
serialize8(msp_wp.wp_no); // wp_no serialize8(msp_wp_no); // wp_no
serialize8(1); // action (WAYPOINT) serialize8(msp_wp.action); // action (WAYPOINT)
serialize32(msp_wp.lat); // lat serialize32(msp_wp.lat); // lat
serialize32(msp_wp.lon); // lon serialize32(msp_wp.lon); // lon
serialize32(msp_wp.alt); // altitude (cm) serialize32(msp_wp.alt); // altitude (cm)
serialize16(0); // P1 serialize16(msp_wp.p1); // P1
serialize16(0); // P2 serialize16(msp_wp.p2); // P2
serialize16(0); // P3 serialize16(msp_wp.p3); // P3
serialize8(msp_wp_last ? 0xA5 : 00); // flags serialize8(msp_wp.flag); // flags
break; break;
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4)); headSerialReply(1 + (GPS_numCh * 4));
@ -1294,15 +1286,8 @@ static bool processInCommand(void)
uint8_t rate; uint8_t rate;
#ifdef GPS #ifdef GPS
struct { uint8_t msp_wp_no;
uint8_t wp_no; navWaypoint_t msp_wp;
uint8_t action;
int32_t lat;
int32_t lon;
int32_t alt;
int16_t p1, p2, p3;
uint8_t flag;
} msp_wp;
#endif #endif
switch (currentPort->cmdMSP) { switch (currentPort->cmdMSP) {
@ -1559,7 +1544,7 @@ static bool processInCommand(void)
onNewGPSData(GPS_coord[LAT], GPS_coord[LON], GPS_altitude, 0, 0, 0, false, false, 9999); onNewGPSData(GPS_coord[LAT], GPS_coord[LON], GPS_altitude, 0, 0, 0, false, false, 9999);
break; break;
case MSP_SET_WP: case MSP_SET_WP:
msp_wp.wp_no = read8(); // get the wp number msp_wp_no = read8(); // get the wp number
msp_wp.action = read8(); // action msp_wp.action = read8(); // action
msp_wp.lat = read32(); // lat msp_wp.lat = read32(); // lat
msp_wp.lon = read32(); // lon msp_wp.lon = read32(); // lon
@ -1568,8 +1553,8 @@ static bool processInCommand(void)
msp_wp.p2 = read16(); // P2 msp_wp.p2 = read16(); // P2
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 == NAV_WP_ACTION_WAYPOINT) { // support only WAYPOINT types
setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt, (msp_wp.flag == 0xA5)); setWaypoint(msp_wp_no, &msp_wp);
} }
break; break;
#endif #endif