Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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  * 2014 Freek van Tienen <freek.v.tienen@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
33 #include "subsystems/gps.h"
34 #include "subsystems/abi.h"
35 #include "led.h"
36 
37 #if GPS_USE_LATLONG
38 /* currently needed to get nav_utm_zone0 */
40 #endif
42 
43 #include <inttypes.h>
44 #include <string.h>
45 #include <math.h>
46 #include <stdlib.h>
47 
48 #ifndef NMEA_PRINT
49 #define NMEA_PRINT(...) {};
50 #endif
51 
52 #if NMEA_PRINT == printf
53 #include <stdio.h>
54 #endif
55 
56 /* line parser status */
57 #define WAIT 0
58 #define GOT_START 1
59 #define GOT_CHECKSUM 2
60 #define GOT_END 3
61 
63 
64 static void nmea_parse_GSA(void);
65 static void nmea_parse_RMC(void);
66 static void nmea_parse_GGA(void);
67 static void nmea_parse_GSV(void);
68 
69 
70 void gps_impl_init(void)
71 {
78  gps_nmea.msg_len = 0;
81 }
82 
83 void gps_nmea_msg(void)
84 {
85  // current timestamp
86  uint32_t now_ts = get_sys_time_usec();
87 
91  if (gps_nmea.pos_available) {
92  if (gps.fix == GPS_FIX_3D) {
95  }
96  AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps);
97  }
99 }
100 
101 void WEAK nmea_configure(void)
102 {
104 }
105 
106 void WEAK nmea_parse_prop_init(void)
107 {
108 }
109 
110 void WEAK nmea_parse_prop_msg(void)
111 {
112 }
113 
119 void nmea_parse_msg(void)
120 {
121 
122  if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "RMC", 3)) {
124  NMEA_PRINT("RMC: \"%s\" \n\r", gps_nmea.msg_buf);
125  nmea_parse_RMC();
126  } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GGA", 3)) {
128  NMEA_PRINT("GGA: \"%s\" \n\r", gps_nmea.msg_buf);
129  nmea_parse_GGA();
130  } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSA", 3)) {
132  NMEA_PRINT("GSA: \"%s\" \n\r", gps_nmea.msg_buf);
133  nmea_parse_GSA();
134  } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSV", 3)) {
137  NMEA_PRINT("GSV: \"%s\" \n\r", gps_nmea.msg_buf);
138  nmea_parse_GSV();
139  } else {
141  NMEA_PRINT("Other/propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf);
143  }
144 
145  // reset line parser
146  gps_nmea.status = WAIT;
147 }
148 
149 
157 {
158 
159  switch (gps_nmea.status) {
160  case WAIT:
161  gps_nmea.msg_len = 0;
162  /* valid message needs to start with dollar sign */
163  if (c == '$') {
165  }
166  break;
167 
168  case GOT_START:
169  switch (c) {
170  case '\r':
171  case '\n':
172  if (gps_nmea.msg_len == 0) {
173  //reject empty lines
174  gps_nmea.status = WAIT;
175  break;
176  }
177  else {
178  // TODO: check for CRC before setting msg as available
181  }
182  break;
183 
184  case '$':
185  // got another dollar sign, msg incomplete: reset
187  NMEA_PRINT("nmea_parse_char: skipping incomplete msg: len=%i, \"%s\"\n\r",
189  gps_nmea.status = WAIT;
190  break;
191 
192  default:
193  // fill the buffer, unless it's full
194  if (gps_nmea.msg_len < NMEA_MAXLEN - 1) {
196  gps_nmea.msg_len++;
197  }
198  else {
200  NMEA_PRINT("nmea_parse_char: msg too long, len=%i, \"%s\"\n\r",
202  gps_nmea.status = WAIT;
203  }
204  break;
205  }
206  break;
207 
208  case GOT_CHECKSUM:
209  // TODO
210  break;
211 
212  case GOT_END:
213  // shouldn't really happen, msg should be parsed and state reset before the next char
214  NMEA_PRINT("nmea_parse_char: this should not happen!");
215  break;
216 
217  default:
218  break;
219  }
220 }
221 
225 uint8_t nmea_calc_crc(const char *buff, int buff_sz)
226 {
227  uint8_t chsum = 0,
228  it;
229 
230  for (it = 0; it < buff_sz; ++it) {
231  chsum ^= buff[it];
232  }
233 
234  return chsum;
235 }
236 
242 static void nmea_parse_GSA(void)
243 {
244  int i = 6; // current position in the message, start after: GPGSA,
245 
246  // attempt to reject empty packets right away
247  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
248  NMEA_PRINT("p_GSA() - skipping empty message\n\r");
249  return;
250  }
251 
252  // get auto2D/3D
253  // ignored
254  nmea_read_until(&i);
255 
256  // get 2D/3D-fix
257  // set gps_mode=3=3d, 2=2d, 1=no fix or 0
258  gps.fix = atoi(&gps_nmea.msg_buf[i]);
259  if (gps.fix == 1) {
260  gps.fix = 0;
261  }
262  NMEA_PRINT("p_GSA() - gps.fix=%i (3=3D)\n\r", gps.fix);
263  nmea_read_until(&i);
264 
265  // up to 12 PRNs of satellites used for fix
266  int satcount = 0;
267  int prn_cnt;
268  for (prn_cnt = 0; prn_cnt < 12; prn_cnt++) {
269  if (gps_nmea.msg_buf[i] != ',') {
270  int prn = atoi(&gps_nmea.msg_buf[i]);
271  NMEA_PRINT("p_GSA() - PRN %i=%i\n\r", satcount, prn);
272  if (!gps_nmea.have_gsv) {
273  gps.svinfos[prn_cnt].svid = prn;
274  }
275  satcount++;
276  }
277  else {
278  if (!gps_nmea.have_gsv) {
279  gps.svinfos[prn_cnt].svid = 0;
280  }
281  }
282  nmea_read_until(&i);
283  }
284 
285  // PDOP
286  float pdop = strtof(&gps_nmea.msg_buf[i], NULL);
287  gps.pdop = pdop * 100;
288  NMEA_PRINT("p_GSA() - pdop=%f\n\r", pdop);
289  nmea_read_until(&i);
290 
291  // HDOP
292  float hdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL);
293  NMEA_PRINT("p_GSA() - hdop=%f\n\r", hdop);
294  nmea_read_until(&i);
295 
296  // VDOP
297  float vdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL);
298  NMEA_PRINT("p_GSA() - vdop=%f\n\r", vdop);
299  nmea_read_until(&i);
300 
301 }
302 
308 static void nmea_parse_RMC(void)
309 {
310  int i = 6; // current position in the message, start after: GPRMC,
311 
312  // attempt to reject empty packets right away
313  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
314  NMEA_PRINT("p_RMC() - skipping empty message\n\r");
315  return;
316  }
317  // First read time (ignored)
318 
319  // get warning
320  nmea_read_until(&i);
321 
322  // get lat
323  nmea_read_until(&i);
324 
325  // get North/South
326  nmea_read_until(&i);
327 
328  // get lon
329  nmea_read_until(&i);
330 
331  // get eath/west
332  nmea_read_until(&i);
333 
334  // get speed
335  nmea_read_until(&i);
336  double speed = strtod(&gps_nmea.msg_buf[i], NULL);
337  gps.gspeed = speed * 1.852 * 100 / (60 * 60);
338  NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps.gspeed * 1000));
339 
340  // get course
341  nmea_read_until(&i);
342  double course = strtod(&gps_nmea.msg_buf[i], NULL);
343  gps.course = RadOfDeg(course) * 1e7;
344  NMEA_PRINT("p_RMC() - course: %f deg\n\r", course);
345 }
346 
347 
353 static void nmea_parse_GGA(void)
354 {
355  int i = 6; // current position in the message, start after: GPGGA,
356  double degrees, minutesfrac;
357  struct LlaCoor_f lla_f;
358 
359  // attempt to reject empty packets right away
360  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
361  NMEA_PRINT("p_GGA() - skipping empty message\n\r");
362  return;
363  }
364 
365  // get UTC time [hhmmss.sss]
366  // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL);
367  // FIXME: parse UTC time correctly
368  double time = strtod(&gps_nmea.msg_buf[i], NULL);
369  gps.tow = (uint32_t)((time + 1) * 1000);
370 
371  // get latitude [ddmm.mmmmm]
372  nmea_read_until(&i);
373  double lat = strtod(&gps_nmea.msg_buf[i], NULL);
374  // convert to pure degrees [dd.dddd] format
375  minutesfrac = modf(lat / 100, &degrees);
376  lat = degrees + (minutesfrac * 100) / 60;
377 
378  // get latitute N/S
379  nmea_read_until(&i);
380  if (gps_nmea.msg_buf[i] == 'S') {
381  lat = -lat;
382  }
383 
384  // convert to radians
385  lla_f.lat = RadOfDeg(lat);
386  gps.lla_pos.lat = lat * 1e7; // convert to fixed-point
387  NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat);
388 
389 
390  // get longitude [ddmm.mmmmm]
391  nmea_read_until(&i);
392  double lon = strtod(&gps_nmea.msg_buf[i], NULL);
393  // convert to pure degrees [dd.dddd] format
394  minutesfrac = modf(lon / 100, &degrees);
395  lon = degrees + (minutesfrac * 100) / 60;
396 
397  // get longitude E/W
398  nmea_read_until(&i);
399  if (gps_nmea.msg_buf[i] == 'W') {
400  lon = -lon;
401  }
402 
403  // convert to radians
404  lla_f.lon = RadOfDeg(lon);
405  gps.lla_pos.lon = lon * 1e7; // convert to fixed-point
406  NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow);
407 
408  // get position fix status
409  nmea_read_until(&i);
410  // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
411  // check for good position fix
412  if ((gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',')) {
414  NMEA_PRINT("p_GGA() - POS_AVAILABLE == TRUE\n\r");
415  } else {
417  NMEA_PRINT("p_GGA() - gps_pos_available == false\n\r");
418  }
419 
420  // get number of satellites used in GPS solution
421  nmea_read_until(&i);
422  gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
423  NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
424 
425  // get HDOP, but we use PDOP from GSA message
426  nmea_read_until(&i);
427  //float hdop = strtof(&gps_nmea.msg_buf[i], NULL);
428  //gps.pdop = hdop * 100;
429 
430  // get altitude (in meters) above geoid (MSL)
431  nmea_read_until(&i);
432  float hmsl = strtof(&gps_nmea.msg_buf[i], NULL);
433  gps.hmsl = hmsl * 1000;
434  NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl);
435 
436  // get altitude units (always M)
437  nmea_read_until(&i);
438 
439  // get geoid seperation
440  nmea_read_until(&i);
441  float geoid = strtof(&gps_nmea.msg_buf[i], NULL);
442  NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid);
443  // height above ellipsoid
444  lla_f.alt = hmsl + geoid;
445  gps.lla_pos.alt = lla_f.alt * 1000;
446  NMEA_PRINT("p_GGA() - gps.alt=%i\n\r", gps.lla_pos.alt);
447 
448  // get seperations units
449  nmea_read_until(&i);
450  // get DGPS age
451  nmea_read_until(&i);
452  // get DGPS station ID
453 
454 #if GPS_USE_LATLONG
455  /* convert to utm */
456  struct UtmCoor_f utm_f;
457  utm_f.zone = nav_utm_zone0;
458  utm_of_lla_f(&utm_f, &lla_f);
459 
460  /* copy results of utm conversion */
461  gps.utm_pos.east = utm_f.east * 100;
462  gps.utm_pos.north = utm_f.north * 100;
465 #endif
466 
467  /* convert to ECEF */
468  struct EcefCoor_f ecef_f;
469  ecef_of_lla_f(&ecef_f, &lla_f);
470  gps.ecef_pos.x = ecef_f.x * 100;
471  gps.ecef_pos.y = ecef_f.y * 100;
472  gps.ecef_pos.z = ecef_f.z * 100;
473 }
474 
479 static void nmea_parse_GSV(void)
480 {
481  int i = 6; // current position in the message, start after: GxGSA,
482 
483  // attempt to reject empty packets right away
484  if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
485  NMEA_PRINT("p_GSV() - skipping empty message\n\r");
486  return;
487  }
488 
489  // check what satellites this messages contains
490  // GPGSV -> GPS
491  // GLGSV -> GLONASS
492  bool_t is_glonass = FALSE;
493  if (!strncmp(&gps_nmea.msg_buf[0] , "GL", 2)) {
494  is_glonass = TRUE;
495  }
496 
497  // total sentences
498  int nb_sen __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]);
499  NMEA_PRINT("p_GSV() - %i sentences\n\r", nb_sen);
500  nmea_read_until(&i);
501 
502  // current sentence
503  int cur_sen = atoi(&gps_nmea.msg_buf[i]);
504  NMEA_PRINT("p_GSV() - sentence=%i\n\r", cur_sen);
505  nmea_read_until(&i);
506 
507  // num satellites in view
508  int num_sat __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]);
509  NMEA_PRINT("p_GSV() - num_sat=%i\n\r", num_sat);
510  nmea_read_until(&i);
511 
512  // up to 4 sats per sentence
513  int sat_cnt;
514  for (sat_cnt = 0; sat_cnt < 4; sat_cnt++) {
515  if (gps_nmea.msg_buf[i] == ',') break;
516  // 4 fields per sat: PRN, elevation (deg), azimuth (deg), SNR
517  int prn = atoi(&gps_nmea.msg_buf[i]);
518  nmea_read_until(&i);
519  int elev = atoi(&gps_nmea.msg_buf[i]);
520  nmea_read_until(&i);
521  int azim = atoi(&gps_nmea.msg_buf[i]);
522  nmea_read_until(&i);
523  int snr = atoi(&gps_nmea.msg_buf[i]);
524  nmea_read_until(&i);
525 
526  int ch_idx = (cur_sen - 1) * 4 + sat_cnt;
527  // don't populate svinfos with GLONASS sats for now
528  if (!is_glonass && ch_idx > 0 && ch_idx < 12) {
529  gps.svinfos[ch_idx].svid = prn;
530  gps.svinfos[ch_idx].cno = snr;
531  gps.svinfos[ch_idx].elev = elev;
532  gps.svinfos[ch_idx].azim = azim;
533  }
534  if (is_glonass) {
535  NMEA_PRINT("p_GSV() - GLONASS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
536  }
537  else {
538  NMEA_PRINT("p_GSV() - GPS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
539  }
540  }
541 }
struct GpsNmea gps_nmea
Definition: gps_nmea.c:62
int32_t z
in centimeters
int32_t north
in centimeters
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:87
int16_t azim
azimuth in deg
Definition: gps.h:63
float east
in meters
void gps_nmea_msg(void)
Definition: gps_nmea.c:83
float north
in meters
#define GOT_END
Definition: gps_nmea.c:60
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:86
vector in EarthCenteredEarthFixed coordinates
int32_t y
in centimeters
void nmea_parse_msg(void)
nmea_parse_char() has a complete line.
Definition: gps_nmea.c:119
void WEAK nmea_parse_prop_init(void)
Definition: gps_nmea.c:106
char msg_buf[NMEA_MAXLEN]
buffer for storing one nmea-line
Definition: gps_nmea.h:45
Main include for ABI (AirBorneInterface).
#define GPS_NMEA_ID
static void nmea_read_until(int *i)
Read until a certain character, placed here for proprietary includes.
Definition: gps_nmea.h:85
uint8_t nav_utm_zone0
Definition: common_nav.c:44
void gps_impl_init(void)
GPS initialization.
Definition: gps_nmea.c:70
float x
in meters
#define NMEA_PRINT(...)
Definition: gps_nmea.c:49
position in UTM coordinates Units: meters
int32_t east
in centimeters
#define NMEA_MAXLEN
Definition: gps_nmea.h:37
uint8_t svid
Satellite ID.
Definition: gps.h:58
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:70
uint8_t gps_nb_ovrn
number if incomplete nmea-messages
Definition: gps_nmea.h:44
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:89
int8_t elev
elevation in deg
Definition: gps.h:62
void nmea_parse_char(uint8_t c)
This is the actual parser.
Definition: gps_nmea.c:156
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:92
uint8_t zone
UTM zone number.
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
bool_t have_gsv
flag set to TRUE if GPGSV message received
Definition: gps_nmea.h:43
#define TRUE
Definition: std.h:4
static void nmea_parse_GGA(void)
Parse GGA NMEA messages.
Definition: gps_nmea.c:353
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:71
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:61
float z
in meters
uint32_t tow
GPS time of week in ms.
Definition: gps.h:84
Device independent GPS code (interface)
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:80
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:68
bool_t is_configured
flag set to TRUE if configuration is finished
Definition: gps_nmea.h:42
#define GOT_CHECKSUM
Definition: gps_nmea.c:59
int32_t lon
in degrees*1e7
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
#define WAIT
Definition: gps_nmea.c:57
uint8_t zone
UTM zone number.
float y
in meters
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:70
uint8_t status
line parser status
Definition: gps_nmea.h:47
static void nmea_parse_GSV(void)
Parse GSV-nmea-messages.
Definition: gps_nmea.c:479
float alt
in meters above WGS84 reference ellipsoid
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:90
void WEAK nmea_configure(void)
The function to be called when a characted from the device is available.
Definition: gps_nmea.c:101
int32_t alt
in millimeters above WGS84 reference ellipsoid
unsigned char uint8_t
Definition: types.h:14
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:76
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:69
uint8_t nmea_calc_crc(const char *buff, int buff_sz)
Calculate control sum of binary buffer.
Definition: gps_nmea.c:225
bool_t msg_available
Definition: gps_nmea.h:40
static void nmea_parse_GSA(void)
Parse GSA NMEA messages.
Definition: gps_nmea.c:242
#define GOT_START
Definition: gps_nmea.c:58
int msg_len
Definition: gps_nmea.h:46
float lon
in radians
void WEAK nmea_parse_prop_msg(void)
Definition: gps_nmea.c:110
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:91
uint8_t num_sv
number of sat in fix
Definition: gps.h:81
arch independent LED (Light Emitting Diodes) API
int32_t x
in centimeters
static void nmea_parse_RMC(void)
Parse RMC NMEA messages.
Definition: gps_nmea.c:308
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:74
float lat
in radians
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:69
bool_t pos_available
Definition: gps_nmea.h:41
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:82
struct GpsState gps
global GPS state
Definition: gps.c:41
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)