Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15: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
*-----------------------------------------------------------*/
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;

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

@ -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);

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

@ -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: