Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "gps_nmea.h"
34#include "modules/gps/gps.h"
35#include "modules/core/abi.h"
36#include "led.h"
37
39
40#include <inttypes.h>
41#include <string.h>
42#include <math.h>
43#include <stdlib.h>
44
45#ifndef NMEA_PRINT
46#define NMEA_PRINT(...) {};
47#endif
48
49#if NMEA_PRINT == printf
50#include <stdio.h>
51#endif
52
53/* line parser status */
54#define WAIT 0
55#define GOT_START 1
56#define GOT_CHECKSUM 2
57#define GOT_END 3
58
60
61static bool nmea_parse_GSA(void);
62static bool nmea_parse_RMC(void);
63static bool nmea_parse_GGA(void);
64static bool nmea_parse_GSV(void);
65
77
79{
80 struct link_device *dev = &((NMEA_GPS_LINK).device);
81
84 return;
85 }
86 while (dev->char_available(dev->periph)) {
87 nmea_parse_char(dev->get_byte(dev->periph));
90 gps_nmea.msg_available = false;
91 }
92 }
93}
94
95void nmea_gps_msg(void)
96{
97 // current timestamp
99
102
103 /* if a message was a valid/supported sentence, send update */
104 if (nmea_parse_msg()) {
105 if (gps_nmea.state.fix == GPS_FIX_3D) {
108 }
110 }
111}
112
114{
115 gps_nmea.is_configured = true;
116}
117
119{
120}
121
123{
124 return false;
125}
126
134{
135 bool msg_valid = false;
136
137 if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "RMC", 3)) {
139 NMEA_PRINT("RMC: \"%s\" \n\r", gps_nmea.msg_buf);
140 msg_valid = nmea_parse_RMC();
141 } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GGA", 3)) {
143 NMEA_PRINT("GGA: \"%s\" \n\r", gps_nmea.msg_buf);
144 msg_valid = nmea_parse_GGA();
145 } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSA", 3)) {
147 NMEA_PRINT("GSA: \"%s\" \n\r", gps_nmea.msg_buf);
148 msg_valid = nmea_parse_GSA();
149 } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSV", 3)) {
151 gps_nmea.have_gsv = true;
152 NMEA_PRINT("GSV: \"%s\" \n\r", gps_nmea.msg_buf);
153 msg_valid = nmea_parse_GSV();
154 } else {
156 NMEA_PRINT("Other/propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf);
157 msg_valid = nmea_parse_prop_msg();
158 }
159
160 // reset line parser
162
163 /* indicate if msg was valid/supported and gps_nmea.state updated */
164 return msg_valid;
165}
166
167
175{
176
177 switch (gps_nmea.status) {
178 case WAIT:
179 gps_nmea.msg_len = 0;
180 /* valid message needs to start with dollar sign */
181 if (c == '$') {
183 }
184 break;
185
186 case GOT_START:
187 switch (c) {
188 case '\r':
189 case '\n':
190 if (gps_nmea.msg_len == 0) {
191 //reject empty lines
193 break;
194 }
195 else {
196 // TODO: check for CRC before setting msg as available
198 gps_nmea.msg_available = true;
199 }
200 break;
201
202 case '$':
203 // got another dollar sign, msg incomplete: reset
205 NMEA_PRINT("nmea_parse_char: skipping incomplete msg: len=%i, \"%s\"\n\r",
208 break;
209
210 default:
211 // fill the buffer, unless it's full
212 if (gps_nmea.msg_len < NMEA_MAXLEN - 1) {
215 }
216 else {
218 NMEA_PRINT("nmea_parse_char: msg too long, len=%i, \"%s\"\n\r",
221 }
222 break;
223 }
224 break;
225
226 case GOT_CHECKSUM:
227 // TODO
228 break;
229
230 case GOT_END:
231 // shouldn't really happen, msg should be parsed and state reset before the next char
232 NMEA_PRINT("nmea_parse_char: this should not happen!");
233 break;
234
235 default:
236 break;
237 }
238}
239
244{
245 uint8_t chsum = 0,
246 it;
247
248 for (it = 0; it < buff_sz; ++it) {
249 chsum ^= buff[it];
250 }
251
252 return chsum;
253}
254
261static bool nmea_parse_GSA(void)
262{
263 int i = 6; // current position in the message, start after: GPGSA,
264
265 // attempt to reject empty packets right away
266 if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
267 NMEA_PRINT("p_GSA() - skipping empty message\n\r");
268 return false;
269 }
270
271 // get auto2D/3D
272 // ignored
273 nmea_read_until(&i);
274
275 // get 2D/3D-fix
276 // set gps_mode=3=3d, 2=2d, 1=no fix or 0
278 if (gps_nmea.state.fix == 1) {
279 gps_nmea.state.fix = 0;
280 }
281 NMEA_PRINT("p_GSA() - gps_nmea.state.fix=%i (3=3D)\n\r", gps_nmea.state.fix);
282 nmea_read_until(&i);
283
284 // up to 12 PRNs of satellites used for fix
285 int satcount = 0;
286 int prn_cnt;
287 for (prn_cnt = 0; prn_cnt < 12; prn_cnt++) {
288 if (gps_nmea.msg_buf[i] != ',') {
289 int prn = atoi(&gps_nmea.msg_buf[i]);
290 NMEA_PRINT("p_GSA() - PRN %i=%i\n\r", satcount, prn);
291 if (!gps_nmea.have_gsv) {
293 }
294 satcount++;
295 }
296 else {
297 if (!gps_nmea.have_gsv) {
299 }
300 }
301 nmea_read_until(&i);
302 }
303
304 // PDOP
305 float pdop = strtof(&gps_nmea.msg_buf[i], NULL);
306 gps_nmea.state.pdop = pdop * 100;
307 NMEA_PRINT("p_GSA() - pdop=%f\n\r", pdop);
308 nmea_read_until(&i);
309
310 // HDOP
311 float hdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL);
312 NMEA_PRINT("p_GSA() - hdop=%f\n\r", hdop);
313 nmea_read_until(&i);
314
315 // VDOP
316 float vdop __attribute__((unused)) = strtof(&gps_nmea.msg_buf[i], NULL);
317 NMEA_PRINT("p_GSA() - vdop=%f\n\r", vdop);
318 nmea_read_until(&i);
319
320 /* indicate that msg was valid and gps_nmea.state updated */
321 return true;
322}
323
330static bool nmea_parse_RMC(void)
331{
332 int i = 6; // current position in the message, start after: GPRMC,
333
334 // attempt to reject empty packets right away
335 if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
336 NMEA_PRINT("p_RMC() - skipping empty message\n\r");
337 return false;
338 }
339 // First read time (ignored)
340
341 // get warning
342 nmea_read_until(&i);
343
344 // get lat
345 nmea_read_until(&i);
346
347 // get North/South
348 nmea_read_until(&i);
349
350 // get lon
351 nmea_read_until(&i);
352
353 // get eath/west
354 nmea_read_until(&i);
355
356 // get speed
357 nmea_read_until(&i);
358 double speed = strtod(&gps_nmea.msg_buf[i], NULL);
359 gps_nmea.state.gspeed = speed * 1.852 * 100 / (60 * 60);
360 NMEA_PRINT("p_RMC() - ground-speed=%f knot = %d cm/s\n\r", speed, (gps_nmea.state.gspeed * 1000));
361
362 // get course
363 nmea_read_until(&i);
364 double course = strtod(&gps_nmea.msg_buf[i], NULL);
366 NMEA_PRINT("p_RMC() - course: %f deg\n\r", course);
368
369 /* indicate that msg was valid and gps_nmea.state updated */
370 return true;
371}
372
373
380static bool nmea_parse_GGA(void)
381{
382 int i = 6; // current position in the message, start after: GPGGA,
383 double degrees, minutesfrac;
384 struct LlaCoor_f lla_f;
385
386 // attempt to reject empty packets right away
387 if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
388 NMEA_PRINT("p_GGA() - skipping empty message\n\r");
389 return false;
390 }
391
392 // get UTC time [hhmmss.sss]
393 // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL);
394 // FIXME: parse UTC time correctly
395 double utc_time = strtod(&gps_nmea.msg_buf[i], NULL);
396 gps_nmea.state.tow = (uint32_t)((utc_time + 1) * 1000);
397
398 // get latitude [ddmm.mmmmm]
399 nmea_read_until(&i);
400 double lat = strtod(&gps_nmea.msg_buf[i], NULL);
401 // convert to pure degrees [dd.dddd] format
402 minutesfrac = modf(lat / 100, &degrees);
403 lat = degrees + (minutesfrac * 100) / 60;
404
405 // get latitute N/S
406 nmea_read_until(&i);
407 if (gps_nmea.msg_buf[i] == 'S') {
408 lat = -lat;
409 }
410
411 // convert to radians
412 lla_f.lat = RadOfDeg(lat);
413 gps_nmea.state.lla_pos.lat = lat * 1e7; // convert to fixed-point
414 NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat);
415
416
417 // get longitude [ddmm.mmmmm]
418 nmea_read_until(&i);
419 double lon = strtod(&gps_nmea.msg_buf[i], NULL);
420 // convert to pure degrees [dd.dddd] format
421 minutesfrac = modf(lon / 100, &degrees);
422 lon = degrees + (minutesfrac * 100) / 60;
423
424 // get longitude E/W
425 nmea_read_until(&i);
426 if (gps_nmea.msg_buf[i] == 'W') {
427 lon = -lon;
428 }
429
430 // convert to radians
431 lla_f.lon = RadOfDeg(lon);
432 gps_nmea.state.lla_pos.lon = lon * 1e7; // convert to fixed-point
433 NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps_nmea.state.tow);
435
436 // get position fix status
437 nmea_read_until(&i);
438 // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
439 // check for good position fix
440 if ((gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',')) {
441 NMEA_PRINT("p_GGA() - POS_AVAILABLE == TRUE\n\r");
442 } else {
443 NMEA_PRINT("p_GGA() - gps_pos_available == false\n\r");
444 }
445
446 // get number of satellites used in GPS solution
447 nmea_read_until(&i);
449 NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps_nmea.state.num_sv);
450
451 // get HDOP, but we use PDOP from GSA message
452 nmea_read_until(&i);
453 //float hdop = strtof(&gps_nmea.msg_buf[i], NULL);
454 //gps_nmea.state.pdop = hdop * 100;
455
456 // get altitude (in meters) above geoid (MSL)
457 nmea_read_until(&i);
458 float hmsl = strtof(&gps_nmea.msg_buf[i], NULL);
459 gps_nmea.state.hmsl = hmsl * 1000;
460 NMEA_PRINT("p_GGA() - gps_nmea.state.hmsl=%i\n\r", gps_nmea.state.hmsl);
462
463 // get altitude units (always M)
464 nmea_read_until(&i);
465
466 // get geoid seperation
467 nmea_read_until(&i);
468 float geoid = strtof(&gps_nmea.msg_buf[i], NULL);
469 NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid);
470 // height above ellipsoid
471 lla_f.alt = hmsl + geoid;
472 gps_nmea.state.lla_pos.alt = lla_f.alt * 1000;
473 NMEA_PRINT("p_GGA() - gps_nmea.state.alt=%i\n\r", gps_nmea.state.lla_pos.alt);
474
475 // get seperations units
476 nmea_read_until(&i);
477 // get DGPS age
478 nmea_read_until(&i);
479 // get DGPS station ID
480
481 /* convert to ECEF */
482 struct EcefCoor_f ecef_f;
483 ecef_of_lla_f(&ecef_f, &lla_f);
484 gps_nmea.state.ecef_pos.x = ecef_f.x * 100;
485 gps_nmea.state.ecef_pos.y = ecef_f.y * 100;
486 gps_nmea.state.ecef_pos.z = ecef_f.z * 100;
488
489 /* indicate that msg was valid and gps_nmea.state updated */
490 return true;
491}
492
498static bool nmea_parse_GSV(void)
499{
500 int i = 6; // current position in the message, start after: GxGSA,
501
502 // attempt to reject empty packets right away
503 if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
504 NMEA_PRINT("p_GSV() - skipping empty message\n\r");
505 return false;
506 }
507
508 // check what satellites this messages contains
509 // GPGSV -> GPS
510 // GLGSV -> GLONASS
511 bool is_glonass = false;
512 if (!strncmp(&gps_nmea.msg_buf[0] , "GL", 2)) {
513 is_glonass = true;
514 }
515
516 // total sentences
517 int nb_sen __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]);
518 NMEA_PRINT("p_GSV() - %i sentences\n\r", nb_sen);
519 nmea_read_until(&i);
520
521 // current sentence
522 int cur_sen = atoi(&gps_nmea.msg_buf[i]);
523 NMEA_PRINT("p_GSV() - sentence=%i\n\r", cur_sen);
524 nmea_read_until(&i);
525
526 // num satellites in view
527 int num_sat __attribute__((unused)) = atoi(&gps_nmea.msg_buf[i]);
528 NMEA_PRINT("p_GSV() - num_sat=%i\n\r", num_sat);
529 nmea_read_until(&i);
530
531 // up to 4 sats per sentence
532 int sat_cnt;
533 for (sat_cnt = 0; sat_cnt < 4; sat_cnt++) {
534 if (gps_nmea.msg_buf[i] == ',') break;
535 // 4 fields per sat: PRN, elevation (deg), azimuth (deg), SNR
536 int prn = atoi(&gps_nmea.msg_buf[i]);
537 nmea_read_until(&i);
538 int elev = atoi(&gps_nmea.msg_buf[i]);
539 nmea_read_until(&i);
540 int azim = atoi(&gps_nmea.msg_buf[i]);
541 nmea_read_until(&i);
542 int snr = atoi(&gps_nmea.msg_buf[i]);
543 nmea_read_until(&i);
544
545 int ch_idx = (cur_sen - 1) * 4 + sat_cnt;
546 // don't populate svinfos with GLONASS sats for now
547 if (!is_glonass && ch_idx > 0 && ch_idx < 12) {
552 }
553 if (is_glonass) {
554 NMEA_PRINT("p_GSV() - GLONASS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
555 }
556 else {
557 NMEA_PRINT("p_GSV() - GPS %i PRN=%i elev=%i azim=%i snr=%i\n\r", ch_idx, prn, elev, azim, snr);
558 }
559 }
560
561 /* indicate that msg was valid and gps_nmea.state updated */
562 return true;
563}
Main include for ABI (AirBorneInterface).
#define GPS_NMEA_ID
static int16_t course[3]
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Device independent GPS code (interface)
int16_t azim
azimuth in deg
Definition gps.h:83
uint32_t tow
GPS time of week in ms.
Definition gps.h:109
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:94
int8_t elev
elevation in deg
Definition gps.h:82
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:92
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition gps.h:99
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition gps.h:81
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition gps.h:91
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition gps.h:114
uint16_t pdop
position dilution of precision scaled by 100
Definition gps.h:105
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition gps.h:117
uint8_t svid
Satellite ID.
Definition gps.h:78
uint8_t nb_channels
Number of scanned satellites.
Definition gps.h:111
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition gps.h:115
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition gps.h:97
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:88
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition gps.h:112
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition gps.h:116
uint8_t num_sv
number of sat in fix
Definition gps.h:106
uint8_t fix
status of fix
Definition gps.h:107
#define GOT_CHECKSUM
Definition gps_nmea.c:56
struct GpsNmea gps_nmea
Definition gps_nmea.c:59
static bool nmea_parse_RMC(void)
Parse RMC NMEA messages.
Definition gps_nmea.c:330
bool nmea_parse_msg(void)
nmea_parse_char() has a complete line.
Definition gps_nmea.c:133
#define GOT_END
Definition gps_nmea.c:57
void gps_nmea_init(void)
Definition gps_nmea.c:66
static bool nmea_parse_GSV(void)
Parse GSV-nmea-messages.
Definition gps_nmea.c:498
bool WEAK nmea_parse_prop_msg(void)
Definition gps_nmea.c:122
uint8_t nmea_calc_crc(const char *buff, int buff_sz)
Calculate control sum of binary buffer.
Definition gps_nmea.c:243
static bool nmea_parse_GSA(void)
Parse GSA NMEA messages.
Definition gps_nmea.c:261
void gps_nmea_event(void)
Definition gps_nmea.c:78
void WEAK nmea_parse_prop_init(void)
Definition gps_nmea.c:118
void nmea_gps_msg(void)
Definition gps_nmea.c:95
void WEAK nmea_configure(void)
The function to be called when a characted from the device is available.
Definition gps_nmea.c:113
#define NMEA_PRINT(...)
Definition gps_nmea.c:46
#define WAIT
Definition gps_nmea.c:54
void nmea_parse_char(uint8_t c)
This is the actual parser.
Definition gps_nmea.c:174
#define GOT_START
Definition gps_nmea.c:55
static bool nmea_parse_GGA(void)
Parse GGA NMEA messages.
Definition gps_nmea.c:380
NMEA protocol specific code.
#define GPS_NMEA_NB_CHANNELS
Definition gps_nmea.h:35
uint8_t status
line parser status
Definition gps_nmea.h:50
int msg_len
Definition gps_nmea.h:49
static void nmea_read_until(int *i)
Read until a certain character, placed here for proprietary includes.
Definition gps_nmea.h:79
struct GpsState state
Definition gps_nmea.h:52
bool have_gsv
flag set to TRUE if GPGSV message received
Definition gps_nmea.h:46
uint8_t gps_nb_ovrn
number if incomplete nmea-messages
Definition gps_nmea.h:47
#define NMEA_MAXLEN
Definition gps_nmea.h:37
bool msg_available
flag set to TRUE if a new msg/sentence is available to be parsed
Definition gps_nmea.h:44
bool is_configured
flag set to TRUE if configuration is finished
Definition gps_nmea.h:45
char msg_buf[NMEA_MAXLEN]
buffer for storing one nmea-line
Definition gps_nmea.h:48
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
in centimeters
int32_t x
in centimeters
int32_t y
in centimeters
int32_t lon
in degrees*1e7
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
arch independent LED (Light Emitting Diodes) API
uint16_t foo
Definition main_demo5.c:58
void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in)
Paparazzi floating point math for geodetic calculations.
float alt
in meters (normally above WGS84 reference ellipsoid)
float lon
in radians
float lat
in radians
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:73
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.