Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_nmea.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2008-2011 The Paparazzi Team
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
34 #include "subsystems/gps.h"
35 
36 #include "led.h"
37 
38 #if GPS_USE_LATLONG
39 /* currently needed to get nav_utm_zone0 */
41 #endif
43 
44 #include <inttypes.h>
45 #include <string.h>
46 #include <math.h>
47 #include <stdlib.h>
48 #ifdef DEBUG_NMEA
49 // do debug-output if run on the DEBUG_NMEA-target
50 
51 #endif
52 
53 
55 
56 void parse_nmea_GPGSA(void);
57 void parse_nmea_GPRMC(void);
58 void parse_nmea_GPGGA(void);
59 
60 
61 void gps_impl_init( void ) {
65  gps_nmea.msg_len = 0;
66 }
67 
68 
73 void parse_nmea_GPGSA(void) {
74  int i = 6; // current position in the message, start after: GPGSA,
75  // char* endptr; // end of parsed substrings
76 
77  // attempt to reject empty packets right away
78  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
79  NMEA_PRINT("p_GPGSA() - skipping empty message\n\r");
80  return;
81  }
82 
83  // get auto2D/3D
84  // ignored
85  while(gps_nmea.msg_buf[i++] != ',') { // next field: fix
86  if (i >= gps_nmea.msg_len) {
87  NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
88  return;
89  }
90  }
91 
92  // get 2D/3D-fix
93  // set gps_mode=3=3d, 2=2d, 1=no fix or 0
94  gps.fix = atoi(&gps_nmea.msg_buf[i]);
95  if (gps.fix == 1)
96  gps.fix = 0;
97  NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix);
98  while(gps_nmea.msg_buf[i++] != ',') { // next field:satellite-number-0
99  if (i >= gps_nmea.msg_len) {
100  NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r");
101  return;
102  }
103  }
104 
105  //int satcount = 0;
106 
107  // TODO: get sateline-numbers for gps_svinfos
108 }
109 
114 void parse_nmea_GPRMC(void) {
115  int i = 6; // current position in the message, start after: GPRMC,
116  char* endptr; // end of parsed substrings
117 
118  // attempt to reject empty packets right away
119  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
120  NMEA_PRINT("p_GPRMC() - skipping empty message\n\r");
121  return;
122  }
123 
124  // get time
125  // ignored
126  while(gps_nmea.msg_buf[i++] != ',') { // next field: warning
127  if (i >= gps_nmea.msg_len) {
128  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
129  return;
130  }
131  }
132 
133  // get warning
134  // ignored
135  while(gps_nmea.msg_buf[i++] != ',') { // next field: lat
136  if (i >= gps_nmea.msg_len) {
137  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
138  return;
139  }
140  }
141  // get lat
142  // ignored
143  while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S
144  if (i >= gps_nmea.msg_len) {
145  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
146  return;
147  }
148  }
149  // get North/South
150  // ignored
151  while(gps_nmea.msg_buf[i++] != ',') { // next field: lon
152  if (i >= gps_nmea.msg_len) {
153  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
154  return;
155  }
156  }
157  // get lon
158  // ignored
159  while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W
160  if (i >= gps_nmea.msg_len) {
161  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
162  return;
163  }
164  }
165  // get eath/west
166  // ignored
167  while(gps_nmea.msg_buf[i++] != ',') { // next field: speed
168  if (i >= gps_nmea.msg_len) {
169  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
170  return;
171  }
172  }
173  // get speed
174  double speed = strtod(&gps_nmea.msg_buf[i], &endptr);
175  gps.gspeed = speed * 1.852 * 100 / (60*60);
176  NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed*1000), (gps.gspeed*1000));
177  while(gps_nmea.msg_buf[i++] != ',') { // next field: course
178  if (i >= gps_nmea.msg_len) {
179  NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r");
180  return;
181  }
182  }
183  double course = strtod(&gps_nmea.msg_buf[i], &endptr);
184  gps.course = RadOfDeg(course) * 1e7;
185  NMEA_PRINT("COURSE: %d \n\r",gps_course);
186 }
187 
188 
193 void parse_nmea_GPGGA(void) {
194  int i = 6; // current position in the message, start after: GPGGA,
195  char* endptr; // end of parsed substrings
196  double degrees, minutesfrac;
197  struct LlaCoor_f lla_f;
198 
199  // attempt to reject empty packets right away
200  if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') {
201  NMEA_PRINT("p_GPGGA() - skipping empty message\n\r");
202  return;
203  }
204 
205  // get UTC time [hhmmss.sss]
206  // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr);
207  // FIXME: parse UTC time correctly
208  double time = strtod(&gps_nmea.msg_buf[i],&endptr);
209  gps.tow = (uint32_t)((time+1)*1000);
210 
211  //AD TODO: strtod itow
212  while(gps_nmea.msg_buf[i++] != ',') { // next field: latitude
213  if (i >= gps_nmea.msg_len) {
214  NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
215  return;
216  }
217  }
218 
219  // get latitude [ddmm.mmmmm]
220  double lat = strtod(&gps_nmea.msg_buf[i], &endptr);
221  // convert to pure degrees [dd.dddd] format
222  minutesfrac = modf(lat/100, &degrees);
223  lat = degrees + (minutesfrac*100)/60;
224  // convert to radians
225  //GpsInfo.PosLLA.lat.f *= (M_PI/180);
226 
227  while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S indicator
228  if (i >= gps_nmea.msg_len) {
229  NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
230  return;
231  }
232  }
233 
234  // correct latitute for N/S
235  if(gps_nmea.msg_buf[i] == 'S')
236  lat = -lat;
237 
238  // convert to radians
239  lla_f.lat = RadOfDeg(lat);
240 
241  gps.lla_pos.lat = lla_f.lat * 1e7; // convert to fixed-point
242  NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), lla_f.lat);
243 
244 
245  while(gps_nmea.msg_buf[i++] != ',') { // next field: longitude
246  if (i >= gps_nmea.msg_len)
247  return;
248  }
249 
250  // get longitude [ddmm.mmmmm]
251  double lon = strtod(&gps_nmea.msg_buf[i], &endptr);
252  // convert to pure degrees [dd.dddd] format
253  minutesfrac = modf(lon/100, &degrees);
254  lon = degrees + (minutesfrac*100)/60;
255  // convert to radians
256  //GpsInfo.PosLLA.lon.f *= (M_PI/180);
257  while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W indicator
258  if (i >= gps_nmea.msg_len)
259  return;
260  }
261 
262  // correct latitute for E/W
263  if(gps_nmea.msg_buf[i] == 'W')
264  lon = -lon;
265 
266  // convert to radians
267  lla_f.lon = RadOfDeg(lon);
268 
269  gps.lla_pos.lon = lla_f.lon * 1e7; // convert to fixed-point
270  NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), lla_f.lon, gps.tow);
271 
272 
273  while(gps_nmea.msg_buf[i++] != ',') { // next field: position fix status
274  if (i >= gps_nmea.msg_len)
275  return;
276  }
277 
278  // position fix status
279  // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
280  // check for good position fix
281  if( (gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',') ) {
283  NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r");
284  } else {
286  NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r");
287  }
288 
289  while(gps_nmea.msg_buf[i++] != ',') { // next field: satellites used
290  if (i >= gps_nmea.msg_len) {
291  NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r\r");
292  return;
293  }
294  }
295  // get number of satellites used in GPS solution
296  gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
297  NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
298 
299  while(gps_nmea.msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision)
300  if (i >= gps_nmea.msg_len) {
301  NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
302  return;
303  }
304  }
305  // we use HDOP here, as the PDOP is not in the message
306  float hdop = strtof(&gps_nmea.msg_buf[i], &endptr);
307  gps.pdop = hdop * 100;
308 
309  while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude
310  if (i >= gps_nmea.msg_len) {
311  NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r");
312  return;
313  }
314  }
315  // get altitude (in meters) above geoid (MSL)
316  // lla_f.alt should actuall be height above ellipsoid,
317  // but since we don't get that, use hmsl instead
318  lla_f.alt = strtof(&gps_nmea.msg_buf[i], &endptr);
319  gps.hmsl = lla_f.alt * 1000;
320  gps.lla_pos.alt = gps.hmsl;
321  NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl);
322 
323  while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude units, always 'M'
324  if (i >= gps_nmea.msg_len)
325  return;
326  }
327  while(gps_nmea.msg_buf[i++] != ',') { // next field: geoid seperation
328  if (i >= gps_nmea.msg_len)
329  return;
330  }
331  while(gps_nmea.msg_buf[i++] != ',') { // next field: seperation units
332  if (i >= gps_nmea.msg_len)
333  return;
334  }
335  while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS age
336  if (i >= gps_nmea.msg_len)
337  return;
338  }
339  while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS station ID
340  if (i >= gps_nmea.msg_len)
341  return;
342  }
343  //while(gps_nmea.msg_buf[i++] != '*'); // next field: checksum
344 
345 #if GPS_USE_LATLONG
346  /* convert to utm */
347  struct UtmCoor_f utm_f;
348  utm_f.zone = nav_utm_zone0;
349  utm_of_lla_f(&utm_f, &lla_f);
350 
351  /* copy results of utm conversion */
352  gps.utm_pos.east = utm_f.east*100;
353  gps.utm_pos.north = utm_f.north*100;
356 #endif
357 
358  /* convert to ECEF */
359  struct EcefCoor_f ecef_f;
360  ecef_of_lla_f(&ecef_f, &lla_f);
361  gps.ecef_pos.x = ecef_f.x * 100;
362  gps.ecef_pos.y = ecef_f.y * 100;
363  gps.ecef_pos.z = ecef_f.z * 100;
364 }
365 
371 void nmea_parse_msg( void ) {
372 
373  if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPRMC", 5)) {
375  NMEA_PRINT("parsing RMC: \"%s\" \n\r",gps_nmea.msg_buf);
376  NMEA_PRINT("RMC");
378  }
379  else {
380  if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGGA", 5)) {
382  NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r",gps_nmea.msg_buf);
383  NMEA_PRINT("GGA");
385  }
386  else {
387  if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGSA", 5)) {
389  NMEA_PRINT("GSA: \"%s\" \n\r",gps_nmea.msg_buf);
390  NMEA_PRINT("GSA");
392  } else {
394  NMEA_PRINT("ignoring: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf);
395  }
396  }
397  }
398 
399  // reset message-buffer
400  gps_nmea.msg_len = 0;
401 }
402 
403 
411  //reject empty lines
412  if (gps_nmea.msg_len == 0) {
413  if (c == '\r' || c == '\n' || c == '$')
414  return;
415  }
416 
417  // fill the buffer, unless it's full
418  if (gps_nmea.msg_len < NMEA_MAXLEN - 1) {
419 
420  // messages end with a linefeed
421  //AD: TRUNK: if (c == '\r' || c == '\n')
422  if (c == '\r' || c == '\n') {
424  } else {
426  gps_nmea.msg_len ++;
427  }
428  }
429 
430  if (gps_nmea.msg_len >= NMEA_MAXLEN - 1)
432 }
struct GpsNmea gps_nmea
Definition: gps_nmea.c:54
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
bool_t pos_available
Definition: gps_nmea.h:46
vector in EarthCenteredEarthFixed coordinates
int32_t lat
in radians*1e7
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
uint8_t zone
UTM zone number.
float y
in meters
void parse_nmea_GPGGA(void)
parse GPGGA-nmea-messages stored in gps_nmea.msg_buf .
Definition: gps_nmea.c:193
void nmea_parse_msg(void)
parse_nmea_char() has a complete line.
Definition: gps_nmea.c:371
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
uint8_t nav_utm_zone0
Definition: common_nav.c:44
void gps_impl_init(void)
Definition: gps_nmea.c:61
uint8_t gps_nb_ovrn
Definition: gps_nmea.h:47
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
float lat
in radians
#define NMEA_MAXLEN
Definition: gps_nmea.h:42
uint8_t fix
status of fix
Definition: gps.h:78
int msg_len
Definition: gps_nmea.h:49
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
vector in Latitude, Longitude and Altitude
void nmea_parse_char(uint8_t c)
This is the actual parser.
Definition: gps_nmea.c:410
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
uint8_t zone
UTM zone number.
float x
in meters
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
void parse_nmea_GPRMC(void)
parse GPRMC-nmea-messages stored in gps_nmea.msg_buf .
Definition: gps_nmea.c:114
#define NMEA_PRINT(...)
Definition: gps_nmea.h:39
float north
in meters
Device independent GPS code (interface)
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
char msg_buf[NMEA_MAXLEN]
buffer for storing one nmea-line
Definition: gps_nmea.h:48
int32_t alt
in millimeters above WGS84 reference ellipsoid
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
int32_t north
in centimeters
int32_t x
in centimeters
float alt
in meters above WGS84 reference ellipsoid
#define TRUE
Definition: imu_chimu.h:144
int32_t east
in centimeters
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
bool_t msg_available
Definition: gps_nmea.h:45
arch independent LED (Light Emitting Diodes) API
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
void parse_nmea_GPGSA(void)
parse GPGSA-nmea-messages stored in nmea_msg_buf .
Definition: gps_nmea.c:73
float lon
in radians
static struct point c
Definition: discsurvey.c:39
float east
in meters
float z
in meters
struct GpsState gps
global GPS state
Definition: gps.c:33
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t num_sv
number of sat in fix
Definition: gps.h:77