diff --git a/conf/autopilot/fixedwing.xml b/conf/autopilot/fixedwing.xml
index 0899eb9b68..f65f6282b1 100644
--- a/conf/autopilot/fixedwing.xml
+++ b/conf/autopilot/fixedwing.xml
@@ -34,7 +34,7 @@
-
+
diff --git a/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile b/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile
new file mode 100644
index 0000000000..9999035a73
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/gps_nmea.makefile
@@ -0,0 +1,15 @@
+# Manta NMEA GPS unit
+
+
+ap.CFLAGS += -DUSE_GPS -DNMEA -DGPS_USE_LATLONG
+ap.CFLAGS += -DGPS_LINK=Uart$(GPS_UART_NR)
+ap.CFLAGS += -DUSE_UART$(GPS_UART_NR)
+ap.CFLAGS += -DUART$(GPS_UART_NR)_BAUD=$(GPS_BAUD)
+
+ifneq ($(GPS_LED),none)
+ ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.srcs += $(SRC_FIXEDWING)/gps_nmea.c
+
+$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
diff --git a/sw/airborne/gps_nmea.c b/sw/airborne/gps_nmea.c
index 6d843b92b6..7a3215b594 100644
--- a/sw/airborne/gps_nmea.c
+++ b/sw/airborne/gps_nmea.c
@@ -1,5 +1,5 @@
/*
- *
+ *
* Copyright (C) 2008 Marcus Wolschon
*
* This file is part of paparazzi.
@@ -17,11 +17,11 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*
*/
-/**
+/**
* file gps_nmea.c
* brief Parser for the NMEA protocol
*
@@ -34,21 +34,23 @@
*/
#include
-#include
+#include
#include
-#ifdef LINUX
-// do debug-output if run on the linux-target
#include
+#ifdef DEBUG_NMEA
+// do debug-output if run on the DEBUG_NMEA-target
+
#endif
-
+#include "generated/airframe.h"
+#include "autopilot.h"
#include "generated/flight_plan.h"
#include "mcu_periph/uart.h"
#include "gps.h"
+#include "gps_nmea.h"
#include "subsystems/nav.h"
#include "latlong.h"
-
int32_t gps_lat; // latitude in degrees * 1e-7
int32_t gps_lon; // longitude in degrees * 1e-7
uint16_t gps_PDOP; //precision
@@ -64,6 +66,16 @@ int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
uint8_t gps_mode;
+#ifdef GPS_CONFIGURE
+static uint8_t gps_status_config;
+#endif
+
+//AD added gps_configuring variable
+// in gps.h if gps_configuring is true GpsParseOrConfigure() checks value of variable
+// and calls gps_configure() or parse_gps_msg() depending on its value
+
+bool_t gps_configuring;
+
// true if parse_ubx() has a complete message and parse_gps_msg() shall parse it
volatile bool_t gps_msg_received = FALSE;
@@ -84,26 +96,31 @@ uint8_t gps_nb_ovrn; // number if incomplete nmea-messages
////////////////////////////////////////////////////////
// uart-configuration
+//AD
+// added GPS_CONFIG_INIT define equal to 0
+#define GPS_CONFIG_INIT 0
void gps_init( void ) {
#ifdef GPS_CONFIGURE
gps_status_config = GPS_CONFIG_INIT;
+ gps_configuring = TRUE;
#endif
}
-void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
-}
#ifdef GPS_CONFIGURE
/* GPS dynamic configuration */
+#include "uart.h"
+
void gps_configure_uart ( void ) {
- //UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
+ //UbxSend_CFG_PRT(0x01, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
//while (GpsUartRunning) ; /* FIXME */
GpsUartInitParam( UART_BAUD(GPS_BAUD), UART_8N1, UART_FIFO_8);
}
void gps_configure ( void ) {
+ gps_configuring=FALSE;
}
#endif /* GPS_CONFIGURE */
@@ -118,24 +135,22 @@ void gps_configure ( void ) {
#define NMEA_MAXLEN 255
char nmea_msg_buf[NMEA_MAXLEN];
int nmea_msg_len = 0;
-
+/*
int GpsFixValid() {
return gps_pos_available;
-}
-
+}
+*/
/**
* parse GPGSA-nmea-messages stored in
* nmea_msg_buf .
*/
void parse_nmea_GPGSA() {
int i = 8; // current position in the message
- char* endptr; // end of parsed substrings
+// char* endptr; // end of parsed substrings
// attempt to reject empty packets right away
if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') {
-#ifdef LINUX
- printf("parse_nmea_GPGSA() - skipping empty message\n");
-#endif
+ NMEA_PRINT("p_GPGSA() - skipping empty message\n\r");
return;
}
@@ -143,9 +158,7 @@ void parse_nmea_GPGSA() {
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: fix
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGSA() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
return;
}
}
@@ -155,19 +168,15 @@ void parse_nmea_GPGSA() {
gps_mode = atoi(&nmea_msg_buf[i]);
if (gps_mode == 1)
gps_mode = 0;
-#ifdef LINUX
- printf("parse_nmea_GPGSA() - gps_mode=%i (3=3D)\n", gps_mode);
-#endif
+ NMEA_PRINT("p_GPGSA() - gps_mode=%i (3=3D)\n\r", gps_mode);
while(nmea_msg_buf[i++] != ',') { // next field:sateline-number-0
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGSA() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
return;
}
}
- int satcount = 0;
+ //int satcount = 0;
// TODO: get sateline-numbers for gps_svinfos
}
@@ -182,9 +191,7 @@ void parse_nmea_GPRMC() {
// attempt to reject empty packets right away
if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping empty message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping empty message\n\r");
return;
}
@@ -192,79 +199,64 @@ void parse_nmea_GPRMC() {
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: warning
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get warning
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: lat
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get lat
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: N/S
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get North/South
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: lon
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get lon
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: E/W
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get eath/west
// ignored
while(nmea_msg_buf[i++] != ',') { // next field: speed
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
// get speed
double speed = strtod(&nmea_msg_buf[i], &endptr);
gps_gspeed = speed * 1.852 * 100 / (60*60);
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - ground-speed=%f knot = %i cm/s\n", speed, gps_gspeed);
-#endif
- while(nmea_msg_buf[i++] != ',') { // next field: speed
+ NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed*1000), (gps_gspeed*1000));
+ while(nmea_msg_buf[i++] != ',') { // next field: course
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPRMC() - skipping incomplete message\n");
-#endif
+ NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
return;
- }
+ }
}
-
-
+ double course = strtod(&nmea_msg_buf[i], &endptr);
+ gps_course=course*10;
+ NMEA_PRINT("COURSE: %d \n\r",gps_course);
}
@@ -279,20 +271,20 @@ void parse_nmea_GPGGA() {
// attempt to reject empty packets right away
if(nmea_msg_buf[i]==',' && nmea_msg_buf[i+1]==',') {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping empty message\n");
-#endif
+ NMEA_PRINT("p_GPGGA() - skipping empty message\n\r");
return;
}
// get UTC time [hhmmss.sss]
// ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
+ double time = strtod(&nmea_msg_buf[i],&endptr);
+ gps_itow = (uint32_t)((time+1)*1000);
+
+//AD TODO: strtod itow
while(nmea_msg_buf[i++] != ',') { // next field: latitude
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping incomplete message\n");
-#endif
- return;
+ NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
+ return;
}
}
@@ -305,26 +297,22 @@ void parse_nmea_GPGGA() {
//GpsInfo.PosLLA.lat.f *= (M_PI/180);
while(nmea_msg_buf[i++] != ',') { // next field: N/S indicator
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping incomplete message\n");
-#endif
- return;
- }
+ NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
+ return;
+ }
}
-
+
// correct latitute for N/S
if(nmea_msg_buf[i] == 'S')
lat = -lat;
while(nmea_msg_buf[i++] != ',') { // next field: longitude
if (i >= nmea_msg_len)
- return;
+ return;
}
gps_lat = lat * 1e7; // convert to fixed-point
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - lat=%f gps_lat=%i\n", lat, gps_lat);
-#endif
-
+ NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), gps_lat);
+
// get longitude [ddmm.mmmmm]
double lon = strtod(&nmea_msg_buf[i], &endptr);
// convert to pure degrees [dd.dddd] format
@@ -334,21 +322,19 @@ void parse_nmea_GPGGA() {
//GpsInfo.PosLLA.lon.f *= (M_PI/180);
while(nmea_msg_buf[i++] != ',') { // next field: E/W indicator
if (i >= nmea_msg_len)
- return;
+ return;
}
-
+
// correct latitute for E/W
if(nmea_msg_buf[i] == 'W')
lon = -lon;
while(nmea_msg_buf[i++] != ',') { // next field: position fix status
if (i >= nmea_msg_len)
- return;
+ return;
}
-
+
gps_lon = lon * 1e7; // convert to fixed-point
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - lon=%f gps_lon=%i\n", lon, gps_lon);
-#endif
+ NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), gps_lon,gps_itow);
latlong_utm_of(RadOfDeg(lat), RadOfDeg(lon), nav_utm_zone0);
@@ -362,107 +348,89 @@ void parse_nmea_GPGGA() {
// check for good position fix
if( (nmea_msg_buf[i] != '0') && (nmea_msg_buf[i] != ',') ) {
gps_pos_available = TRUE;
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - gps_pos_available == true\n");
-#endif
+ NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r");
} else {
gps_pos_available = FALSE;
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - gps_pos_available == false\n");
-#endif
+ NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r");
}
while(nmea_msg_buf[i++] != ',') { // next field: satellites used
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping incomplete message\n");
-#endif
- return;
- }
+ NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r\r");
+ return;
+ }
}
-
+
// get number of satellites used in GPS solution
gps_numSV = atoi(&nmea_msg_buf[i]);
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - gps_numSatlitesUsed=%i\n", gps_numSV);
-#endif
+ NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps_numSV);
while(nmea_msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision)
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping incomplete message\n");
-#endif
- return;
- }
+ NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
+ return;
+ }
}
while(nmea_msg_buf[i++] != ',') { // next field: altitude
if (i >= nmea_msg_len) {
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - skipping incomplete message\n");
-#endif
- return;
- }
+ NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
+ return;
+ }
}
-
+
// get altitude (in meters)
double alt = strtod(&nmea_msg_buf[i], &endptr);
gps_alt = alt * 10;
-#ifdef LINUX
- printf("parse_nmea_GPGGA() - gps_alt=%i\n", gps_alt);
-#endif
+ NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps_alt);
+
while(nmea_msg_buf[i++] != ',') { // next field: altitude units, always 'M'
if (i >= nmea_msg_len)
- return;
+ return;
}
while(nmea_msg_buf[i++] != ',') { // next field: geoid seperation
if (i >= nmea_msg_len)
- return;
+ return;
}
while(nmea_msg_buf[i++] != ',') { // next field: seperation units
if (i >= nmea_msg_len)
- return;
+ return;
}
while(nmea_msg_buf[i++] != ',') { // next field: DGPS age
if (i >= nmea_msg_len)
- return;
+ return;
}
while(nmea_msg_buf[i++] != ',') { // next field: DGPS station ID
if (i >= nmea_msg_len)
- return;
+ return;
}
//while(nmea_msg_buf[i++] != '*'); // next field: checksum
}
/**
- * parse_ubx() has a complete line.
+ * parse_nmea_char() has a complete line.
* Find out what type of message it is and
* hand it to the parser for that type.
*/
void parse_gps_msg( void ) {
- if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPRMC", 6)) {
+ if(nmea_msg_len > 5 && !strncmp(nmea_msg_buf , "GPRMC", 5)) {
nmea_msg_buf[nmea_msg_len] = 0;
-#ifdef LINUX
- printf("parse_gps_msg() - parsing RMC gps-message \"%s\"\n",nmea_msg_buf);
-#endif
+ NMEA_PRINT("parsing RMC: \"%s\" \n\r",nmea_msg_buf);
+ NMEA_PRINT("RMC");
parse_nmea_GPRMC();
} else
- if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGGA", 6)) {
+ if(nmea_msg_len > 5 && !strncmp(nmea_msg_buf , "GPGGA", 5)) {
nmea_msg_buf[nmea_msg_len] = 0;
-#ifdef LINUX
- printf("parse_gps_msg() - parsing GGA gps-message \"%s\"\n",nmea_msg_buf);
-#endif
+ NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r",nmea_msg_buf);
+ NMEA_PRINT("GGA");
parse_nmea_GPGGA();
} else
- if(nmea_msg_len > 7 && !strncmp(nmea_msg_buf + 1 , "$GPGSA", 6)) {
+ if(nmea_msg_len > 5 && !strncmp(nmea_msg_buf , "GPGSA", 5)) {
nmea_msg_buf[nmea_msg_len] = 0;
-#ifdef LINUX
- printf("parse_gps_msg() - parsing GSA gps-message \"%s\"\n",nmea_msg_buf);
-#endif
+ NMEA_PRINT("GSA: \"%s\" \n\r",nmea_msg_buf);
+ NMEA_PRINT("GSA");
parse_nmea_GPGSA();
} else {
nmea_msg_buf[nmea_msg_len] = 0;
-#ifdef LINUX
- printf("parse_gps_msg() - ignoring unknown gps-message \"%s\" len=%i\n",nmea_msg_buf, nmea_msg_len);
-#endif
+ NMEA_PRINT("ignoring: len=%i \n\r \"%s\" \n\r", nmea_msg_len, nmea_msg_buf);
}
// reset message-buffer
@@ -476,8 +444,7 @@ void parse_gps_msg( void ) {
* setting gps_msg_received to TRUE
* after a full line.
*/
-void parse_ubx( uint8_t c ) {
-
+void parse_nmea_char( uint8_t c ) {
//reject empty lines
if (nmea_msg_len == 0) {
if (c == '\r' || c == '\n' || c == '$')
@@ -488,6 +455,7 @@ void parse_ubx( uint8_t c ) {
if (nmea_msg_len < NMEA_MAXLEN - 1) {
// messages end with a linefeed
+ //AD: TRUNK: if (c == '\r' || c == '\n')
if (c == '\r' || c == '\n') {
gps_msg_received = TRUE;
} else {
diff --git a/sw/airborne/gps_nmea.h b/sw/airborne/gps_nmea.h
new file mode 100644
index 0000000000..3428a0a95d
--- /dev/null
+++ b/sw/airborne/gps_nmea.h
@@ -0,0 +1,72 @@
+/*
+ * Paparazzi autopilot $Id: gps_ubx.h 6376 2010-11-07 04:30:44Z flixr $
+ *
+ * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
+ *
+ * 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 gps_nmea.h
+ * \brief NMEA protocol specific code
+ *
+*/
+
+
+#ifndef GPS_NMEA_H
+#define GPS_NMEA_H
+
+#define GPS_NB_CHANNELS 16
+#include "gps.h"
+
+#ifdef DEBUG_NMEA
+#define NMEA_PRINT(...) { UsbSPrintString( __VA_ARGS__);};
+#else
+#define NMEA_PRINT(...) {};
+#endif
+
+void parse_nmea_GPGSA(void);
+void parse_nmea_GPRMC(void);
+void parse_nmea_GPGGA(void);
+
+extern uint16_t gps_reset;
+
+
+
+/** The function to be called when a characted friom the device is available */
+void parse_nmea_char( uint8_t c );
+
+
+#define GpsParse(_gps_buffer, _gps_buffer_size) { \
+ uint8_t i; \
+ for(i = 0; i < _gps_buffer_size; i++) { \
+ parse_ubx(_gps_buffer[i]); \
+ } \
+}
+
+#define GpsFixValid() (gps_mode == 3)
+
+#define gps_nmea_Reset(_val) { \
+ gps_reset = _val; \
+}
+
+#ifdef GPS_TIMESTAMP
+uint32_t itow_from_ticks(uint32_t clock_ticks);
+#endif
+#define ubxsend_cfg_rst(a,b) {};
+#endif /* GPS_NMEA_H */