From f5ac6a5514ce2a1286d3418c99e50494af7dc46a Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 27 Nov 2015 11:33:40 +1000 Subject: [PATCH] Preliminary MSP waypoint protocol compatibility. Only WAYPOINT action is supported for now --- src/main/flight/navigation_rewrite.c | 10 ++++++++-- src/main/flight/navigation_rewrite.h | 2 +- src/main/io/serial_msp.c | 9 ++++++--- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 6cca289fbc..b683b1708d 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -1413,7 +1413,7 @@ static void applyEmergencyLandingController(uint32_t currentTime) /*----------------------------------------------------------- * 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; @@ -1426,16 +1426,21 @@ void getWaypoint(uint8_t wpNumber, int32_t * wpLat, int32_t * wpLon, int32_t * w if (STATE(GPS_FIX_HOME)) { wpLLH = GPS_home; } + + *isLastWaypoint = true; } // WP #255 - special waypoint - directly get actualPosition else if (wpNumber == 255) { geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.actualState.pos, &wpLLH); + *isLastWaypoint = true; } // WP #1 - #15 - common waypoints - pre-programmed mission else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { if (wpNumber <= posControl.waypointCount) { geoConvertLocalToGeodetic(&posControl.gpsOrigin, &posControl.waypointList[wpNumber - 1].pos, &wpLLH); } + + *isLastWaypoint = (wpNumber >= posControl.waypointCount); } *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 else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { uint8_t wpIndex = wpNumber - 1; + /* 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; posControl.waypointList[wpIndex] = wpPos; posControl.waypointCount = wpIndex + 1; diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index 5f7f5865fc..a8b3a5091f 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -167,7 +167,7 @@ int8_t naivationGetHeadingControlState(void); float getEstimatedActualVelocity(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 geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 72f9d41cfb..3ed5674616 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -724,6 +724,7 @@ static bool processOutCommand(uint8_t cmdMSP) int16_t p1, p2, p3; uint8_t flag; } msp_wp; + bool msp_wp_last; #endif switch (cmdMSP) { @@ -1082,7 +1083,7 @@ static bool processOutCommand(uint8_t cmdMSP) 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); + getWaypoint(msp_wp.wp_no, &msp_wp.lat, &msp_wp.lon, &msp_wp.alt, &msp_wp_last); headSerialReply(21); serialize8(msp_wp.wp_no); // wp_no serialize8(1); // action (WAYPOINT) @@ -1092,7 +1093,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(0); // P1 serialize16(0); // P2 serialize16(0); // P3 - serialize8(0); // flags + serialize8(msp_wp_last ? 0xA5 : 00); // flags break; case MSP_GPSSVINFO: headSerialReply(1 + (GPS_numCh * 4)); @@ -1566,7 +1567,9 @@ static bool processInCommand(void) msp_wp.p2 = read16(); // P2 msp_wp.p3 = read16(); // P3 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; #endif case MSP_SET_FEATURE: