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

Preliminary MSP waypoint protocol compatibility. Only WAYPOINT action is supported for now

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-27 11:33:40 +10:00
parent 3f16205f39
commit f5ac6a5514
3 changed files with 15 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1413,7 +1413,7 @@ static void applyEmergencyLandingController(uint32_t currentTime)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* WP controller * WP controller
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * wpAlt) void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * wpAlt, bool * isLastWaypoint)
{ {
gpsLocation_t wpLLH; gpsLocation_t wpLLH;
@ -1426,16 +1426,21 @@ void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * w
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {
wpLLH = GPS_home; wpLLH = GPS_home;
} }
*isLastWaypoint = true;
} }
// WP #255 - special waypoint - directly get actualPosition // WP #255 - special waypoint - directly get actualPosition
else if (wpNumber == 255) { else if (wpNumber == 255) {
geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH); geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH);
*isLastWaypoint = true;
} }
// 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)) {
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 = (wpNumber >= posControl.waypointCount);
} }
*wpLat = wpLLH.lat; *wpLat = wpLLH.lat;
@ -1488,8 +1493,9 @@ 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; 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) { if (wpIndex <= posControl.waypointCount) { // 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; posControl.waypointList[wpIndex] = wpPos;
posControl.waypointCount = wpIndex + 1; posControl.waypointCount = wpIndex + 1;

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

@ -167,7 +167,7 @@ int8_t naivationGetHeadingControlState(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); 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);
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos); void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos);

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

@ -724,6 +724,7 @@ static bool processOutCommand(uint8_t cmdMSP)
int16_t p1, p2, p3; int16_t p1, p2, p3;
uint8_t flag; uint8_t flag;
} msp_wp; } msp_wp;
bool msp_wp_last;
#endif #endif
switch (cmdMSP) { switch (cmdMSP) {
@ -1082,7 +1083,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break; break;
case MSP_WP: case MSP_WP:
msp_wp.wp_no = read8(); // get the wp number msp_wp.wp_no = read8(); // get the wp number
getWaypoint(msp_wp.wp_no, &msp_wp.lat, &msp_wp.lon, &msp_wp.alt); getWaypoint(msp_wp.wp_no, &msp_wp.lat, &msp_wp.lon, &msp_wp.alt, &msp_wp_last);
headSerialReply(21); headSerialReply(21);
serialize8(msp_wp.wp_no); // wp_no serialize8(msp_wp.wp_no); // wp_no
serialize8(1); // action (WAYPOINT) serialize8(1); // action (WAYPOINT)
@ -1092,7 +1093,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(0); // P1 serialize16(0); // P1
serialize16(0); // P2 serialize16(0); // P2
serialize16(0); // P3 serialize16(0); // P3
serialize8(0); // flags serialize8(msp_wp_last ? 0xA5 : 00); // flags
break; break;
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4)); headSerialReply(1 + (GPS_numCh * 4));
@ -1566,7 +1567,9 @@ 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
setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt); if (msp_wp.action == 1) { // support only WAYPOINT types
setWaypoint(msp_wp.wp_no, msp_wp.lat, msp_wp.lon, msp_wp.alt);
}
break; break;
#endif #endif
case MSP_SET_FEATURE: case MSP_SET_FEATURE: