[gps_nmea] attempt to fix nmea parser at least a little bit

* fix gps.course
* fix gps.lla_pos
* fix altitude: it is in mm
* compute gps.ecef_pos for rotorcrafts
* added subsystem for rotorcrafts

conf/autopilot/subsystems/fixedwing/gps_nmea.makefile

@@ -3,7 +3,7 @@
 # NMEA GPS unit
 
 
-ap.CFLAGS += -DUSE_GPS -DNMEA -DGPS_USE_LATLONG
+ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
 ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
 ap.CFLAGS += -DUSE_$(GPS_PORT)
 ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)

conf/autopilot/subsystems/rotorcraft/gps_nmea.makefile

@@ -0,0 +1,23 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# NMEA GPS unit
+
+
+ap.CFLAGS += -DUSE_GPS
+ap.CFLAGS += -DGPS_LINK=$(GPS_PORT)
+ap.CFLAGS += -DUSE_$(GPS_PORT)
+ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
+
+ifneq ($(GPS_LED),none)
+  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
+ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+

sw/airborne/subsystems/gps/gps_nmea.c

@@ -22,10 +22,8 @@
  */
 
 /**
- * file gps_nmea.c
- * brief Parser for the NMEA protocol
- *
- * This file is a drop-in replacement for gps_ubx.c
+ * @file gps_nmea.c
+ * Parser for the NMEA protocol.
  *
  * TODO: THIS NMEA-PARSER IS NOT WELL TESTED AND INCOMPLETE!!!
  * Status:
@@ -37,7 +35,10 @@
 
 #include "led.h"
 
+#if GPS_USE_LATLONG
+/* currently needed to get nav_utm_zone0 */
 #include "subsystems/navigation/common_nav.h"
+#endif
 #include "math/pprz_geodetic_float.h"
 
 #include <inttypes.h>
@@ -92,8 +93,8 @@ void parse_nmea_GPGSA(void) {
   // get 2D/3D-fix
   // set gps_mode=3=3d, 2=2d, 1=no fix or 0
   gps.fix = atoi(&gps_nmea.msg_buf[i]);
-  if (gps.fix == 1)
-    gps.fix = 0;
+  //if (gps.fix == 1)
+  //  gps.fix = 0;
   NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix);
   while(gps_nmea.msg_buf[i++] != ',') {              // next field:satellite-number-0
     if (i >= gps_nmea.msg_len) {
@@ -181,7 +182,7 @@ void parse_nmea_GPRMC(void) {
     }
   }
   double course = strtod(&gps_nmea.msg_buf[i], &endptr);
-  gps.course=course*10; //FIXME should be GPS heading in rad*1e7
+  gps.course = RadOfDeg(course) * 1e7;
   NMEA_PRINT("COURSE: %d \n\r",gps_course);
 }
 
@@ -222,7 +223,7 @@ void parse_nmea_GPGGA(void) {
   lat = degrees + (minutesfrac*100)/60;
   // convert to radians
   //GpsInfo.PosLLA.lat.f *= (M_PI/180);
-  lla_f.lat = RadOfDeg(lat);
+
   while(gps_nmea.msg_buf[i++] != ',') {              // next field: N/S indicator
     if (i >= gps_nmea.msg_len) {
       NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
@@ -233,17 +234,19 @@ void parse_nmea_GPGGA(void) {
   // correct latitute for N/S
   if(gps_nmea.msg_buf[i] == 'S')
     lat = -lat;
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: longitude
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
 
   // convert to radians
   lla_f.lat = RadOfDeg(lat);
 
-  gps.lla_pos.lat =lla_f.lat * 1e7; // convert to fixed-point
+  gps.lla_pos.lat = lla_f.lat * 1e7; // convert to fixed-point
   NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), lla_f.lat);
 
+
+  while(gps_nmea.msg_buf[i++] != ',') {              // next field: longitude
+    if (i >= gps_nmea.msg_len)
+      return;
+  }
+
   // get longitude [ddmm.mmmmm]
   double lon = strtod(&gps_nmea.msg_buf[i], &endptr);
   // convert to pure degrees [dd.dddd] format
@@ -259,35 +262,29 @@ void parse_nmea_GPGGA(void) {
   // correct latitute for E/W
   if(gps_nmea.msg_buf[i] == 'W')
     lon = -lon;
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: position fix status
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
+
+  // convert to radians
+  lla_f.lon = RadOfDeg(lon);
 
   gps.lla_pos.lon = lla_f.lon * 1e7; // convert to fixed-point
   NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), lla_f.lon, gps.tow);
 
-  /* convert to utm */
-  struct UtmCoor_f utm_f;
-  utm_f.zone = nav_utm_zone0;
-  utm_of_lla_f(&utm_f, &lla_f);
-
-  /* copy results of utm conversion */
-  gps.utm_pos.east = utm_f.east*100;
-  gps.utm_pos.north = utm_f.north*100;
-  gps.utm_pos.alt = utm_f.alt*1000;
-  gps.utm_pos.zone = nav_utm_zone0;
-
 
+  while(gps_nmea.msg_buf[i++] != ',') {              // next field: position fix status
+    if (i >= gps_nmea.msg_len)
+      return;
+  }
 
   // position fix status
   // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
   // check for good position fix
   if( (gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',') )  {
     gps_nmea.pos_available = TRUE;
+    //gps.fix = GPS_FIX_3D;
     NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r");
   } else {
     gps_nmea.pos_available = FALSE;
+    //gps.fix = GPS_FIX_NONE;
     NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r");
   }
   while(gps_nmea.msg_buf[i++] != ',') {              // next field: satellites used
@@ -314,9 +311,11 @@ void parse_nmea_GPGGA(void) {
   }
 
   // get altitude (in meters)
-  // FIXME alt above ellipsoid or geoid (MSL) ???
+  // alt above geoid (MSL)
   double alt = strtod(&gps_nmea.msg_buf[i], &endptr);
-  gps.hmsl = alt * 100;
+  lla_f.alt = alt;
+  gps.hmsl = lla_f.alt * 1000;
+  gps.lla_pos.alt = lla_f.alt * 1000;
   NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl);
 
   while(gps_nmea.msg_buf[i++] != ',') {              // next field: altitude units, always 'M'
@@ -340,6 +339,26 @@ void parse_nmea_GPGGA(void) {
       return;
   }
   //while(gps_nmea.msg_buf[i++] != '*');              // next field: checksum
+
+#if GPS_USE_LATLONG
+  /* convert to utm */
+  struct UtmCoor_f utm_f;
+  utm_f.zone = nav_utm_zone0;
+  utm_of_lla_f(&utm_f, &lla_f);
+
+  /* copy results of utm conversion */
+  gps.utm_pos.east = utm_f.east*100;
+  gps.utm_pos.north = utm_f.north*100;
+  gps.utm_pos.alt = utm_f.alt*1000;
+  gps.utm_pos.zone = nav_utm_zone0;
+#endif
+
+  /* convert to ECEF */
+  struct EcefCoor_f ecef_f;
+  ecef_of_lla_f(&ecef_f, &lla_f);
+  gps.ecef_pos.x = ecef_f.x * 100;
+  gps.ecef_pos.y = ecef_f.y * 100;
+  gps.ecef_pos.z = ecef_f.z * 100;
 }
 
 /**

sw/airborne/subsystems/gps/gps_nmea.h

@@ -20,8 +20,8 @@
  *
  */
 
-/** \file gps_nmea.h
- * \brief NMEA protocol specific code
+/** @file gps_nmea.h
+ * NMEA protocol specific code.
  *
 */