mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Waypoint storage reworked - store raw waypoint data (MWP-compliant), convert to local coordinates as required
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
fd28138535
commit
ce18dc94eb
4 changed files with 99 additions and 111 deletions
|
|
@ -73,7 +73,7 @@ static void resetPositionController(void);
|
|||
static void setupAltitudeController(void);
|
||||
void resetNavigation(void);
|
||||
|
||||
static void setDesiredPositionToWaypointAndUpdateInitialBearing(navWaypointPosition_t * waypoint);
|
||||
static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint);
|
||||
void calculateInitialHoldPosition(t_fp_vector * pos);
|
||||
|
||||
/*************************************************************************************************/
|
||||
|
|
@ -829,7 +829,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
|
||||
posControl.activeWaypointIndex = 0;
|
||||
|
||||
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
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;
|
||||
}
|
||||
|
||||
if (isWaypointReached(&posControl.waypointList[posControl.activeWaypointIndex]) || isWaypointMissed(&posControl.waypointList[posControl.activeWaypointIndex])) {
|
||||
if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
|
||||
// Waypoint reached
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
else {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -858,7 +858,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
|
|||
{
|
||||
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));
|
||||
|
||||
if (isLastWaypoint) {
|
||||
|
|
@ -868,7 +868,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
|
|||
else {
|
||||
// Waypoint reached, do something and move on to next waypoint
|
||||
posControl.activeWaypointIndex++;
|
||||
setDesiredPositionToWaypointAndUpdateInitialBearing(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
|
||||
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)
|
||||
{
|
||||
// We only can miss not home waypoint
|
||||
if (waypoint->flags.isHomeWaypoint) {
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
int32_t bearingError = calculateBearingToDestination(&waypoint->pos) - waypoint->yaw;
|
||||
bearingError = wrap_18000(bearingError);
|
||||
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)
|
||||
|
|
@ -1242,7 +1236,6 @@ static void updateDesiredRTHAltitude(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void setHomePosition(t_fp_vector * pos, int32_t yaw)
|
||||
{
|
||||
posControl.homePosition.flags.isHomeWaypoint = true;
|
||||
posControl.homePosition.pos = *pos;
|
||||
posControl.homePosition.yaw = yaw;
|
||||
|
||||
|
|
@ -1503,76 +1496,73 @@ static void applyEmergencyLandingController(uint32_t currentTime)
|
|||
/*-----------------------------------------------------------
|
||||
* 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;
|
||||
|
||||
wpLLH.lat = 0.0f;
|
||||
wpLLH.lon = 0.0f;
|
||||
wpLLH.alt = 0.0f;
|
||||
/* Default waypoint to send */
|
||||
wpData->action = NAV_WP_ACTION_RTH;
|
||||
wpData->lat = 0;
|
||||
wpData->lon = 0;
|
||||
wpData->alt = 0;
|
||||
wpData->p1 = 0;
|
||||
wpData->p2 = 0;
|
||||
wpData->p3 = 0;
|
||||
wpData->flag = NAV_WP_FLAG_LAST;
|
||||
|
||||
// WP #0 - special waypoint - HOME
|
||||
if (wpNumber == 0) {
|
||||
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
|
||||
else if (wpNumber == 255) {
|
||||
gpsLocation_t 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
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && posControl.waypointListValid) {
|
||||
if (wpNumber <= posControl.waypointCount) {
|
||||
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;
|
||||
*wpData = posControl.waypointList[wpNumber - 1];
|
||||
}
|
||||
}
|
||||
|
||||
*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;
|
||||
navWaypointPosition_t wpPos;
|
||||
|
||||
// Ignore mission updates if position estimator is not ready yet
|
||||
if (posControl.flags.hasValidPositionSensor)
|
||||
return;
|
||||
|
||||
// Convert to local coordinates
|
||||
wpLLH.lat = wpLat;
|
||||
wpLLH.lon = wpLon;
|
||||
wpLLH.alt = wpAlt;
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos);
|
||||
wpPos.yaw = 0; // FIXME
|
||||
// Pre-fill structure to convert to local coordinates
|
||||
wpLLH.lat = wpData->lat;
|
||||
wpLLH.lon = wpData->lon;
|
||||
wpLLH.alt = wpData->alt;
|
||||
|
||||
// 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
|
||||
setHomePosition(&wpPos.pos, wpPos.yaw);
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos);
|
||||
setHomePosition(&wpPos.pos, 0);
|
||||
}
|
||||
// WP #255 - special waypoint - directly set desiredPosition
|
||||
// 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
|
||||
uint32_t wpDistance = calculateDistanceToDestination(&wpPos.pos);
|
||||
navSetWaypointFlags_t waypointUpdateFlags = NAV_POS_UPDATE_XY;
|
||||
|
||||
// If we received global altitude == 0, use current altitude
|
||||
if (wpAlt != 0) {
|
||||
if (wpData->alt != 0) {
|
||||
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
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
|
||||
/* Sanity check - can set waypoints only sequentially - one by one */
|
||||
if (wpNumber <= (posControl.waypointCount + 1)) { // condition is true if wpIndex is at most 1 record ahead the waypointCount
|
||||
wpPos.flags.isHomeWaypoint = false;
|
||||
wpPos.flags.isLastWaypoint = isLastWaypoint;
|
||||
|
||||
posControl.waypointList[wpNumber - 1] = wpPos;
|
||||
posControl.waypointCount = wpNumber;
|
||||
posControl.waypointListValid = isLastWaypoint;
|
||||
}
|
||||
posControl.waypointList[wpNumber - 1] = *wpData;
|
||||
posControl.waypointCount = wpNumber;
|
||||
posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
waypoint->yaw = calculateBearingToDestination(&waypoint->pos);
|
||||
posControl.activeWaypoint.yaw = calculateBearingToDestination(&posControl.activeWaypoint.pos);
|
||||
|
||||
// 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 (posControl.waypointCount == 0) {
|
||||
/* No waypoints, holding current position */
|
||||
/* No waypoints */
|
||||
return true;
|
||||
}
|
||||
else if (posControl.activeWaypointIndex == (posControl.waypointCount - 1) ||
|
||||
posControl.waypointList[posControl.activeWaypointIndex].flags.isLastWaypoint) {
|
||||
else if ((posControl.activeWaypointIndex == (posControl.waypointCount - 1)) ||
|
||||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
|
|
|||
|
|
@ -130,13 +130,22 @@ typedef struct gpsOrigin_s {
|
|||
int32_t alt; // Altitude in centimeters (meters * 100)
|
||||
} gpsOrigin_s;
|
||||
|
||||
#define NAV_WP_FLAG_LAST 0xA5
|
||||
#define NAV_WP_ACTION_WAYPOINT 0x01
|
||||
#define NAV_WP_ACTION_RTH 0x04
|
||||
|
||||
typedef struct {
|
||||
struct {
|
||||
bool isHomeWaypoint; // for home waypoint yaw is set to "launch heading", for common waypoints - to "initial bearing"
|
||||
bool isLastWaypoint;
|
||||
} flags;
|
||||
t_fp_vector pos;
|
||||
int32_t yaw; // deg * 100
|
||||
uint8_t action;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int32_t alt;
|
||||
int16_t p1, p2, p3;
|
||||
uint8_t flag;
|
||||
} navWaypoint_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector pos;
|
||||
int32_t yaw; // deg * 100
|
||||
} navWaypointPosition_t;
|
||||
|
||||
#if defined(NAV)
|
||||
|
|
@ -168,8 +177,8 @@ bool naivationBlockArming(void);
|
|||
float getEstimatedActualVelocity(int axis);
|
||||
float getEstimatedActualPosition(int axis);
|
||||
|
||||
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, bool isLastWaypoint);
|
||||
void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
|
||||
void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
|
||||
|
||||
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos);
|
||||
void geoConvertLocalToGeodetic(gpsOrigin_s * origin, t_fp_vector * pos, gpsLocation_t * llh);
|
||||
|
|
|
|||
|
|
@ -242,10 +242,12 @@ typedef struct {
|
|||
int32_t homeDirection; // deg*100
|
||||
|
||||
/* Waypoint list */
|
||||
navWaypointPosition_t waypointList[NAV_MAX_WAYPOINTS];
|
||||
int8_t waypointCount;
|
||||
int8_t activeWaypointIndex;
|
||||
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
|
||||
bool waypointListValid;
|
||||
int8_t waypointCount;
|
||||
|
||||
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
|
||||
int8_t activeWaypointIndex;
|
||||
|
||||
/* Internals */
|
||||
int16_t rcAdjustment[4];
|
||||
|
|
|
|||
|
|
@ -716,16 +716,8 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
uint32_t i, tmp, junk;
|
||||
|
||||
#ifdef GPS
|
||||
struct {
|
||||
uint8_t wp_no;
|
||||
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;
|
||||
int8_t msp_wp_no;
|
||||
navWaypoint_t msp_wp;
|
||||
#endif
|
||||
|
||||
switch (cmdMSP) {
|
||||
|
|
@ -1083,18 +1075,18 @@ static bool processOutCommand(uint8_t cmdMSP)
|
|||
serialize8(GPS_update & 1);
|
||||
break;
|
||||
case MSP_WP:
|
||||
msp_wp.wp_no = read8(); // get the wp number
|
||||
getWaypoint(msp_wp.wp_no, &msp_wp.lat, &msp_wp.lon, &msp_wp.alt, &msp_wp_last);
|
||||
msp_wp_no = read8(); // get the wp number
|
||||
getWaypoint(msp_wp_no, &msp_wp);
|
||||
headSerialReply(21);
|
||||
serialize8(msp_wp.wp_no); // wp_no
|
||||
serialize8(1); // action (WAYPOINT)
|
||||
serialize8(msp_wp_no); // wp_no
|
||||
serialize8(msp_wp.action); // action (WAYPOINT)
|
||||
serialize32(msp_wp.lat); // lat
|
||||
serialize32(msp_wp.lon); // lon
|
||||
serialize32(msp_wp.alt); // altitude (cm)
|
||||
serialize16(0); // P1
|
||||
serialize16(0); // P2
|
||||
serialize16(0); // P3
|
||||
serialize8(msp_wp_last ? 0xA5 : 00); // flags
|
||||
serialize16(msp_wp.p1); // P1
|
||||
serialize16(msp_wp.p2); // P2
|
||||
serialize16(msp_wp.p3); // P3
|
||||
serialize8(msp_wp.flag); // flags
|
||||
break;
|
||||
case MSP_GPSSVINFO:
|
||||
headSerialReply(1 + (GPS_numCh * 4));
|
||||
|
|
@ -1294,15 +1286,8 @@ static bool processInCommand(void)
|
|||
uint8_t rate;
|
||||
|
||||
#ifdef GPS
|
||||
struct {
|
||||
uint8_t wp_no;
|
||||
uint8_t action;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int32_t alt;
|
||||
int16_t p1, p2, p3;
|
||||
uint8_t flag;
|
||||
} msp_wp;
|
||||
uint8_t msp_wp_no;
|
||||
navWaypoint_t msp_wp;
|
||||
#endif
|
||||
|
||||
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);
|
||||
break;
|
||||
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.lat = read32(); // lat
|
||||
msp_wp.lon = read32(); // lon
|
||||
|
|
@ -1568,8 +1553,8 @@ static bool processInCommand(void)
|
|||
msp_wp.p2 = read16(); // P2
|
||||
msp_wp.p3 = read16(); // P3
|
||||
msp_wp.flag = read8(); // future: to set nav flag
|
||||
if (msp_wp.action == 1) { // support only WAYPOINT types
|
||||
setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt, (msp_wp.flag == 0xA5));
|
||||
if (msp_wp.action == NAV_WP_ACTION_WAYPOINT) { // support only WAYPOINT types
|
||||
setWaypoint(msp_wp_no, &msp_wp);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue