diff --git a/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile
new file mode 100644
index 0000000000..15577f5d71
--- /dev/null
+++ b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile
@@ -0,0 +1,32 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# Furuno NMEA GPS unit
+
+GPS_LED ?= none
+
+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)
+ap.CFLAGS += -DNMEA_PARSE_PROP
+
+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 $(SRC_SUBSYSTEMS)/gps/gps_furuno.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
+
+nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
+nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile
new file mode 100644
index 0000000000..7d12d6adb2
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile
@@ -0,0 +1,24 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# Furuno 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)
+ap.CFLAGS += -DNMEA_PARSE_PROP
+
+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 $(SRC_SUBSYSTEMS)/gps/gps_furuno.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+
diff --git a/sw/airborne/subsystems/gps/gps_furuno.c b/sw/airborne/subsystems/gps/gps_furuno.c
new file mode 100644
index 0000000000..befb57da65
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_furuno.c
@@ -0,0 +1,109 @@
+/*
+ *
+ * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/gps/gps_furuno.c
+ * GPS furuno based NMEA parser
+ */
+
+#include "gps_nmea.h"
+#include "subsystems/gps.h"
+#include <stdio.h>
+#include <string.h>
+
+#define GPS_FURUNO_SETTINGS_NB    10
+static const char *gps_furuno_settings[GPS_FURUNO_SETTINGS_NB] = {
+  "PERDAPI,FIXPERSEC,5",        // Receive position every 5 Hz
+  "PERDAPI,CROUT,ALLOFF",       // Disable all propriarty output
+  "PERDCFG,NMEAOUT,GGA,1",      // Enable GGA every fix
+  "PERDCFG,NMEAOUT,RMC,1",      // Enable RMC every fix
+  "PERDCFG,NMEAOUT,GSA,1",      // Enable GSA every fix
+  "PERDCFG,NMEAOUT,GNS,0",      // Disable GSA
+  "PERDCFG,NMEAOUT,ZDA,0",      // Disable ZDA
+  "PERDCFG,NMEAOUT,GSV,0",      // Disable GSV
+  "PERDCFG,NMEAOUT,GST,0",      // Disable ZDA
+  "PERDAPI,CROUT,V"             // Enable raw velocity
+};
+
+static void nmea_parse_perdcrv(void);
+
+void nmea_parse_prop_init(void)
+{
+  static uint8_t i = 0;
+  uint8_t j, len, crc;
+  char buf[128];
+
+  // Return when doen
+  if (i == GPS_FURUNO_SETTINGS_NB) {
+    return;
+  }
+
+  for (; i < GPS_FURUNO_SETTINGS_NB; i++) {
+    len = strlen(gps_furuno_settings[i]);
+    crc = nmea_calc_crc(gps_furuno_settings[i], len);
+    sprintf(buf, "$%s*%02X\r\n", gps_furuno_settings[i], crc);
+
+    // Check if there is enough space to send the config msg
+    if (GpsLink(CheckFreeSpace(len + 6))) {
+      for (j = 0; j < len + 6; j++) {
+        GpsLink(Transmit(buf[j]));
+      }
+    } else {
+      break;
+    }
+  }
+}
+
+void nmea_parse_prop_msg(void)
+{
+  if (gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "PERDCRV", 7)) {
+    nmea_parse_perdcrv();
+  }
+}
+
+void nmea_parse_perdcrv(void)
+{
+  int i = 8;
+
+  // Ignore reserved
+  nmea_read_until(&i);
+
+  // Ignore reserved
+  nmea_read_until(&i);
+
+  //EAST VEL
+  double east_vel = strtod(&gps_nmea.msg_buf[i], NULL);
+  gps.ned_vel.y = east_vel * 100; // in cm/s
+
+  // Ignore reserved
+  nmea_read_until(&i);
+
+  // NORTH VEL
+  double north_vel = strtod(&gps_nmea.msg_buf[i], NULL);
+  gps.ned_vel.x = north_vel * 100; // in cm/s
+
+  //Convert velocity to ecef
+  struct LtpDef_i ltp;
+  ltp_def_from_ecef_i(&ltp, &gps.ecef_pos);
+  ecef_of_ned_vect_i(&gps.ecef_vel, &ltp, &gps.ned_vel);
+}
diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c
index 1190294c51..987ebe2a82 100644
--- a/sw/airborne/subsystems/gps/gps_nmea.c
+++ b/sw/airborne/subsystems/gps/gps_nmea.c
@@ -1,6 +1,7 @@
 /*
  *
  * Copyright (C) 2008-2011 The Paparazzi Team
+ *               2014 Freek van Tienen <freek.v.tienen@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -45,322 +46,29 @@
 #include <string.h>
 #include <math.h>
 #include <stdlib.h>
-#ifdef DEBUG_NMEA
-// do debug-output if run on the DEBUG_NMEA-target
-
-#endif
-
 
 struct GpsNmea gps_nmea;
 
-void parse_nmea_GPGSA(void);
-void parse_nmea_GPRMC(void);
-void parse_nmea_GPGGA(void);
+static void nmea_parse_GSA(void);
+static void nmea_parse_RMC(void);
+static void nmea_parse_GGA(void);
 
 
-void gps_impl_init( void ) {
+void gps_impl_init(void)
+{
   gps_nmea.msg_available = FALSE;
   gps_nmea.pos_available = FALSE;
   gps_nmea.gps_nb_ovrn = 0;
   gps_nmea.msg_len = 0;
+  nmea_parse_prop_init();
 }
 
-
-/**
- * parse GPGSA-nmea-messages stored in
- * nmea_msg_buf .
- */
-void parse_nmea_GPGSA(void) {
-  int i = 6;     // current position in the message, start after: GPGSA,
-  //      char* endptr;  // end of parsed substrings
-
-  // attempt to reject empty packets right away
-  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
-    NMEA_PRINT("p_GPGSA() - skipping empty message\n\r");
-    return;
-  }
-
-  // get auto2D/3D
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: fix
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-
-  // 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;
-  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) {
-      NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-
-  //int satcount = 0;
-
-  // TODO: get sateline-numbers for gps_svinfos
+void WEAK nmea_parse_prop_init(void)
+{
 }
 
-/**
- * parse GPRMC-nmea-messages stored in
- * gps_nmea.msg_buf .
- */
-void parse_nmea_GPRMC(void) {
-  int i = 6;     // current position in the message, start after: GPRMC,
-  char* endptr;  // end of parsed substrings
-
-  // attempt to reject empty packets right away
-  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
-    NMEA_PRINT("p_GPRMC() - skipping empty message\n\r");
-    return;
-  }
-
-  // get time
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: warning
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-
-  // get warning
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: lat
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get lat
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: N/S
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get North/South
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: lon
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get lon
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: E/W
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get eath/west
-  // ignored
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: speed
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get speed
-  double speed = strtod(&gps_nmea.msg_buf[i], &endptr);
-  gps.gspeed = speed * 1.852 * 100 / (60*60);
-  NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed*1000), (gps.gspeed*1000));
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: course
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  double course = strtod(&gps_nmea.msg_buf[i], &endptr);
-  gps.course = RadOfDeg(course) * 1e7;
-  NMEA_PRINT("COURSE: %d \n\r",gps_course);
-}
-
-
-/**
- * parse GPGGA-nmea-messages stored in
- * gps_nmea.msg_buf .
- */
-void parse_nmea_GPGGA(void) {
-  int i = 6;     // current position in the message, start after: GPGGA,
-  char* endptr;  // end of parsed substrings
-  double degrees, minutesfrac;
-  struct LlaCoor_f lla_f;
-
-  // attempt to reject empty packets right away
-  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
-    NMEA_PRINT("p_GPGGA() - skipping empty message\n\r");
-    return;
-  }
-
-  // get UTC time [hhmmss.sss]
-  // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
-  // FIXME: parse UTC time correctly
-  double time = strtod(&gps_nmea.msg_buf[i],&endptr);
-  gps.tow = (uint32_t)((time+1)*1000);
-
-  //AD TODO: strtod itow
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: latitude
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-
-  // get latitude [ddmm.mmmmm]
-  double lat = strtod(&gps_nmea.msg_buf[i], &endptr);
-  // convert to pure degrees [dd.dddd] format
-  minutesfrac = modf(lat/100, &degrees);
-  lat = degrees + (minutesfrac*100)/60;
-  // convert to radians
-  //GpsInfo.PosLLA.lat.f *= (M_PI/180);
-
-  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");
-      return;
-    }
-  }
-
-  // correct latitute for N/S
-  if(gps_nmea.msg_buf[i] == 'S')
-    lat = -lat;
-
-  // convert to radians
-  lla_f.lat = RadOfDeg(lat);
-
-  gps.lla_pos.lat = 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
-  minutesfrac = modf(lon/100, &degrees);
-  lon = degrees + (minutesfrac*100)/60;
-  // convert to radians
-  //GpsInfo.PosLLA.lon.f *= (M_PI/180);
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: E/W indicator
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
-
-  // correct latitute for E/W
-  if(gps_nmea.msg_buf[i] == 'W')
-    lon = -lon;
-
-  // convert to radians
-  lla_f.lon = RadOfDeg(lon);
-
-  gps.lla_pos.lon = 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);
-
-
-  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;
-    NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r");
-  } else {
-    gps_nmea.pos_available = FALSE;
-    NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r");
-  }
-
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: satellites used
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r\r");
-      return;
-    }
-  }
-  // get number of satellites used in GPS solution
-  gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
-  NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
-
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: HDOP (horizontal dilution of precision)
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // we use HDOP here, as the PDOP is not in the message
-  float hdop = strtof(&gps_nmea.msg_buf[i], &endptr);
-  gps.pdop = hdop * 100;
-
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: altitude
-    if (i >= gps_nmea.msg_len) {
-      NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
-      return;
-    }
-  }
-  // get altitude (in meters) above geoid (MSL)
-  // lla_f.alt should actuall be height above ellipsoid,
-  // but since we don't get that, use hmsl instead
-  lla_f.alt = strtof(&gps_nmea.msg_buf[i], &endptr);
-  gps.hmsl = lla_f.alt * 1000;
-  gps.lla_pos.alt = gps.hmsl;
-  NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl);
-
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: altitude units, always 'M'
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: geoid seperation
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: seperation units
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: DGPS age
-    if (i >= gps_nmea.msg_len)
-      return;
-  }
-  while(gps_nmea.msg_buf[i++] != ',') {              // next field: DGPS station ID
-    if (i >= gps_nmea.msg_len)
-      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 = gps.lla_pos.alt;
-  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;
+void WEAK nmea_parse_prop_msg(void)
+{
 }
 
 /**
@@ -368,32 +76,28 @@ void parse_nmea_GPGGA(void) {
  * Find out what type of message it is and
  * hand it to the parser for that type.
  */
-void nmea_parse_msg( void ) {
+void nmea_parse_msg(void)
+{
 
-  if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPRMC", 5)) {
+  if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "RMC", 3)) {
     gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
-    NMEA_PRINT("parsing RMC: \"%s\" \n\r",gps_nmea.msg_buf);
+    NMEA_PRINT("parsing RMC: \"%s\" \n\r", gps_nmea.msg_buf);
     NMEA_PRINT("RMC");
-    parse_nmea_GPRMC();
-  }
-  else {
-    if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGGA", 5)) {
-      gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
-      NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r",gps_nmea.msg_buf);
-      NMEA_PRINT("GGA");
-      parse_nmea_GPGGA();
-    }
-    else {
-      if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGSA", 5)) {
-        gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
-        NMEA_PRINT("GSA: \"%s\" \n\r",gps_nmea.msg_buf);
-        NMEA_PRINT("GSA");
-        parse_nmea_GPGSA();
-      } else {
-        gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
-        NMEA_PRINT("ignoring: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf);
-      }
-    }
+    nmea_parse_RMC();
+  } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GGA", 3)) {
+    gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
+    NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r", gps_nmea.msg_buf);
+    NMEA_PRINT("GGA");
+    nmea_parse_GGA();
+  } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSA", 3)) {
+    gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
+    NMEA_PRINT("GSA: \"%s\" \n\r", gps_nmea.msg_buf);
+    NMEA_PRINT("GSA");
+    nmea_parse_GSA();
+  } else {
+    gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
+    NMEA_PRINT("Propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf);
+    nmea_parse_prop_msg();
   }
 
   // reset message-buffer
@@ -407,11 +111,13 @@ void nmea_parse_msg( void ) {
  * setting gps_nmea.msg_available to TRUE
  * after a full line.
  */
-void nmea_parse_char( uint8_t c ) {
+void nmea_parse_char(uint8_t c)
+{
   //reject empty lines
   if (gps_nmea.msg_len == 0) {
-    if (c == '\r' || c == '\n' || c == '$')
+    if (c == '\r' || c == '\n' || c == '$') {
       return;
+    }
   }
 
   // fill the buffer, unless it's full
@@ -427,6 +133,219 @@ void nmea_parse_char( uint8_t c ) {
     }
   }
 
-  if (gps_nmea.msg_len >= NMEA_MAXLEN - 1)
+  if (gps_nmea.msg_len >= NMEA_MAXLEN - 1) {
     gps_nmea.msg_available = TRUE;
+  }
+}
+
+/**
+ * Calculate control sum of binary buffer
+ */
+uint8_t nmea_calc_crc(const char *buff, int buff_sz)
+{
+  uint8_t chsum = 0,
+          it;
+
+  for (it = 0; it < buff_sz; ++it) {
+    chsum ^= buff[it];
+  }
+
+  return chsum;
+}
+
+/**
+ * parse GPGSA-nmea-messages stored in
+ * nmea_msg_buf .
+ */
+static void nmea_parse_GSA(void)
+{
+  int i = 6;     // current position in the message, start after: GPGSA,
+
+  // attempt to reject empty packets right away
+  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
+    NMEA_PRINT("p_GPGSA() - skipping empty message\n\r");
+    return;
+  }
+
+  // get auto2D/3D
+  // ignored
+  nmea_read_until(&i);
+
+  // 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;
+  }
+  NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix);
+  nmea_read_until(&i);
+
+  //int satcount = 0;
+
+  // TODO: get sateline-numbers for gps_svinfos
+}
+
+/**
+ * parse GPRMC-nmea-messages stored in
+ * gps_nmea.msg_buf .
+ */
+static void nmea_parse_RMC(void)
+{
+  int i = 6;     // current position in the message, start after: GPRMC,
+
+  // attempt to reject empty packets right away
+  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
+    NMEA_PRINT("p_GPRMC() - skipping empty message\n\r");
+    return;
+  }
+  // First read time (ignored)
+
+  // get warning
+  nmea_read_until(&i);
+
+  // get lat
+  nmea_read_until(&i);
+
+  // get North/South
+  nmea_read_until(&i);
+
+  // get lon
+  nmea_read_until(&i);
+
+  // get eath/west
+  nmea_read_until(&i);
+
+  // get speed
+  nmea_read_until(&i);
+  double speed = strtod(&gps_nmea.msg_buf[i], NULL);
+  gps.gspeed = speed * 1.852 * 100 / (60 * 60);
+  NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed * 1000), (gps.gspeed * 1000));
+
+  // get course
+  nmea_read_until(&i);
+  double course = strtod(&gps_nmea.msg_buf[i], NULL);
+  gps.course = RadOfDeg(course) * 1e7;
+  NMEA_PRINT("COURSE: %d \n\r", gps_course);
+}
+
+
+/**
+ * parse GPGGA-nmea-messages stored in
+ * gps_nmea.msg_buf .
+ */
+static void nmea_parse_GGA(void)
+{
+  int i = 6;     // current position in the message, start after: GPGGA,
+  double degrees, minutesfrac;
+  struct LlaCoor_f lla_f;
+
+  // attempt to reject empty packets right away
+  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
+    NMEA_PRINT("p_GPGGA() - skipping empty message\n\r");
+    return;
+  }
+
+  // get UTC time [hhmmss.sss]
+  // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL);
+  // FIXME: parse UTC time correctly
+  double time = strtod(&gps_nmea.msg_buf[i], NULL);
+  gps.tow = (uint32_t)((time + 1) * 1000);
+
+  // get latitude [ddmm.mmmmm]
+  nmea_read_until(&i);
+  double lat = strtod(&gps_nmea.msg_buf[i], NULL);
+  // convert to pure degrees [dd.dddd] format
+  minutesfrac = modf(lat / 100, &degrees);
+  lat = degrees + (minutesfrac * 100) / 60;
+
+  // get latitute N/S
+  nmea_read_until(&i);
+  if (gps_nmea.msg_buf[i] == 'S') {
+    lat = -lat;
+  }
+
+  // convert to radians
+  lla_f.lat = RadOfDeg(lat);
+  gps.lla_pos.lat = lat * 1e7; // convert to fixed-point
+  NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat * 1000), lla_f.lat);
+
+
+  // get longitude [ddmm.mmmmm]
+  nmea_read_until(&i);
+  double lon = strtod(&gps_nmea.msg_buf[i], NULL);
+  // convert to pure degrees [dd.dddd] format
+  minutesfrac = modf(lon / 100, &degrees);
+  lon = degrees + (minutesfrac * 100) / 60;
+
+  // get longitude E/W
+  nmea_read_until(&i);
+  if (gps_nmea.msg_buf[i] == 'W') {
+    lon = -lon;
+  }
+
+  // convert to radians
+  lla_f.lon = RadOfDeg(lon);
+  gps.lla_pos.lon = 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);
+
+  // get position fix status
+  nmea_read_until(&i);
+  // 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;
+    NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r");
+  } else {
+    gps_nmea.pos_available = FALSE;
+    NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r");
+  }
+
+  // get number of satellites used in GPS solution
+  nmea_read_until(&i);
+  gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
+  NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
+
+  // we use HDOP here, as the PDOP is not in the message
+  nmea_read_until(&i);
+  float hdop = strtof(&gps_nmea.msg_buf[i], NULL);
+  gps.pdop = hdop * 100;
+
+  // get altitude (in meters) above geoid (MSL)
+  nmea_read_until(&i);
+  // lla_f.alt should actuall be height above ellipsoid,
+  // but since we don't get that, use hmsl instead
+  lla_f.alt = strtof(&gps_nmea.msg_buf[i], NULL);
+  gps.hmsl = lla_f.alt * 1000;
+  gps.lla_pos.alt = gps.hmsl;
+  NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl);
+
+  // get altitude units (allways M)
+  nmea_read_until(&i);
+  // get geoid seperation
+  nmea_read_until(&i);
+  // get seperations units
+  nmea_read_until(&i);
+  // get DGPS age
+  nmea_read_until(&i);
+  // get DGPS station ID
+
+#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 = gps.lla_pos.alt;
+  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;
 }
diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h
index 0772e68b90..cadece9d0b 100644
--- a/sw/airborne/subsystems/gps/gps_nmea.h
+++ b/sw/airborne/subsystems/gps/gps_nmea.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2004-2011 The Paparazzi Team
+ *               2014 Freek van Tienen <freek.v.tienen@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -31,11 +32,7 @@
 
 #include "mcu_periph/uart.h"
 
-#define GPS_NB_CHANNELS 16
-
-#ifdef DEBUG_NMEA
-#define NMEA_PRINT(...) {  UsbSPrintString( __VA_ARGS__);};
-#else
+#ifndef DEBUG_NMEA
 #define NMEA_PRINT(...) {};
 #endif
 
@@ -62,6 +59,7 @@ extern struct GpsNmea gps_nmea;
 #define GpsBuffer() GpsLink(ChAvailable())
 
 #define GpsEvent(_sol_available_callback) {             \
+    nmea_parse_prop_init();                             \
     if (GpsBuffer()) {                                  \
       ReadGpsBuffer();                                  \
     }                                                   \
@@ -80,19 +78,27 @@ extern struct GpsNmea gps_nmea;
     }                                                   \
   }
 
-#define ReadGpsBuffer() {					\
-    while (GpsLink(ChAvailable())&&!gps_nmea.msg_available)	\
-      nmea_parse_char(GpsLink(Getch()));			\
+#define ReadGpsBuffer() {         \
+    while (GpsLink(ChAvailable())&&!gps_nmea.msg_available) \
+      nmea_parse_char(GpsLink(Getch()));      \
   }
 
 
-
 /** The function to be called when a characted friom the device is available */
 extern void nmea_parse_char(uint8_t c);
-
 extern void nmea_parse_msg(void);
+extern uint8_t nmea_calc_crc(const char *buff, int buff_sz);
+void nmea_parse_prop_init(void);
+void nmea_parse_prop_msg(void);
 
-
-#define gps_nmea_Reset(_val) { }
+/** Read until a certain character, placed here for proprietary includes */
+static inline void nmea_read_until(int *i)
+{
+  while (gps_nmea.msg_buf[*i++] != ',') {
+    if (*i >= gps_nmea.msg_len) {
+      return;
+    }
+  }
+}
 
 #endif /* GPS_NMEA_H */