protocol_nmea.cpp
Go to the documentation of this file.
1 #include <stdint.h>
2 #include "protocol_nmea.h"
3 #include "ISPose.h"
4 #include "ISEarth.h"
5 
6 
8 // Utility functions
10 
11 uint32_t ASCII_compute_checksum(uint8_t* str, int size)
12 {
13  uint32_t checksum = 0;
14 
15  uint8_t *end = str + size;
16  for(uint8_t *ptr=str; ptr<end; ptr++)
17  {
18  checksum ^= *ptr;
19  }
20 
21  return checksum;
22 }
23 
24 // All strings must be NULL terminated!
25 char *ASCII_find_next_field(char *str)
26 {
27  while(*str != 0 && *str != ',') //move down looking for end of string.
28  ++str;
29 
30  if(*str == ',') //move past comma (if not at end of string)
31  ++str;
32 
33  return str;
34 }
35 
36 double ddmm2deg(double ddmm)
37 {
38  double deg = (int)ddmm / 100 ;
39  ddmm -= deg * 100 ;
40  return deg + (ddmm / 60) ;
41 }
42 
43 void set_gpsPos_status_mask(uint32_t *status, uint32_t state, uint32_t mask)
44 {
45  *status &= ~mask;
46  *status |= state & mask;
47 }
48 
49 /* convert calendar day/time to time -------------------------------------------
50 * convert calendar day/time to gtime_t struct
51 * args : double *ep I day/time {year,month,day,hour,min,sec}
52 * return : gtime_t struct
53 * notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
54 *-----------------------------------------------------------------------------*/
55 gtime_t epoch2time(const double *ep)
56 {
57  const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
58  gtime_t time={0};
59  int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
60 
61  if (year<1970||2099<year||mon<1||12<mon) return time;
62 
63  /* leap year if year%4==0 in 1901-2099 */
64  days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
65  sec=(int)floor(ep[5]);
66  time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
67  time.sec=ep[5]-sec;
68  return time;
69 }
70 
71 static const double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */
72 
73 /* time to gps time ------------------------------------------------------------
74 * convert gtime_t struct to week and tow in gps time
75 * args : gtime_t t I gtime_t struct
76 * int *week IO week number in gps time (NULL: no output)
77 * return : time of week in gps time (s)
78 *-----------------------------------------------------------------------------*/
79 double time2gpst(gtime_t t, int *week)
80 {
81  gtime_t t0=epoch2time(gpst0);
82  time_t sec=t.time-t0.time;
83  time_t w=(time_t)(sec/(86400*7));
84 
85  if (week) *week=(int)w;
86  return (double)(sec-(double)w*86400*7)+t.sec;
87 }
88 
89 
91 // DID to NMEA
93 
94 int dev_info_to_nmea_info(char a[], const int aSize, dev_info_t &info)
95 {
96 // unsigned int checkSum = 0;
97 // serWrite(portNum, (unsigned char*)"$", 1);
98 // ASCII_PORT_WRITE_NO_FORMAT(portNum, "INFO", checkSum, 4);
99 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.serialNumber); // 1
100 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.hardwareVer[0]); // 2
101 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.hardwareVer[1]);
102 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.hardwareVer[2]);
103 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.hardwareVer[3]);
104 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.firmwareVer[0]); // 3
105 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.firmwareVer[1]);
106 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.firmwareVer[2]);
107 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.firmwareVer[3]);
108 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.buildNumber); // 4
109 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.protocolVer[0]); // 5
110 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.protocolVer[1]);
111 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.protocolVer[2]);
112 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%d", devInfo.protocolVer[3]);
113 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%d", devInfo.repoRevision); // 6
114 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%s", devInfo.manufacturer); // 7
115 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%04d", devInfo.buildDate[1]); // 8
116 // ASCII_PORT_WRITE(portNum, a, checkSum, "-%02d", devInfo.buildDate[2]);
117 // ASCII_PORT_WRITE(portNum, a, checkSum, "-%02d", devInfo.buildDate[3]);
118 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%02d", devInfo.buildTime[0]); // 9
119 // ASCII_PORT_WRITE(portNum, a, checkSum, ":%02d", devInfo.buildTime[1]);
120 // ASCII_PORT_WRITE(portNum, a, checkSum, ":%02d", devInfo.buildTime[2]);
121 // ASCII_PORT_WRITE(portNum, a, checkSum, ".%02d", devInfo.buildTime[3]);
122 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%s", devInfo.addInfo); // 10
123 // ASCII_PORT_WRITE_NO_CHECKSUM(portNum, a, "*%.2x\r\n", checkSum);
124 
125  int n = SNPRINTF(a, aSize, "$INFO"
126  ",%d" // 1
127  ",%d.%d.%d.%d" // 2
128  ",%d.%d.%d.%d" // 3
129  ",%d" // 4
130  ",%d.%d.%d.%d" // 5
131  ",%d" // 6
132  ",%s" // 7
133  ",%04d-%02d-%02d" // 8
134  ",%02d:%02d:02%d.02%d" // 9
135  ",%s", // 10
136  (int)info.serialNumber, // 1
137  info.hardwareVer[0], info.hardwareVer[1], info.hardwareVer[2], info.hardwareVer[3], // 2
138  info.firmwareVer[0], info.firmwareVer[1], info.firmwareVer[2], info.firmwareVer[3], // 3
139  (int)info.buildNumber, // 4
140  info.protocolVer[0], info.protocolVer[1], info.protocolVer[2], info.protocolVer[3], // 5
141  (int)info.repoRevision, // 6
142  info.manufacturer, // 7
143  info.buildDate[1], info.buildDate[2], info.buildDate[3], // 8
144  info.buildTime[0], info.buildTime[1], info.buildTime[2], info.buildTime[3], // 9
145  info.addInfo); // 10
146 
147  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
148  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
149  return n;
150 }
151 
152 int tow_to_nmea_ptow(char a[], const int aSize, double imuTow, double insTow, unsigned int gpsWeek)
153 {
154  int n = SNPRINTF(a, aSize, "$PTOW");
155  n += SNPRINTF(a+n, aSize-n, ",%.6lf", imuTow); // 1
156  n += SNPRINTF(a+n, aSize-n, ",%.6lf", insTow); // 2
157  n += SNPRINTF(a+n, aSize-n, ",%u", gpsWeek); // 3
158 
159  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
160  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
161  return n;
162 }
163 
164 int dimu_to_nmea_pimu(char a[], const int aSize, dual_imu_t &dimu)
165 {
166  int n = SNPRINTF(a, aSize, "$PIMU");
167  n += SNPRINTF(a+n, aSize-n, ",%.3lf", dimu.time); // 1
168 
169  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[0].pqr[0]); // 2
170  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[0].pqr[1]); // 3
171  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[0].pqr[2]); // 4
172 
173  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[0].acc[0]); // 5
174  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[0].acc[1]); // 6
175  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[0].acc[2]); // 7
176 
177  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[1].pqr[0]); // 8
178  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[1].pqr[1]); // 9
179  n += SNPRINTF(a+n, aSize-n, ",%.4f", dimu.I[1].pqr[2]); // 10
180 
181  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[1].acc[0]); // 11
182  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[1].acc[1]); // 12
183  n += SNPRINTF(a+n, aSize-n, ",%.3f", dimu.I[1].acc[2]); // 13
184 
185  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
186  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
187  return n;
188 }
189 
190 int pimu_to_nmea_ppimu(char a[], const int aSize, preintegrated_imu_t &pimu)
191 {
192  int n = SNPRINTF(a, aSize, "$PPIMU");
193  n += SNPRINTF(a+n, aSize-n, ",%.3lf", pimu.time); // 1
194 
195  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta1[0]); // 2
196  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta1[1]); // 3
197  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta1[2]); // 4
198 
199  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta2[0]); // 5
200  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta2[1]); // 6
201  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.theta2[2]); // 7
202 
203  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel1[0]); // 8
204  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel1[1]); // 9
205  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel1[2]); // 10
206 
207  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel2[0]); // 11
208  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel2[1]); // 12
209  n += SNPRINTF(a+n, aSize-n, ",%.4f", pimu.vel2[2]); // 13
210 
211  n += SNPRINTF(a+n, aSize-n, ",%.3f", pimu.dt); // 14
212 
213  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
214  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
215  return n;
216 }
217 
218 int ins1_to_nmea_pins1(char a[], const int aSize, ins_1_t &ins1)
219 {
220  int n = SNPRINTF(a, aSize, "$PINS1");
221  n += SNPRINTF(a+n, aSize-n, ",%.3lf", ins1.timeOfWeek); // 1
222 
223  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins1.week); // 2
224  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins1.insStatus); // 3
225  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins1.hdwStatus); // 4
226 
227  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins1.theta[0]); // 5
228  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins1.theta[1]); // 6
229  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins1.theta[2]); // 7
230 
231  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.uvw[0]); // 8
232  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.uvw[1]); // 9
233  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.uvw[2]); // 10
234 
235  n += SNPRINTF(a+n, aSize-n, ",%.8lf", ins1.lla[0]); // 11
236  n += SNPRINTF(a+n, aSize-n, ",%.8lf", ins1.lla[1]); // 12
237  n += SNPRINTF(a+n, aSize-n, ",%.3lf", ins1.lla[2]); // 13
238 
239  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.ned[0]); // 14
240  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.ned[1]); // 15
241  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins1.ned[2]); // 16
242 
243  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
244  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
245  return n;
246 }
247 
248 int ins2_to_nmea_pins2(char a[], const int aSize, ins_2_t &ins2)
249 {
250  int n = SNPRINTF(a, aSize, "$PINS2");
251  n += SNPRINTF(a+n, aSize-n, ",%.3lf", ins2.timeOfWeek); // 1
252 
253  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins2.week); // 2
254  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins2.insStatus); // 3
255  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)ins2.hdwStatus); // 4
256 
257  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins2.qn2b[0]); // 5
258  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins2.qn2b[1]); // 6
259  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins2.qn2b[2]); // 7
260  n += SNPRINTF(a+n, aSize-n, ",%.4f", ins2.qn2b[3]); // 8
261 
262  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins2.uvw[0]); // 9
263  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins2.uvw[1]); // 10
264  n += SNPRINTF(a+n, aSize-n, ",%.3f", ins2.uvw[2]); // 11
265 
266  n += SNPRINTF(a+n, aSize-n, ",%.8lf", ins2.lla[0]); // 12
267  n += SNPRINTF(a+n, aSize-n, ",%.8lf", ins2.lla[1]); // 13
268  n += SNPRINTF(a+n, aSize-n, ",%.3lf", ins2.lla[2]); // 14
269 
270  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
271  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
272  return n;
273 }
274 
275 int strobe_to_nmea_pstrb(char a[], const int aSize, strobe_in_time_t &strobe)
276 {
277  int n = SNPRINTF(a, aSize, "$PSTRB");
278  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)strobe.week); // 1
279  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)strobe.timeOfWeekMs); // 2
280  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)strobe.pin); // 3
281  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)strobe.count); // 4
282 
283  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
284  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
285  return n;
286 }
287 
288 int gps_to_nmea_pgpsp(char a[], const int aSize, gps_pos_t &pos, gps_vel_t &vel)
289 {
290  int n = SNPRINTF(a, aSize, "$PGPSP");
291  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)pos.timeOfWeekMs); // 1
292  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)pos.week); // 2
293  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)pos.status); // 3
294 
295  n += SNPRINTF(a+n, aSize-n, ",%.8lf", pos.lla[0]); // 4
296  n += SNPRINTF(a+n, aSize-n, ",%.8lf", pos.lla[1]); // 5
297  n += SNPRINTF(a+n, aSize-n, ",%.2lf", pos.lla[2]); // 6
298 
299  n += SNPRINTF(a+n, aSize-n, ",%.2f", pos.hMSL); // 7
300  n += SNPRINTF(a+n, aSize-n, ",%.2f", pos.pDop); // 8
301  n += SNPRINTF(a+n, aSize-n, ",%.2f", pos.hAcc); // 9
302  n += SNPRINTF(a+n, aSize-n, ",%.2f", pos.vAcc); // 10
303 
304  n += SNPRINTF(a+n, aSize-n, ",%.2f", vel.vel[0]); // 11
305  n += SNPRINTF(a+n, aSize-n, ",%.2f", vel.vel[1]); // 12
306  n += SNPRINTF(a+n, aSize-n, ",%.2f", vel.vel[2]); // 13
307  n += SNPRINTF(a+n, aSize-n, ",%.2f", vel.sAcc); // 14
308 
309  n += SNPRINTF(a+n, aSize-n, ",%.1f", pos.cnoMean); // 15
310  n += SNPRINTF(a+n, aSize-n, ",%.4lf", pos.towOffset); // 16
311  n += SNPRINTF(a+n, aSize-n, ",%u", (unsigned int)pos.leapS); // 17
312 
313  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
314  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
315  return n;
316 }
317 
318 static int asciiSnprintfLatToDegMin(char* a, size_t aSize, double v)
319 {
320  int degrees = (int)(v);
321  double minutes = (v-((double)degrees))*60.0;
322 
323  return SNPRINTF(a, aSize, ",%02d%07.4lf,%c", abs(degrees), fabs(minutes), (degrees >= 0 ? 'N' : 'S'));
324 }
325 
326 static int asciiSnprintfLonToDegMin(char* a, size_t aSize, double v)
327 {
328  int degrees = (int)(v);
329  double minutes = (v-((double)degrees))*60.0;
330 
331  return SNPRINTF(a, aSize, ",%03d%07.4lf,%c", abs(degrees), fabs(minutes), (degrees >= 0 ? 'E' : 'W'));
332 }
333 
334 static int asciiSnprintfGPSTimeOfLastFix(char* a, size_t aSize, uint32_t timeOfWeekMs)
335 {
336  unsigned int millisecondsToday = timeOfWeekMs % 86400000;
337  unsigned int hours = millisecondsToday / 1000 / 60 / 60;
338  unsigned int minutes = (millisecondsToday / (1000 * 60)) % 60;
339  unsigned int seconds = (millisecondsToday / 1000) % 60;
340 
341  return SNPRINTF(a, aSize, ",%02u%02u%02u", hours, minutes, seconds);
342 }
343 
344 static int asciiSnprintfGPSTimeOfLastFixMilliseconds(char* a, size_t aSize, uint32_t timeOfWeekMs)
345 {
346  unsigned int millisecondsToday = timeOfWeekMs % 86400000;
347  unsigned int hours = millisecondsToday / 1000 / 60 / 60;
348  unsigned int minutes = (millisecondsToday / (1000 * 60)) % 60;
349  unsigned int seconds = (millisecondsToday / 1000) % 60;
350  unsigned int milliseconds = millisecondsToday % 1000;
351 
352  return SNPRINTF(a, aSize, ",%02u%02u%02u.%03u", hours, minutes, seconds, milliseconds);
353 }
354 
355 static int asciiSnprintfGPSDateOfLastFix(char* a, size_t aSize, gps_pos_t &pos)
356 {
357  double julian = gpsToJulian(pos.week, pos.timeOfWeekMs, pos.leapS);
358  int32_t year, month, day, hours, minutes, seconds, milliseconds;
359  julianToDate(julian, &year, &month, &day, &hours, &minutes, &seconds, &milliseconds);
360 
361  return SNPRINTF(a, aSize, ",%02u%02u%02u", (unsigned int)day, (unsigned int)month, (unsigned int)year);
362 }
363 
364 static int asciiSnprintfGPSDateOfLastFixCSV(char* a, size_t aSize, gps_pos_t &pos) //Comma Separated Values
365 {
366  double julian = gpsToJulian(pos.week, pos.timeOfWeekMs, pos.leapS);
367  int32_t year, month, day, hours, minutes, seconds, milliseconds;
368  julianToDate(julian, &year, &month, &day, &hours, &minutes, &seconds, &milliseconds);
369 
370  return SNPRINTF(a, aSize, ",%02u,%02u,%02u", (unsigned int)day, (unsigned int)month, (unsigned int)year);
371 }
372 
373 int gps_to_nmea_gga(char a[], const int aSize, gps_pos_t &pos)
374 {
375  int fixQuality;
376  switch((pos.status&GPS_STATUS_FIX_MASK))
377  {
378  default:
379  case GPS_STATUS_FIX_NONE: fixQuality = 0; break;
380  case GPS_STATUS_FIX_SBAS:
381  case GPS_STATUS_FIX_2D:
382  case GPS_STATUS_FIX_RTK_SINGLE:
383  case GPS_STATUS_FIX_3D: fixQuality = 1; break;
384  case GPS_STATUS_FIX_DGPS: fixQuality = 2; break;
385  case GPS_STATUS_FIX_TIME_ONLY: fixQuality = 3; break;
386  case GPS_STATUS_FIX_RTK_FIX: fixQuality = 4; break;
387  case GPS_STATUS_FIX_RTK_FLOAT: fixQuality = 5; break;
388  case GPS_STATUS_FIX_DEAD_RECKONING_ONLY:
389  case GPS_STATUS_FIX_GPS_PLUS_DEAD_RECK: fixQuality = 6; break;
390  }
391 
392  // NMEA GGA line - http://www.gpsinformation.org/dale/nmea.htm#GGA
393  /*
394  GGA Global Positioning System Fix Data
395  123519 Fix taken at 12:35:19 UTC
396  4807.038,N Latitude 48 deg 07.038' N
397  01131.000,E Longitude 11 deg 31.000' E
398  . Fix quality: 0 = invalid
399  . 1 = GPS fix (SPS)
400  . 2 = DGPS fix
401  . 3 = PPS fix
402  . 4 = Real Time Kinematic
403  . 5 = Float RTK
404  . 6 = estimated (dead reckoning) (2.3 feature)
405  . 7 = Manual input mode
406  . 8 = Simulation mode
407  08 Number of satellites being tracked
408  0.9 Horizontal dilution of position
409  545.4,M MSL altitude in meters
410  46.9,M HAE altitude (above geoid / WGS84 ellipsoid)
411  ellipsoid
412  (empty field) time in seconds since last DGPS update
413  (empty field) DGPS station ID number
414  *47 the checksum data, always begins with *
415  */
416 
417 // unsigned int checkSum = 0;
418 // serWrite( portNum, (unsigned char*)"$", 1);
419 // ASCII_PORT_WRITE_NO_FORMAT(portNum, "GPGGA", checkSum, 5);
420 // asciiPortWriteGPSTimeOfLastFix(portNum, a, checkSum); // 1
421 // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[0], ",%02d", 'N', 'S'); // 2,3
422 // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[1], ",%03d", 'E', 'W'); // 4,5
423 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%u", (unsigned)(fixQuality)); // 6
424 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%02u", (unsigned)(pos.status&GPS_STATUS_NUM_SATS_USED_MASK)); // 7
425 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.2f", pos.pDop); // 8
426 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.2f,M", pos.hMSL); // 9,10
427 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.2f,M", pos.hMSL - pos.lla[2]); // 11,12
428 // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",,", checkSum, 2); // 13,14
429 // ASCII_PORT_WRITE_NO_CHECKSUM(portNum, a, "*%.2x\r\n", checkSum);
430 
431  int n = SNPRINTF(a, aSize, "$GPGGA");
432  n += asciiSnprintfGPSTimeOfLastFix(a+n, aSize-n, pos.timeOfWeekMs); // 1
433  n += asciiSnprintfLatToDegMin(a+n, aSize-n, pos.lla[0]); // 2,3
434  n += asciiSnprintfLonToDegMin(a+n, aSize-n, pos.lla[1]); // 4,5
435  n += SNPRINTF(a+n, aSize-n,
436  ",%u" // 6
437  ",%02u" // 7
438  ",%.2f" // 8
439  ",%.2f,M" // 9,10
440  ",%.2f,M" // 11,12
441  ",,", // 13,14
442  (unsigned int)fixQuality, // 6
443  (unsigned int)(pos.status&GPS_STATUS_NUM_SATS_USED_MASK), // 7
444  pos.pDop, // 8
445  pos.hMSL, // 9,10
446  pos.hMSL - pos.lla[2]); // 11,12 13,14
447 
448  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
449  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
450  return n;
451 }
452 
453 int gps_to_nmea_gll(char a[], const int aSize, gps_pos_t &pos)
454 {
455  // NMEA GLL line - http://www.gpsinformation.org/dale/nmea.htm#GLL
456  /*
457  GLL Geographic position, Latitude and Longitude
458  4916.46,N Latitude 49 deg. 16.45 min. North
459  12311.12,W Longitude 123 deg. 11.12 min. West
460  225444 Fix taken at 22:54:44 UTC
461  A Data Active or V (void)
462  *iD checksum data
463  */
464 
465 // unsigned int checkSum = 0;
466 // serWrite( portNum, (unsigned char*)"$", 1);
467 // ASCII_PORT_WRITE_NO_FORMAT(portNum, "GPGLL", checkSum, 5);
468 // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[0], ",%02d", 'N', 'S'); // 1,2
469 // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[1], ",%03d", 'E', 'W'); // 3,4
470 // asciiPortWriteGPSTimeOfLastFix(portNum, a, checkSum); // 5
471 // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",A", checkSum, 2); // 6
472 // ASCII_PORT_WRITE_NO_CHECKSUM(portNum, a, "*%.2x\r\n", checkSum);
473 
474  int n = SNPRINTF(a, aSize, "$GPGLL");
475  n += asciiSnprintfLatToDegMin(a+n, aSize-n, pos.lla[0]); // 1,2
476  n += asciiSnprintfLonToDegMin(a+n, aSize-n, pos.lla[1]); // 3,4
477  n += asciiSnprintfGPSTimeOfLastFix(a+n, aSize-n, pos.timeOfWeekMs); // 5
478  n += SNPRINTF(a+n, aSize-n, ",A"); // 6
479 
480  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
481  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
482  return n;
483 }
484 
485 int gps_to_nmea_gsa(char a[], const int aSize, gps_pos_t &pos, gps_sat_t &sat)
486 {
487  int fixQuality;
488  switch((pos.status&GPS_STATUS_FIX_MASK))
489  {
490  default:
491  fixQuality = 0; break;
492  case GPS_STATUS_FIX_2D:
493  fixQuality = 2; break;
494 
495  case GPS_STATUS_FIX_3D:
496  case GPS_STATUS_FIX_SBAS:
497  case GPS_STATUS_FIX_DGPS:
498  case GPS_STATUS_FIX_RTK_FIX:
499  case GPS_STATUS_FIX_RTK_SINGLE:
500  case GPS_STATUS_FIX_RTK_FLOAT:
501  fixQuality = 3; break;
502  }
503 
504  // NMEA GSA line - http://www.gpsinformation.org/dale/nmea.htm#GSA
505  /*
506  eg1. $GPGSA,A,3,,,,,,16,18,,22,24,,,3.6,2.1,2.2*3C
507  eg2. $GPGSA,A,3,19,28,14,18,27,22,31,39,,,,,1.7,1.0,1.3*35
508 
509  1 = Mode:
510  . M=Manual, forced to operate in 2D or 3D
511  . A=Automatic, 3D/2D
512  2 = Mode:
513  . 1=Fix not available
514  . 2=2D
515  . 3=3D
516  3-14 = IDs of SVs used in position fix (null for unused fields)
517  15 = PDOP
518  16 = HDOP
519  17 = VDOP
520  */
521 
522 // unsigned int checkSum = 0;
523 // serWrite( portNum, (unsigned char*)"$", 1);
524 // ASCII_PORT_WRITE_NO_FORMAT(portNum, "GPGSA", checkSum, 5);
525 // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",A", checkSum, 2); // 1
526 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%02u", (unsigned)(fixQuality)); // 2
527 // for (uint32_t i = 0; i < 12; i++) // 3-14
528 // {
529 // if(g_gps1Sat.sat[i].svId)
530 // {
531 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%02u", (unsigned)(g_gps1Sat.sat[i].svId));
532 // }
533 // else
534 // {
535 // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",", checkSum, 1);
536 // }
537 // }
538 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.1f", pos.pDop); // 15
539 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.1f", pos.hAcc); // 16
540 // ASCII_PORT_WRITE(portNum, a, checkSum, ",%.1f", pos.vAcc); // 17
541 // ASCII_PORT_WRITE_NO_CHECKSUM(portNum, a, "*%.2x\r\n", checkSum);
542 
543  int n = SNPRINTF(a, aSize, "$GPGSA"
544  ",A" // 1
545  ",%02u", // 2
546  (unsigned int)fixQuality); // 1,2
547 
548  for (uint32_t i = 0; i < 12; i++) // 3-14
549  {
550  if(sat.sat[i].svId)
551  {
552  n += SNPRINTF(a+n, aSize-n, ",%02u", (unsigned)(sat.sat[i].svId));
553  }
554  else
555  {
556  n += SNPRINTF(a+n, aSize-n, ",");
557  }
558  }
559 
560  n += SNPRINTF(a+n, aSize-n,
561  ",%.1f" // 15
562  ",%.1f" // 16
563  ",%.1f", // 17
564  pos.pDop, // 15
565  pos.hAcc, // 16
566  pos.vAcc); // 17
567 
568  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
569  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
570  return n;
571 }
572 
573 int gps_to_nmea_rmc(char a[], const int aSize, gps_pos_t &pos, gps_vel_t &vel, float magDeclination)
574 {
575  Quat_t qe2n;
576  Vector3_t vel_ned_;
577  quat_ecef2ned((float)pos.lla[0], (float)pos.lla[1], qe2n);
578  quatConjRot(vel_ned_, qe2n, vel.vel);
579 
580  // unsigned int checkSum = 0;
581  // serWrite(portNum, (unsigned char*)"$", 1);
582  // ASCII_PORT_WRITE_NO_FORMAT(portNum, "GPRMC", checkSum, 5);
583  // asciiPortWriteGPSTimeOfLastFix(portNum, a, checkSum); // 1 // time of last fix
584  // if((pos.status&GPS_STATUS_FIX_MASK)!=GPS_STATUS_FIX_NONE)
585  // {
586  // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",A", checkSum, 2); // 2 // A=active (good)
587  // }
588  // else
589  // {
590  // ASCII_PORT_WRITE_NO_FORMAT(portNum, ",V", checkSum, 2); // 2 // V=void (bad,warning)
591  // }
592  // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[0], ",%02d", 'N', 'S'); // 3,4 // lat lon (degrees minutes)
593  // asciiPortWriteCoordDegMin(portNum, a, checkSum, pos.lla[1], ",%03d", 'E', 'W'); // 5,6
594  //
595  // float speedInKnots = C_METERS_KNOTS_F * mag_Vec2(vel_ned_);
596  // ASCII_PORT_WRITE(portNum, a, checkSum, ",%05.1f", speedInKnots); // 7 // speed in knots
597  // // float courseMadeTrue = atan2f(g_navInGpsA.velNed[1], g_navInGpsA.velNed[0]);
598  // float courseMadeTrue = 0.0f;
599  // ASCII_PORT_WRITE(portNum, a, checkSum, ",%05.1f", (courseMadeTrue*C_RAD2DEG_F)); // 8 // course made true
600  // asciiPortWriteGPSDateOfLastFix(portNum, a, checkSum); // 9 // date of last fix UTC
601  //
602  // // Magnetic variation degrees (Easterly var. subtracts from true course), i.e. 020.3,E - left pad to 3 zero
603  // float magDec = g_nvmFlashCfg->magDeclination * C_RAD2DEG_F;
604  // bool positive = (magDec >= 0.0);
605  // ASCII_PORT_WRITE(portNum, a, checkSum, ",%05.1f,", abs(magDec)); // 10 // Magnetic variation
606  // ASCII_PORT_WRITE_NO_FORMAT(portNum, (positive ? "E" : "W"), checkSum, 1); // 11
607  // ASCII_PORT_WRITE_NO_CHECKSUM(portNum, a, "*%.2x\r\n", checkSum);
608 
609  int n = SNPRINTF(a, aSize, "$GPRMC");
610  n += asciiSnprintfGPSTimeOfLastFix(a+n, aSize-n, pos.timeOfWeekMs); // 1 // time of last fix
611  if((pos.status&GPS_STATUS_FIX_MASK)!=GPS_STATUS_FIX_NONE)
612  {
613  n += SNPRINTF(a+n, aSize-n, ",A"); // 2 // A=active (good)
614  }
615  else
616  {
617  n += SNPRINTF(a+n, aSize-n, ",V"); // 2 // V=void (bad,warning)
618  }
619  n += asciiSnprintfLatToDegMin(a+n, aSize-n, pos.lla[0]); // 3,4 // lat (degrees minutes)
620  n += asciiSnprintfLonToDegMin(a+n, aSize-n, pos.lla[1]); // 5,6 // lon (degrees minutes)
621 
622  float speedInKnots = C_METERS_KNOTS_F * mag_Vec2(vel_ned_);
623  // float courseMadeTrue = atan2f(g_navInGpsA.velNed[1], g_navInGpsA.velNed[0]);
624  float courseMadeTrue = 0.0f;
625  n += SNPRINTF(a+n, aSize-n,
626  ",%05.1f" // 7
627  ",%05.1f", // 8
628  speedInKnots, // 7 // speed in knots
629  courseMadeTrue*C_RAD2DEG_F); // 8 // course made true
630 
631  n += asciiSnprintfGPSDateOfLastFix(a+n, aSize-n, pos); // 9 // date of last fix UTC
632 
633  // Magnetic variation degrees (Easterly var. subtracts from true course), i.e. 020.3,E - left pad to 3 zero
634  float magDec = magDeclination * C_RAD2DEG_F;
635  bool positive = (magDec >= 0.0);
636 
637  n += SNPRINTF(a+n, aSize-n,
638  ",%05.1f" // 10
639  ",%s", // 11
640  fabsf(magDec), // 10 // Magnetic variation
641  (positive ? "E" : "W")); // 11
642 
643  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
644  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
645  return n;
646 }
647 
648 int gps_to_nmea_zda(char a[], const int aSize, gps_pos_t &pos)
649 {
650  // NMEA ZDA line - http://www.gpsinformation.org/dale/nmea.htm#ZDA
651  /*
652  hhmmss HrMinSec(UTC)
653  dd,mm,yyy Day,Month,Year
654  xx local zone hours -13..13 - Fixed field: 00
655  yy local zone minutes 0..59 - Fixed field: 00
656  *CC checksum
657  */
658 
659  int n = SNPRINTF(a, aSize, "$GPZDA"); //Field 1
660  n += asciiSnprintfGPSTimeOfLastFix(a+n, aSize-n, pos.timeOfWeekMs + pos.leapS*1000); //Field 2
661  n += asciiSnprintfGPSDateOfLastFixCSV(a+n, aSize-n, pos); // 2,3,4
662  n += SNPRINTF(a+n, aSize-n, ",00,00"); // 5,6
663 
664  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
665  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
666  return n;
667 }
668 
669 int gps_to_nmea_pashr(char a[], const int aSize, gps_pos_t &pos, ins_1_t &ins1, float heave, inl2_ned_sigma_t &sigma)
670 {
671  // NMEA PASHR - RT300 proprietary roll and pitch sentence
672  /*
673  hhmmss.sss - UTC time
674  hhh.hh - Heading in degrees
675  T - flag to indicate that the Heading is True Heading (i.e. to True North)
676  rrr.rr - Roll Angle in degrees
677  ppp.pp - Pitch Angle in degrees
678  xxx.xx - Heave
679  a.aaa - Roll Angle Accuracy Estimate (Stdev) in degrees
680  b.bbb - Pitch Angle Accuracy Estimate (Stdev) in degrees
681  c.ccc - Heading Angle Accuracy Estimate (Stdev) in degrees
682  d - Aiding Status
683  e - IMU Status
684  hh - Checksum
685  */
686 
687  int n = SNPRINTF(a, aSize, "$PASHR"); //Field 1 - Name
688  n += asciiSnprintfGPSTimeOfLastFixMilliseconds(a+n, aSize-n, pos.timeOfWeekMs + pos.leapS*1000); //Field 2 - UTC Time
689 
690  n += SNPRINTF(a+n, aSize-n, ",%.2f", RAD2DEG(ins1.theta[2])); //Field 3 - Heading value in decimal degrees.
691  n += SNPRINTF(a+n, aSize-n, ",T"); //Field 4 - T (heading respect to True North)
692  n += SNPRINTF(a+n, aSize-n, ",%+.2f", RAD2DEG(ins1.theta[0])); //Field 5 - Roll in degrees
693  n += SNPRINTF(a+n, aSize-n, ",%+.2f", RAD2DEG(ins1.theta[1])); //Field 6 - Pitch in degrees
694  n += SNPRINTF(a+n, aSize-n, ",%+.2f", heave); //Field 7 - Heave
695 
696  n += SNPRINTF(a+n, aSize-n, ",%.3f", RAD2DEG(sigma.PattNED[0])); //roll accuracy //8
697  n += SNPRINTF(a+n, aSize-n, ",%.3f", RAD2DEG(sigma.PattNED[1])); //pitch accuracy //9
698  n += SNPRINTF(a+n, aSize-n, ",%.3f", RAD2DEG(sigma.PattNED[2])); //heading accuracy //10
699 
700  int fix = 0;
701  if(INS_STATUS_NAV_FIX_STATUS(ins1.insStatus) >= GPS_NAV_FIX_POSITIONING_RTK_FLOAT)
702  {
703  fix = 2;
704  }
705  else if(INS_STATUS_NAV_FIX_STATUS(ins1.insStatus) >= GPS_NAV_FIX_POSITIONING_3D)
706  {
707  fix = 1;
708  }
709  n += SNPRINTF(a+n, aSize-n, ",%d", fix); //Field 11 - GPS Quality
710  n += SNPRINTF(a+n, aSize-n, ",%d", INS_STATUS_SOLUTION(ins1.insStatus) >= INS_STATUS_SOLUTION_NAV); //Field 12 - INS Status
711 
712  unsigned int checkSum = ASCII_compute_checksum((uint8_t*)(a+1), n);
713  n += SNPRINTF(a+n, aSize-n, "*%.2x\r\n", checkSum);
714  return n;
715 }
716 
717 
719 // Parse NMEA Functions
721 
722 uint32_t parse_nmea_ascb(int pHandle, const char msg[], int msgSize, ascii_msgs_t asciiPeriod[NUM_COM_PORTS])
723 {
724  if(pHandle >= NUM_COM_PORTS)
725  {
726  return -1;
727  }
728  char *ptr = (char *)msg;
729 
730  // Default to current settings
731  ascii_msgs_t tmp = asciiPeriod[pHandle];
732  uint32_t options = 0;
733 
734  ptr = ASCII_find_next_field(ptr); // Options
735  if(*ptr!=','){ options = atoi(ptr); }
736  ptr = ASCII_find_next_field(ptr); // PIMU
737  if(*ptr!=','){ tmp.pimu = atoi(ptr); }
738  ptr = ASCII_find_next_field(ptr); // PPIMU
739  if(*ptr!=','){ tmp.ppimu = atoi(ptr); }
740  ptr = ASCII_find_next_field(ptr); // PINS1
741  if(*ptr!=','){ tmp.pins1 = atoi(ptr); }
742  ptr = ASCII_find_next_field(ptr); // PINS2
743  if(*ptr!=','){ tmp.pins2 = atoi(ptr); }
744  ptr = ASCII_find_next_field(ptr); // PGPSP
745  if(*ptr!=','){ tmp.pgpsp = atoi(ptr); }
746  ptr = ASCII_find_next_field(ptr); // reserved
747 
748  ptr = ASCII_find_next_field(ptr); // gpgga
749  if(*ptr!=','){ tmp.gpgga = atoi(ptr); }
750  ptr = ASCII_find_next_field(ptr); // gpgll
751  if(*ptr!=','){ tmp.gpgll = atoi(ptr); }
752  ptr = ASCII_find_next_field(ptr); // gpgsa
753  if(*ptr!=','){ tmp.gpgsa = atoi(ptr); }
754  ptr = ASCII_find_next_field(ptr); // gprmc
755  if(*ptr!=','){ tmp.gprmc = atoi(ptr); }
756  ptr = ASCII_find_next_field(ptr); // gpzda
757  if(*ptr!=','){ tmp.gpzda = atoi(ptr); }
758  ptr = ASCII_find_next_field(ptr); // pashr
759  if(*ptr!=','){ tmp.pashr = atoi(ptr); }
760 
761  // Copy tmp to corresponding port(s)
762  switch(options&RMC_OPTIONS_PORT_MASK)
763  {
764  case 0xFF: // All ports
765  for(int i=0; i<NUM_COM_PORTS; i++)
766  {
767  asciiPeriod[i] = tmp;
768  }
769  break;
770 
771  default: // Current port
772  case RMC_OPTIONS_PORT_CURRENT: asciiPeriod[pHandle] = tmp; break;
773  case RMC_OPTIONS_PORT_SER0: asciiPeriod[0] = tmp; break;
774  case RMC_OPTIONS_PORT_SER1: asciiPeriod[1] = tmp; break;
775  case RMC_OPTIONS_PORT_USB: asciiPeriod[2] = tmp; break;
776  }
777 
778  return options;
779 }
780 
781 
782 /* G_ZDA message
783 * Provides day/month/year fort calculating iTOW.
784 */
785 int parse_nmea_zda(const char msg[], int msgSize, double &day, double &month, double &year)
786 {
787  char *ptr = (char *)&msg[7];
788  //$xxZDA,time,day,month,year,ltzh,ltzn*cs<CR><LF>
789 
790  //time
791  ptr = ASCII_find_next_field(ptr);
792 
793  //day
794  day = atoi(ptr);
795  ptr = ASCII_find_next_field(ptr);
796 
797  //month
798  month = atoi(ptr);
799  ptr = ASCII_find_next_field(ptr);
800 
801  //year
802  year = atoi(ptr);
803 
804  return 0;
805 }
806 
807 /* G_GNS Message
808 * Provides position data (newer message) using the following:
809 * Time
810 * Position (lat, lon)
811 * Positioning mode (fix type)
812 * Number Satellites
813 * Altitude & Geoid separation
814 */
815 int parse_nmea_gns(const char msg[], int msgSize, gps_pos_t *gpsPos, double datetime[6], int *satsUsed, int navMode)
816 {
817  char *ptr = (char *)&msg[7];
818  //$xxGNS,time,lat,NS,lon,EW,posMode,numSV,HDOP,alt,sep,diffAge,diffStation,navStatus*cs<CR><LF>
819 
820  //UTC time, hhmmss
821  double UTCtime = atof(ptr);
822  ptr = ASCII_find_next_field(ptr);
823 
824  //Convert time to iTOW
825  datetime[3] = ((int)UTCtime / 10000) % 100;
826  datetime[4] = ((int)UTCtime / 100) % 100;
827  double subSec = UTCtime - (int)UTCtime;
828  datetime[5] = (double)((int)UTCtime % 100) + subSec + gpsPos->leapS;
829 
830  gtime_t gtm = epoch2time(datetime);
831  int week;
832  double iTOWd = time2gpst(gtm, &week);
833  uint32_t iTOW = (uint32_t)((iTOWd + 0.00001) * 1000.0);
834 
835  //Latitude
836  Vector3d lla;
837  lla[0] = ddmm2deg(atof(ptr));
838  ptr = ASCII_find_next_field(ptr);
839  if(*ptr == 'S')
840  lla[0] = -lla[0];
841  ptr = ASCII_find_next_field(ptr);
842 
843  //Longitude
844  lla[1] = ddmm2deg(atof(ptr));
845  ptr = ASCII_find_next_field(ptr);
846  if(*ptr == 'W')
847  lla[1] = -lla[1];
848  ptr = ASCII_find_next_field(ptr);
849 
850  //Positioning Mode
851  char pMode[4] = {0,0,0,0};
852  if(*ptr != ',')
853  pMode[0] = *ptr++;
854  if(*ptr != ',')
855  pMode[1] = *ptr++;
856  if(*ptr != ',')
857  pMode[2] = *ptr++;
858  if(*ptr != ',')
859  pMode[3] = *ptr++;
860  ptr = ASCII_find_next_field(ptr);
861 
862  //Based off of ZED-F9P datasheet
863  int fixType = 0;
864  int differential = 0;
865  if(pMode[0] == 'R' || pMode[1] == 'R' || pMode[2] == 'R' || pMode[3] == 'R') //RTK fixed
866  fixType = 2;
867  else if(pMode[0] == 'F' || pMode[1] == 'F' || pMode[2] == 'F' || pMode[3] == 'F') //RTK float
868  fixType = 2;
869  else if(pMode[0] == 'D' || pMode[1] == 'D' || pMode[2] == 'D' || pMode[3] == 'D') //2D/3D GNSS fix
870  {
871  fixType = 2;
872  differential = 1;
873  }
874  else if(pMode[0] == 'A' || pMode[1] == 'A' || pMode[2] == 'A' || pMode[3] == 'A') //2D/3D GNSS fix
875  fixType = 2;
876  else if(pMode[0] == 'E' || pMode[1] == 'E' || pMode[2] == 'E' || pMode[3] == 'E') //Dead rekoning fix
877  fixType = 1;
878 
879  //Determine 2D / 3D
880  if(fixType == 2 && navMode == 3)
881  fixType = 3;
882 
883  //Number of satellites used in solution
884  *satsUsed = atoi(ptr);
885  ptr = ASCII_find_next_field(ptr);
886 
887  //HDOP
888  ptr = ASCII_find_next_field(ptr);
889 
890  //MSL Altitude (altitude above mean sea level)
891  lla[2] = atof(ptr);
892  gpsPos->hMSL = (float)lla[2];
893  ptr = ASCII_find_next_field(ptr);
894 
895  //Geoid separation (difference between ellipsoid and mean sea level)
896  double sep = atof(ptr);
897 
898  //Store data
899  set_gpsPos_status_mask(&(gpsPos->status), *satsUsed, GPS_STATUS_NUM_SATS_USED_MASK);
900  set_gpsPos_status_mask(&(gpsPos->status), (fixType != 0) << GPS_STATUS_FLAGS_BIT_OFFSET, GPS_STATUS_FLAGS_FIX_OK);
901  set_gpsPos_status_mask(&(gpsPos->status), fixType << GPS_STATUS_FIX_BIT_OFFSET, GPS_STATUS_FIX_MASK);
902  set_gpsPos_status_mask(&(gpsPos->status), differential << (GPS_STATUS_FLAGS_BIT_OFFSET + 1), GPS_STATUS_FLAGS_DGPS_USED);
903 
904  gpsPos->lla[0] = lla[0];
905  gpsPos->lla[1] = lla[1];
906  gpsPos->lla[2] = lla[2] + sep;
907 
908  //Change LLA to radians
909  lla[0] = DEG2RAD(lla[0]);
910  lla[1] = DEG2RAD(lla[1]);
911  lla[2] = gpsPos->lla[2]; // Use ellipsoid alt
912 
913  //Convert LLA to ECEF. Ensure LLA uses ellipsoid alt
914  Vector3d ecef;
915  lla2ecef(lla, ecef);
916 
917  gpsPos->timeOfWeekMs = iTOW;
918  gpsPos->week = week;
919  gpsPos->ecef[0] = ecef[0];
920  gpsPos->ecef[1] = ecef[1];
921  gpsPos->ecef[2] = ecef[2];
922  //gpsPos->hAcc = 0;
923  //gpsPos->vAcc = 0;
924 
925  //Indicate it is coming from NMEA
926  gpsPos->status |= GPS_STATUS_FLAGS_GPS_NMEA_DATA;
927 
928  return 0;
929 }
930 
931 /* G_GGA Message
932 * Provides position data (older message) using the following:
933 * Time
934 * Position (lat, lon)
935 * Quality (fix type)
936 * Number Satellites
937 * Altitude & Geoid separation
938 */
939 int parse_nmea_gga(const char msg[], int msgSize, gps_pos_t *gpsPos, double datetime[6], int *satsUsed, int navMode)
940 {
941  char *ptr = (char *)&msg[7];
942  //$xxGGA,time,lat,NS,lon,EW,quality,numSV,HDOP,alt,altUnit,sep,sepUnit,diffAge,diffStation*cs<CR><LF>
943 
944  //UTC time, hhmmss
945  double UTCtime = atof(ptr);
946  ptr = ASCII_find_next_field(ptr);
947 
948  //Convert time to iTOW
949  datetime[3] = ((int)UTCtime / 10000) % 100;
950  datetime[4] = ((int)UTCtime / 100) % 100;
951  double subSec = UTCtime - (int)UTCtime;
952  datetime[5] = (double)((int)UTCtime % 100) + subSec + gpsPos->leapS;
953 
954  gtime_t gtm = epoch2time(datetime);
955  int week;
956  double iTOWd = time2gpst(gtm, &week);
957  uint32_t iTOW = (uint32_t)((iTOWd + 0.00001) * 1000.0);
958 
959  //Latitude
960  Vector3d lla;
961  lla[0] = ddmm2deg(atof(ptr));
962  ptr = ASCII_find_next_field(ptr);
963  if(*ptr == 'S')
964  lla[0] = -lla[0];
965  ptr = ASCII_find_next_field(ptr);
966 
967  //Longitude
968  lla[1] = ddmm2deg(atof(ptr));
969  ptr = ASCII_find_next_field(ptr);
970  if(*ptr == 'W')
971  lla[1] = -lla[1];
972  ptr = ASCII_find_next_field(ptr);
973 
974  //quality
975  int quality = atoi(ptr);
976  ptr = ASCII_find_next_field(ptr);
977 
978  //Based off of ZED-F9P datasheet
979  int fixType = 0;
980  int differential = 0;
981  switch(quality)
982  {
983  case 1:
984  case 4:
985  case 5:
986  fixType = 2;
987  break;
988  case 2:
989  fixType = 2;
990  differential = 1;
991  break;
992  case 6:
993  fixType = 1;
994  break;
995  }
996 
997  //Determine 2D / 3D
998  if(fixType == 2 && navMode == 3)
999  fixType = 3;
1000 
1001  //Number of satellites used in solution
1002  *satsUsed = atoi(ptr);
1003  ptr = ASCII_find_next_field(ptr);
1004 
1005  //HDOP
1006  ptr = ASCII_find_next_field(ptr);
1007 
1008  //MSL Altitude (altitude above mean sea level)
1009  lla[2] = atof(ptr);
1010  gpsPos->hMSL = (float)lla[2];
1011  ptr = ASCII_find_next_field(ptr);
1012 
1013  //altUnit
1014  ptr = ASCII_find_next_field(ptr);
1015 
1016  //Geoid separation
1017  double sep = atof(ptr);
1018 
1019  //Store data
1020  set_gpsPos_status_mask(&(gpsPos->status), *satsUsed, GPS_STATUS_NUM_SATS_USED_MASK);
1021  set_gpsPos_status_mask(&(gpsPos->status), (fixType != 0) << GPS_STATUS_FLAGS_BIT_OFFSET, GPS_STATUS_FLAGS_FIX_OK);
1022  set_gpsPos_status_mask(&(gpsPos->status), fixType << GPS_STATUS_FIX_BIT_OFFSET, GPS_STATUS_FIX_MASK);
1023  set_gpsPos_status_mask(&(gpsPos->status), differential << (GPS_STATUS_FLAGS_BIT_OFFSET + 1), GPS_STATUS_FLAGS_DGPS_USED);
1024 
1025  gpsPos->lla[0] = lla[0];
1026  gpsPos->lla[1] = lla[1];
1027  gpsPos->lla[2] = lla[2] + sep;
1028 
1029  //Change LLA to radians
1030  lla[0] = DEG2RAD(lla[0]);
1031  lla[1] = DEG2RAD(lla[1]);
1032  lla[2] = gpsPos->lla[2]; // Use ellipsoid alt
1033 
1034  //Convert LLA to ECEF. Ensure LLA uses ellipsoid alt
1035  Vector3d ecef;
1036  lla2ecef(lla, ecef);
1037 
1038  gpsPos->timeOfWeekMs = iTOW;
1039  gpsPos->week = week;
1040  gpsPos->ecef[0] = ecef[0];
1041  gpsPos->ecef[1] = ecef[1];
1042  gpsPos->ecef[2] = ecef[2];
1043  //gpsPos->hAcc = 0;
1044  //gpsPos->vAcc = 0;
1045 
1046  //Indicate it is coming from NMEA
1047  gpsPos->status |= GPS_STATUS_FLAGS_GPS_NMEA_DATA;
1048 
1049  return 0;
1050 }
1051 
1052 /* G_RMC Message
1053 * Provides speed (speed and course over ground)
1054 */
1055 int parse_nmea_rmc(const char msg[], int msgSize, gps_vel_t *gpsVel, double datetime[6], int *satsUsed, int navMode)
1056 {
1057  char *ptr = (char *)&msg[7];
1058  //$xxRMC,time,status,lat,NS,lon,EW,spd,cog,date,mv,mvEW,posMode,navStatus*cs<CR><LF>
1059 
1060  //UTC time, hhmmss
1061  double UTCtime = atof(ptr);
1062  ptr = ASCII_find_next_field(ptr);
1063 
1064  //Skip 5
1065  for(int i=0;i<5;++i)
1066  {
1067  ptr = ASCII_find_next_field(ptr);
1068  }
1069 
1070  //spd & cog
1071  float spdm_s = (float)atof(ptr) * C_KNOTS_METERS_F;
1072  ptr = ASCII_find_next_field(ptr);
1073  float cogRad = DEG2RAD((float)atof(ptr));
1074 
1075  //Convert time to iTOW
1076  datetime[3] = ((int)UTCtime / 10000) % 100;
1077  datetime[4] = ((int)UTCtime / 100) % 100;
1078  double subSec = UTCtime - (int)UTCtime;
1079  datetime[5] = (double)((int)UTCtime % 100) + subSec;
1080 
1081  gtime_t gtm = epoch2time(datetime);
1082  double iTOWd = time2gpst(gtm, 0);
1083  gpsVel->timeOfWeekMs = (uint32_t)((iTOWd + 0.00001) * 1000.0);
1084 
1085  //Speed data in NED
1086  gpsVel->vel[0] = spdm_s * cosf(cogRad);
1087  gpsVel->vel[1] = spdm_s * sinf(cogRad);
1088  gpsVel->vel[2] = 0;
1089  //dependencies_.gpsVel->sAcc = 0;
1090 
1091  //Indicate it is coming from NMEA
1092  gpsVel->status |= GPS_STATUS_FLAGS_GPS_NMEA_DATA;
1093 
1094  return 0;
1095 }
1096 
1097 /* G_GSA Message
1098 * Provides pDOP and navigation mode (saved to determine 2D/3D mode)
1099 */
1100 int parse_nmea_gsa(const char msg[], int msgSize, gps_pos_t *gpsPos, int *navMode)
1101 {
1102  char *ptr = (char *)&msg[7];
1103  //$xxGSA,opMode,navMode{,svid},PDOP,HDOP,VDOP,systemId*cs<CR><LF>
1104 
1105  //Operation mode
1106  ptr = ASCII_find_next_field(ptr);
1107 
1108  //Navigation mode - save for use to determine 2D / 3D mode
1109  *navMode = atoi(ptr);
1110 
1111  //Skip 13
1112  for(int i=0;i<13;++i)
1113  {
1114  ptr = ASCII_find_next_field(ptr);
1115  }
1116 
1117  //pDOP
1118  gpsPos->pDop = (float)atof(ptr);
1119 
1120  return 0;
1121 }
1122 
1123 /* G_GSV Message
1124 * Provides satellite information
1125 * Multiple GSV messages will come in a block. We wait until block is finished before flagging data is ready.
1126 */
1127 int parse_nmea_gsv(const char msg[], int msgSize, gps_sat_t* gpsSat, int lastGSVmsg[2], int *satCount, uint32_t *cnoSum, uint32_t *cnoCount)
1128 {
1129  char *ptr = (char *)&msg[7];
1130  //$xxGSV,numMsg,msgNum,numSV{,svid,elv,az,cno},signalId*cs<CR><LF>
1131 
1132  //numMsg
1133  //int numMsg = atoi(ptr);
1134  ptr = ASCII_find_next_field(ptr);
1135 
1136  //msgNum
1137  int msgNum = atoi(ptr);
1138  ptr = ASCII_find_next_field(ptr);
1139 
1140  //numSV
1141  int numSV = atoi(ptr);
1142  ptr = ASCII_find_next_field(ptr);
1143 
1144  //For some reason the ZED-F9P outputs double messages with the second set having a zero signal strength for satellites for protocol version 27.10 & 27.11
1145  // (Data sheet for ZED-F9P indicates this message is only supported in version 27.11)
1146  //Only process the first message
1147  if(lastGSVmsg[0] != msg[2] || lastGSVmsg[1] < msgNum)
1148  {
1149  //save message
1150  lastGSVmsg[0] = msg[2];
1151  lastGSVmsg[1] = msgNum;
1152 
1153  //Process up to 4 satellites
1154  int countSat = numSV - (msgNum - 1) * 4;
1155  if(countSat > 4)
1156  countSat = 4;
1157 
1158  for(int i=0;i<countSat;++i)
1159  {
1160  //svid
1161  int svid = atoi(ptr);
1162  ptr = ASCII_find_next_field(ptr);
1163 
1164  //elv
1165  int elv = atoi(ptr);
1166  ptr = ASCII_find_next_field(ptr);
1167 
1168  //az
1169  int az = atoi(ptr);
1170  ptr = ASCII_find_next_field(ptr);
1171 
1172  //cno
1173  int cno = atoi(ptr);
1174  ptr = ASCII_find_next_field(ptr);
1175 
1176  //Save data (only if there is room available)
1177  if(*satCount < MAX_NUM_SAT_CHANNELS && gpsSat)
1178  {
1179  auto& svDest = gpsSat->sat[(*satCount)++];
1180  //IDs are different based on the GNSS type, convert to be the same as UBX message
1181  switch(msg[2])
1182  {
1183  default:
1184  case 'P': //GPS, SBAS, QZSS
1185  if(svid >= 193) //QZSS
1186  {
1187  svDest.gnssId = 5;
1188  if(svid >= 193)
1189  svDest.svId = svid-192;
1190  else
1191  svDest.svId = svid;
1192  }
1193  else if(svid > 32) //SBAS
1194  {
1195  svDest.gnssId = 1;
1196  if(svid <= 64)
1197  svDest.svId = svid + 87;
1198  else
1199  svDest.svId = svid;
1200  }
1201  else //GPS
1202  {
1203  svDest.gnssId = 0;
1204  svDest.svId = svid;
1205  }
1206  break;
1207  case 'L': //GLONASS
1208  svDest.gnssId = 6;
1209  if(svid >= 65)
1210  svDest.svId = svid - 64;
1211  else
1212  svDest.svId = svid;
1213  break;
1214  case 'A': //Galileo
1215  svDest.gnssId = 2;
1216  svDest.svId = svid;
1217  break;
1218  case 'B': //BeiDou
1219  svDest.gnssId = 3;
1220  svDest.svId = svid;
1221  break;
1222  }
1223  svDest.cno = cno;
1224  svDest.elev = elv;
1225  svDest.azim = az;
1226  svDest.prRes = 0;
1227  svDest.flags = 0;
1228  }
1229 
1230  // Calculate the sum and count of non-zero cno values, in order to calculate the cnoMean
1231  if (cno != 0)
1232  {
1233  (*cnoSum) += cno;
1234  ++(*cnoCount);
1235  }
1236  }
1237  }
1238 
1239  return 0;
1240 }
GPS_STATUS_FIX_RTK_SINGLE
Definition: data_sets.h:375
GPS_STATUS_FIX_RTK_FLOAT
Definition: data_sets.h:376
PACKED::I
imus_t I
Definition: data_sets.h:617
parse_nmea_gns
int parse_nmea_gns(const char msg[], int msgSize, gps_pos_t *gpsPos, double datetime[6], int *satsUsed, int navMode)
Definition: protocol_nmea.cpp:815
parse_nmea_gsa
int parse_nmea_gsa(const char msg[], int msgSize, gps_pos_t *gpsPos, int *navMode)
Definition: protocol_nmea.cpp:1100
gpst0
static const double gpst0[]
Definition: protocol_nmea.cpp:71
PACKED::gnssId
uint8_t gnssId
Definition: data_sets.h:807
RMC_OPTIONS_PORT_SER1
#define RMC_OPTIONS_PORT_SER1
Definition: data_sets.h:1215
gps_to_nmea_gll
int gps_to_nmea_gll(char a[], const int aSize, gps_pos_t &pos)
Definition: protocol_nmea.cpp:453
gps_to_nmea_pashr
int gps_to_nmea_pashr(char a[], const int aSize, gps_pos_t &pos, ins_1_t &ins1, float heave, inl2_ned_sigma_t &sigma)
Definition: protocol_nmea.cpp:669
PACKED::towOffset
double towOffset
Definition: data_sets.h:774
PACKED::repoRevision
uint32_t repoRevision
Definition: data_sets.h:457
gtime_t::sec
double sec
Definition: data_sets.h:2012
PACKED::pashr
uint32_t pashr
Definition: data_sets.h:1175
PACKED::leapS
uint8_t leapS
Definition: data_sets.h:777
PACKED::gpgga
uint32_t gpgga
Definition: data_sets.h:1160
PACKED::gprmc
uint32_t gprmc
Definition: data_sets.h:1169
PACKED::qn2b
float qn2b[4]
Definition: data_sets.h:534
GPS_STATUS_FIX_NONE
Definition: data_sets.h:365
GPS_STATUS_FIX_BIT_OFFSET
Definition: data_sets.h:379
set_gpsPos_status_mask
void set_gpsPos_status_mask(uint32_t *status, uint32_t state, uint32_t mask)
Definition: protocol_nmea.cpp:43
RAD2DEG
#define RAD2DEG(rad)
Definition: ISConstants.h:360
PACKED::uvw
is_can_uvw uvw
Definition: CAN_comm.h:257
DEG2RAD
#define DEG2RAD(deg)
Definition: ISConstants.h:363
C_KNOTS_METERS_F
#define C_KNOTS_METERS_F
Definition: ISConstants.h:472
PACKED::vel
float vel[3]
Definition: data_sets.h:793
Catch::end
not_this_one end(...)
PACKED::svId
uint8_t svId
Definition: data_sets.h:810
RMC_OPTIONS_PORT_USB
#define RMC_OPTIONS_PORT_USB
Definition: data_sets.h:1216
python.pylib.pose.lla2ecef
def lla2ecef(lla, metric=True)
Definition: pose.py:393
asciiSnprintfGPSDateOfLastFixCSV
static int asciiSnprintfGPSDateOfLastFixCSV(char *a, size_t aSize, gps_pos_t &pos)
Definition: protocol_nmea.cpp:364
quatConjRot
void quatConjRot(Vector3_t result, const Quat_t q, const Vector3_t v)
Definition: ISPose.c:161
asciiSnprintfGPSTimeOfLastFixMilliseconds
static int asciiSnprintfGPSTimeOfLastFixMilliseconds(char *a, size_t aSize, uint32_t timeOfWeekMs)
Definition: protocol_nmea.cpp:344
parse_nmea_zda
int parse_nmea_zda(const char msg[], int msgSize, double &day, double &month, double &year)
Definition: protocol_nmea.cpp:785
PACKED::timeOfWeekMs
float timeOfWeekMs
Definition: CAN_comm.h:29
parse_nmea_ascb
uint32_t parse_nmea_ascb(int pHandle, const char msg[], int msgSize, ascii_msgs_t asciiPeriod[NUM_COM_PORTS])
Definition: protocol_nmea.cpp:722
PACKED::gpzda
uint32_t gpzda
Definition: data_sets.h:1172
dimu_to_nmea_pimu
int dimu_to_nmea_pimu(char a[], const int aSize, dual_imu_t &dimu)
Definition: protocol_nmea.cpp:164
PACKED::pins1
uint32_t pins1
Definition: data_sets.h:1148
quat_ecef2ned
void quat_ecef2ned(float lat, float lon, float *qe2n)
Definition: ISPose.c:267
PACKED::pqr
float pqr[3]
Definition: data_sets.h:603
tow_to_nmea_ptow
int tow_to_nmea_ptow(char a[], const int aSize, double imuTow, double insTow, unsigned int gpsWeek)
Definition: protocol_nmea.cpp:152
PACKED::week
uint32_t week
Definition: CAN_comm.h:26
INS_STATUS_NAV_FIX_STATUS
#define INS_STATUS_NAV_FIX_STATUS(insStatus)
Definition: data_sets.h:237
PACKED::theta1
int16_t theta1
Definition: CAN_comm.h:48
GPS_NAV_FIX_POSITIONING_RTK_FLOAT
Definition: data_sets.h:272
mag_Vec2
#define mag_Vec2(v)
Definition: ISMatrix.h:33
PACKED::vel2
int16_t vel2
Definition: CAN_comm.h:191
gps_to_nmea_pgpsp
int gps_to_nmea_pgpsp(char a[], const int aSize, gps_pos_t &pos, gps_vel_t &vel)
Definition: protocol_nmea.cpp:288
PACKED::timeOfWeek
double timeOfWeek
Definition: data_sets.h:417
GPS_STATUS_FLAGS_FIX_OK
Definition: data_sets.h:382
GPS_STATUS_FLAGS_BIT_OFFSET
Definition: data_sets.h:407
PACKED::ppimu
uint32_t ppimu
Definition: data_sets.h:1145
PACKED::hMSL
float hMSL
Definition: data_sets.h:759
GPS_STATUS_FLAGS_GPS_NMEA_DATA
Definition: data_sets.h:405
asciiSnprintfLonToDegMin
static int asciiSnprintfLonToDegMin(char *a, size_t aSize, double v)
Definition: protocol_nmea.cpp:326
gtime_t
Definition: data_sets.h:2006
GPS_STATUS_FIX_GPS_PLUS_DEAD_RECK
Definition: data_sets.h:369
ins1_to_nmea_pins1
int ins1_to_nmea_pins1(char a[], const int aSize, ins_1_t &ins1)
Definition: protocol_nmea.cpp:218
PACKED::pins2
uint32_t pins2
Definition: data_sets.h:1151
PACKED::pDop
float pDop
Definition: data_sets.h:768
C_METERS_KNOTS_F
#define C_METERS_KNOTS_F
Definition: ISConstants.h:470
asciiSnprintfGPSDateOfLastFix
static int asciiSnprintfGPSDateOfLastFix(char *a, size_t aSize, gps_pos_t &pos)
Definition: protocol_nmea.cpp:355
NUM_COM_PORTS
#define NUM_COM_PORTS
Definition: data_sets.h:1199
gpsToJulian
double gpsToJulian(int32_t gpsWeek, int32_t gpsMilliseconds, int32_t leapSeconds)
Definition: data_sets.c:708
PACKED::gpgsa
uint32_t gpgsa
Definition: data_sets.h:1166
PACKED::buildNumber
uint32_t buildNumber
Definition: data_sets.h:451
ASCII_find_next_field
char * ASCII_find_next_field(char *str)
Definition: protocol_nmea.cpp:25
RMC_OPTIONS_PORT_MASK
#define RMC_OPTIONS_PORT_MASK
Definition: data_sets.h:1211
PACKED::hdwStatus
uint32_t hdwStatus
Definition: CAN_comm.h:40
options
options
PACKED::hardwareVer
uint8_t hardwareVer[4]
Definition: data_sets.h:445
PACKED::cnoMean
uint32_t cnoMean
Definition: CAN_comm.h:226
INS_STATUS_SOLUTION_NAV
Definition: data_sets.h:218
Quat_t
Vector4_t Quat_t
Definition: ISPose.h:36
C_RAD2DEG_F
#define C_RAD2DEG_F
Definition: ISConstants.h:562
GPS_STATUS_FIX_DGPS
Definition: data_sets.h:373
gps_to_nmea_gsa
int gps_to_nmea_gsa(char a[], const int aSize, gps_pos_t &pos, gps_sat_t &sat)
Definition: protocol_nmea.cpp:485
strobe_to_nmea_pstrb
int strobe_to_nmea_pstrb(char a[], const int aSize, strobe_in_time_t &strobe)
Definition: protocol_nmea.cpp:275
PACKED::addInfo
char addInfo[DEVINFO_ADDINFO_STRLEN]
Definition: data_sets.h:469
PACKED::sAcc
float sAcc
Definition: data_sets.h:796
PACKED::buildDate
uint8_t buildDate[4]
Definition: data_sets.h:463
gtime_t::time
int64_t time
Definition: data_sets.h:2009
PACKED::lla
double lla[3]
Definition: data_sets.h:511
asciiSnprintfLatToDegMin
static int asciiSnprintfLatToDegMin(char *a, size_t aSize, double v)
Definition: protocol_nmea.cpp:318
PACKED::theta2
int16_t theta2
Definition: CAN_comm.h:49
PACKED::PattNED
float PattNED[3]
Definition: data_sets.h:1954
ASCII_compute_checksum
uint32_t ASCII_compute_checksum(uint8_t *str, int size)
Definition: protocol_nmea.cpp:11
PACKED::status
uint32_t status
Definition: CAN_comm.h:114
PACKED::gpgll
uint32_t gpgll
Definition: data_sets.h:1163
PACKED::manufacturer
char manufacturer[DEVINFO_MANUFACTURER_STRLEN]
Definition: data_sets.h:460
PACKED::count
uint32_t count
Definition: data_sets.h:1978
SNPRINTF
#define SNPRINTF
Definition: ISConstants.h:146
GPS_STATUS_FIX_2D
Definition: data_sets.h:367
GPS_STATUS_FIX_SBAS
Definition: data_sets.h:374
INS_STATUS_SOLUTION
#define INS_STATUS_SOLUTION(insStatus)
Definition: data_sets.h:213
dev_info_to_nmea_info
int dev_info_to_nmea_info(char a[], const int aSize, dev_info_t &info)
Definition: protocol_nmea.cpp:94
PACKED::vel1
int16_t vel1
Definition: CAN_comm.h:181
PACKED::dt
uint16_t dt
Definition: CAN_comm.h:174
GPS_STATUS_FIX_MASK
Definition: data_sets.h:378
Vector3d
double Vector3d[3]
Definition: ISConstants.h:790
PACKED::acc
float acc[3]
Definition: data_sets.h:606
PACKED::insStatus
uint32_t insStatus
Definition: CAN_comm.h:37
parse_nmea_gga
int parse_nmea_gga(const char msg[], int msgSize, gps_pos_t *gpsPos, double datetime[6], int *satsUsed, int navMode)
Definition: protocol_nmea.cpp:939
MAX_NUM_SAT_CHANNELS
#define MAX_NUM_SAT_CHANNELS
Definition: data_sets.h:146
GPS_STATUS_FIX_RTK_FIX
Definition: data_sets.h:377
time2gpst
double time2gpst(gtime_t t, int *week)
Definition: protocol_nmea.cpp:79
GPS_STATUS_FIX_DEAD_RECKONING_ONLY
Definition: data_sets.h:366
PACKED::theta
float theta[3]
Definition: data_sets.h:505
PACKED::pgpsp
uint32_t pgpsp
Definition: data_sets.h:1154
RMC_OPTIONS_PORT_CURRENT
#define RMC_OPTIONS_PORT_CURRENT
Definition: data_sets.h:1213
PACKED::serialNumber
uint32_t serialNumber
Definition: data_sets.h:442
GPS_STATUS_FIX_3D
Definition: data_sets.h:368
GPS_STATUS_FLAGS_DGPS_USED
Definition: data_sets.h:383
PACKED::ned
float ned[3]
Definition: data_sets.h:514
gps_to_nmea_gga
int gps_to_nmea_gga(char a[], const int aSize, gps_pos_t &pos)
Definition: protocol_nmea.cpp:373
PACKED::ecef
double ecef[3]
Definition: data_sets.h:420
protocol_nmea.h
GPS_NAV_FIX_POSITIONING_3D
Definition: data_sets.h:271
PACKED::sat
gps_sat_sv_t sat[MAX_NUM_SAT_CHANNELS]
Definition: data_sets.h:860
gps_to_nmea_zda
int gps_to_nmea_zda(char a[], const int aSize, gps_pos_t &pos)
Definition: protocol_nmea.cpp:648
pimu_to_nmea_ppimu
int pimu_to_nmea_ppimu(char a[], const int aSize, preintegrated_imu_t &pimu)
Definition: protocol_nmea.cpp:190
PACKED::buildTime
uint8_t buildTime[4]
Definition: data_sets.h:466
PACKED::protocolVer
uint8_t protocolVer[4]
Definition: data_sets.h:454
PACKED::firmwareVer
uint8_t firmwareVer[4]
Definition: data_sets.h:448
PACKED::hAcc
float hAcc
Definition: data_sets.h:762
PACKED::pin
uint32_t pin
Definition: data_sets.h:1975
PACKED::vAcc
float vAcc
Definition: data_sets.h:765
GPS_STATUS_FIX_TIME_ONLY
Definition: data_sets.h:370
epoch2time
gtime_t epoch2time(const double *ep)
Definition: protocol_nmea.cpp:55
RMC_OPTIONS_PORT_SER0
#define RMC_OPTIONS_PORT_SER0
Definition: data_sets.h:1214
PACKED::time
is_can_time time
Definition: CAN_comm.h:252
ddmm2deg
double ddmm2deg(double ddmm)
Definition: protocol_nmea.cpp:36
ins2_to_nmea_pins2
int ins2_to_nmea_pins2(char a[], const int aSize, ins_2_t &ins2)
Definition: protocol_nmea.cpp:248
PACKED::pimu
preintegrated_imu_t pimu
Definition: data_sets.h:708
julianToDate
void julianToDate(double julian, int32_t *year, int32_t *month, int32_t *day, int32_t *hour, int32_t *minute, int32_t *second, int32_t *millisecond)
Definition: data_sets.c:607
asciiSnprintfGPSTimeOfLastFix
static int asciiSnprintfGPSTimeOfLastFix(char *a, size_t aSize, uint32_t timeOfWeekMs)
Definition: protocol_nmea.cpp:334
gps_to_nmea_rmc
int gps_to_nmea_rmc(char a[], const int aSize, gps_pos_t &pos, gps_vel_t &vel, float magDeclination)
Definition: protocol_nmea.cpp:573
parse_nmea_gsv
int parse_nmea_gsv(const char msg[], int msgSize, gps_sat_t *gpsSat, int lastGSVmsg[2], int *satCount, uint32_t *cnoSum, uint32_t *cnoCount)
Definition: protocol_nmea.cpp:1127
parse_nmea_rmc
int parse_nmea_rmc(const char msg[], int msgSize, gps_vel_t *gpsVel, double datetime[6], int *satsUsed, int navMode)
Definition: protocol_nmea.cpp:1055
GPS_STATUS_NUM_SATS_USED_MASK
Definition: data_sets.h:362

inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04