Paparazzi UAS v7.1_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
32
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
59struct GpsNmea gps_nmea = {0};
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
66void gps_nmea_init(void)
67{
68 gps_nmea.state.nb_channels = GPS_NMEA_NB_CHANNELS;
69 gps_nmea.is_configured = false;
70 gps_nmea.msg_available = false;
71 gps_nmea.have_gsv = false;
72 gps_nmea.gps_nb_ovrn = 0;
73 gps_nmea.msg_len = 0;
76}
77
79{
80 struct link_device *dev = &((NMEA_GPS_LINK).device);
81
82 if (!gps_nmea.is_configured) {
84 return;
85 }
86 while (dev->char_available(dev->periph)) {
87 nmea_parse_char(dev->get_byte(dev->periph));
88 if (gps_nmea.msg_available) {
90 gps_nmea.msg_available = false;
91 }
92 }
93}
94
95void nmea_gps_msg(void)
96{
97 // current timestamp
98 uint32_t now_ts = get_sys_time_usec();
99
100 gps_nmea.state.last_msg_ticks = sys_time.nb_sec_rem;
101 gps_nmea.state.last_msg_time = sys_time.nb_sec;
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) {
106 gps_nmea.state.last_3dfix_ticks = sys_time.nb_sec_rem;
107 gps_nmea.state.last_3dfix_time = sys_time.nb_sec;
108 }
109 AbiSendMsgGPS(GPS_NMEA_ID, now_ts, &gps_nmea.state);
110 }
111}
112
113void WEAK nmea_configure(void)
114{
115 gps_nmea.is_configured = true;
116}
117
118void WEAK nmea_parse_prop_init(void)
119{
120}
121
122bool WEAK nmea_parse_prop_msg(void)
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)) {
138 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
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)) {
142 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
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)) {
146 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
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)) {
150 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
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 {
155 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
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
161 gps_nmea.status = WAIT;
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 == '$') {
182 gps_nmea.status = GOT_START;
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
192 gps_nmea.status = WAIT;
193 break;
194 }
195 else {
196 // TODO: check for CRC before setting msg as available
197 gps_nmea.status = GOT_END;
198 gps_nmea.msg_available = true;
199 }
200 break;
201
202 case '$':
203 // got another dollar sign, msg incomplete: reset
204 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
205 NMEA_PRINT("nmea_parse_char: skipping incomplete msg: len=%i, \"%s\"\n\r",
206 gps_nmea.msg_len, gps_nmea.msg_buf);
207 gps_nmea.status = WAIT;
208 break;
209
210 default:
211 // fill the buffer, unless it's full
212 if (gps_nmea.msg_len < NMEA_MAXLEN - 1) {
213 gps_nmea.msg_buf[gps_nmea.msg_len] = c;
214 gps_nmea.msg_len++;
215 }
216 else {
217 gps_nmea.msg_buf[gps_nmea.msg_len] = 0;
218 NMEA_PRINT("nmea_parse_char: msg too long, len=%i, \"%s\"\n\r",
219 gps_nmea.msg_len, gps_nmea.msg_buf);
220 gps_nmea.status = WAIT;
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
243uint8_t nmea_calc_crc(const char *buff, int buff_sz)
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
277 gps_nmea.state.fix = atoi(&gps_nmea.msg_buf[i]);
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) {
292 gps_nmea.state.svinfos[prn_cnt].svid = prn;
293 }
294 satcount++;
295 }
296 else {
297 if (!gps_nmea.have_gsv) {
298 gps_nmea.state.svinfos[prn_cnt].svid = 0;
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);
365 gps_nmea.state.course = RadOfDeg(course) * 1e7;
366 NMEA_PRINT("p_RMC() - course: %f deg\n\r", course);
367 SetBit(gps_nmea.state.valid_fields, GPS_VALID_COURSE_BIT);
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);
434 SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_LLA_BIT);
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);
448 gps_nmea.state.num_sv = atoi(&gps_nmea.msg_buf[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);
461 SetBit(gps_nmea.state.valid_fields, GPS_VALID_HMSL_BIT);
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;
487 SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
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) {
548 gps_nmea.state.svinfos[ch_idx].svid = prn;
549 gps_nmea.state.svinfos[ch_idx].cno = snr;
550 gps_nmea.state.svinfos[ch_idx].elev = elev;
551 gps_nmea.state.svinfos[ch_idx].azim = azim;
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.
static struct uart_periph * dev
Device independent GPS code (interface).
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
#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
static void nmea_read_until(int *i)
Read until a certain character, placed here for proprietary includes.
Definition gps_nmea.h:79
#define NMEA_MAXLEN
Definition gps_nmea.h:37
arch independent LED (Light Emitting Diodes) API
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 z
in meters
float lon
in radians
float x
in meters
float lat
in radians
float y
in meters
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.