1 /* Copyright (c) 2012-2017, The Linux Foundation. All rights reserved.
2 *
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are
5 * met:
6 * * Redistributions of source code must retain the above copyright
7 * notice, this list of conditions and the following disclaimer.
8 * * Redistributions in binary form must reproduce the above
9 * copyright notice, this list of conditions and the following
10 * disclaimer in the documentation and/or other materials provided
11 * with the distribution.
12 * * Neither the name of The Linux Foundation nor the names of its
13 * contributors may be used to endorse or promote products derived
14 * from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
17 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
20 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
25 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
26 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 *
28 */
29
30 #define LOG_NDEBUG 0
31 #define LOG_TAG "LocSvc_nmea"
32 #include <loc_nmea.h>
33 #include <math.h>
34 #include <log_util.h>
35 #include <loc_pla.h>
36 #include <loc_cfg.h>
37
38 #define GLONASS_SV_ID_OFFSET 64
39 #define QZSS_SV_ID_OFFSET (-192)
40 #define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64
41 #define MAX_SATELLITES_IN_USE 12
42 #define MSEC_IN_ONE_WEEK 604800000ULL
43 #define UTC_GPS_OFFSET_MSECS 315964800000ULL
44
45 // GNSS system id according to NMEA spec
46 #define SYSTEM_ID_GPS 1
47 #define SYSTEM_ID_GLONASS 2
48 #define SYSTEM_ID_GALILEO 3
49 #define SYSTEM_ID_BDS 4
50 #define SYSTEM_ID_QZSS 5
51 #define SYSTEM_ID_NAVIC 6
52
53 //GNSS signal id according to NMEA spec
54 #define SIGNAL_ID_ALL_SIGNALS 0
55 #define SIGNAL_ID_GPS_L1CA 1
56 #define SIGNAL_ID_GPS_L1P 2
57 #define SIGNAL_ID_GPS_L1M 3
58 #define SIGNAL_ID_GPS_L2P 4
59 #define SIGNAL_ID_GPS_L2CM 5
60 #define SIGNAL_ID_GPS_L2CL 6
61 #define SIGNAL_ID_GPS_L5I 7
62 #define SIGNAL_ID_GPS_L5Q 8
63
64
65 #define SIGNAL_ID_GLO_G1CA 1
66 #define SIGNAL_ID_GLO_G1P 2
67 #define SIGNAL_ID_GLO_G2CA 3
68 #define SIGNAL_ID_GLO_G2P 4
69
70
71 #define SIGNAL_ID_GAL_E5A 1
72 #define SIGNAL_ID_GAL_E5B 2
73 #define SIGNAL_ID_GAL_E5AB 3
74 #define SIGNAL_ID_GAL_E6A 4
75 #define SIGNAL_ID_GAL_E6BC 5
76 #define SIGNAL_ID_GAL_L1A 6
77 #define SIGNAL_ID_GAL_L1BC 7
78
79 #define SIGNAL_ID_BDS_B1I 1
80 #define SIGNAL_ID_BDS_B1Q 2
81 #define SIGNAL_ID_BDS_B1C 3
82 #define SIGNAL_ID_BDS_B1A 4
83 #define SIGNAL_ID_BDS_B2A 5
84 #define SIGNAL_ID_BDS_B2B 6
85 #define SIGNAL_ID_BDS_B2AB 7
86 #define SIGNAL_ID_BDS_B3I 8
87 #define SIGNAL_ID_BDS_B3Q 9
88 #define SIGNAL_ID_BDS_B3A 0xA
89 #define SIGNAL_ID_BDS_B2I 0xB
90 #define SIGNAL_ID_BDS_B2Q 0xC
91
92 #define SIGNAL_ID_QZSS_L1CA 1
93 #define SIGNAL_ID_QZSS_L1CD 2
94 #define SIGNAL_ID_QZSS_L1CP 3
95 #define SIGNAL_ID_QZSS_LIS 4
96 #define SIGNAL_ID_QZSS_L2CM 5
97 #define SIGNAL_ID_QZSS_L2CL 6
98 #define SIGNAL_ID_QZSS_L5I 7
99 #define SIGNAL_ID_QZSS_L5Q 8
100 #define SIGNAL_ID_QZSS_L6D 9
101 #define SIGNAL_ID_QZSS_L6E 0xA
102
103 #define SIGNAL_ID_NAVIC_L5SPS 1
104 #define SIGNAL_ID_NAVIC_SSPS 2
105 #define SIGNAL_ID_NAVIC_L5RS 3
106 #define SIGNAL_ID_NAVIC_SRS 4
107 #define SIGNAL_ID_NAVIC_L1SPS 5
108
109
110 typedef struct loc_nmea_sv_meta_s
111 {
112 char talker[3];
113 LocGnssConstellationType svType;
114 uint64_t mask;
115 uint32_t svCount;
116 uint32_t totalSvUsedCount;
117 uint32_t svIdOffset;
118 uint32_t signalId;
119 uint32_t systemId;
120 } loc_nmea_sv_meta;
121
122 typedef struct loc_sv_cache_info_s
123 {
124 uint64_t gps_used_mask;
125 uint64_t glo_used_mask;
126 uint64_t gal_used_mask;
127 uint64_t qzss_used_mask;
128 uint64_t bds_used_mask;
129 uint64_t navic_used_mask;
130 uint32_t gps_l1_count;
131 uint32_t gps_l5_count;
132 uint32_t glo_g1_count;
133 uint32_t glo_g2_count;
134 uint32_t gal_e1_count;
135 uint32_t gal_e5_count;
136 uint32_t qzss_l1_count;
137 uint32_t qzss_l5_count;
138 uint32_t bds_b1_count;
139 uint32_t bds_b2_count;
140 uint32_t navic_l5_count;
141 float hdop;
142 float pdop;
143 float vdop;
144 } loc_sv_cache_info;
145
146 /*===========================================================================
147 FUNCTION convert_Lla_to_Ecef
148
149 DESCRIPTION
150 Convert LLA to ECEF
151
152 DEPENDENCIES
153 NONE
154
155 RETURN VALUE
156 NONE
157
158 SIDE EFFECTS
159 N/A
160
161 ===========================================================================*/
convert_Lla_to_Ecef(const LocLla & plla,LocEcef & pecef)162 static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef)
163 {
164 double r;
165
166 r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat));
167 pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon);
168 pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon);
169 pecef.Z = (r * OMES + plla.alt) * sin(plla.lat);
170 }
171
172 /*===========================================================================
173 FUNCTION convert_WGS84_to_PZ90
174
175 DESCRIPTION
176 Convert datum from WGS84 to PZ90
177
178 DEPENDENCIES
179 NONE
180
181 RETURN VALUE
182 NONE
183
184 SIDE EFFECTS
185 N/A
186
187 ===========================================================================*/
convert_WGS84_to_PZ90(const LocEcef & pWGS84,LocEcef & pPZ90)188 static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90)
189 {
190 double deltaX = DatumConstFromWGS84[0];
191 double deltaY = DatumConstFromWGS84[1];
192 double deltaZ = DatumConstFromWGS84[2];
193 double deltaScale = DatumConstFromWGS84[3];
194 double rotX = DatumConstFromWGS84[4];
195 double rotY = DatumConstFromWGS84[5];
196 double rotZ = DatumConstFromWGS84[6];
197
198 pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z);
199 pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z);
200 pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y);
201 }
202
203 /*===========================================================================
204 FUNCTION convert_Ecef_to_Lla
205
206 DESCRIPTION
207 Convert ECEF to LLA
208
209 DEPENDENCIES
210 NONE
211
212 RETURN VALUE
213 NONE
214
215 SIDE EFFECTS
216 N/A
217
218 ===========================================================================*/
convert_Ecef_to_Lla(const LocEcef & pecef,LocLla & plla)219 static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla)
220 {
221 double p, r;
222 double EcefA = C_PZ90A;
223 double EcefB = C_PZ90B;
224 double Ecef1Mf;
225 double EcefE2;
226 double Mu;
227 double Smu;
228 double Cmu;
229 double Phi;
230 double Sphi;
231 double N;
232
233 p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y);
234 r = sqrt(p * p + pecef.Z * pecef.Z);
235 if (r < 1.0) {
236 plla.lat = 1.0;
237 plla.lon = 1.0;
238 plla.alt = 1.0;
239 }
240 Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA;
241 EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA);
242 if (p > 1.0) {
243 Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p);
244 } else {
245 if (pecef.Z > 0.0) {
246 Mu = M_PI / 2.0;
247 } else {
248 Mu = -M_PI / 2.0;
249 }
250 }
251 Smu = sin(Mu);
252 Cmu = cos(Mu);
253 Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu,
254 Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu));
255 Sphi = sin(Phi);
256 N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi);
257 plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N;
258 plla.lat = Phi;
259 if ( p > 1.0) {
260 plla.lon = atan2(pecef.Y, pecef.X);
261 } else {
262 plla.lon = 0.0;
263 }
264 }
265
266 /*===========================================================================
267 FUNCTION convert_signalType_to_signalId
268
269 DESCRIPTION
270 convert signalType to signal ID
271
272 DEPENDENCIES
273 NONE
274
275 RETURN VALUE
276 value of signal ID
277
278 SIDE EFFECTS
279 N/A
280
281 ===========================================================================*/
convert_signalType_to_signalId(GnssSignalTypeMask signalType)282 static uint32_t convert_signalType_to_signalId(GnssSignalTypeMask signalType)
283 {
284 uint32_t signalId = SIGNAL_ID_ALL_SIGNALS;
285
286 switch (signalType) {
287 case GNSS_SIGNAL_GPS_L1CA:
288 signalId = SIGNAL_ID_GPS_L1CA;
289 break;
290 case GNSS_SIGNAL_GPS_L2:
291 signalId = SIGNAL_ID_GPS_L2CL;
292 break;
293 case GNSS_SIGNAL_GPS_L5:
294 signalId = SIGNAL_ID_GPS_L5Q;
295 break;
296 case GNSS_SIGNAL_GLONASS_G1:
297 signalId = SIGNAL_ID_GLO_G1CA;
298 break;
299 case GNSS_SIGNAL_GLONASS_G2:
300 signalId = SIGNAL_ID_GLO_G2CA;
301 break;
302 case GNSS_SIGNAL_GALILEO_E1:
303 signalId = SIGNAL_ID_GAL_L1BC;
304 break;
305 case GNSS_SIGNAL_GALILEO_E5A:
306 signalId = SIGNAL_ID_GAL_E5A;
307 break;
308 case GNSS_SIGNAL_GALILEO_E5B:
309 signalId = SIGNAL_ID_GAL_E5B;
310 break;
311 case GNSS_SIGNAL_QZSS_L1CA:
312 signalId = SIGNAL_ID_QZSS_L1CA;
313 break;
314 case GNSS_SIGNAL_QZSS_L2:
315 signalId = SIGNAL_ID_QZSS_L2CL;
316 break;
317 case GNSS_SIGNAL_QZSS_L5:
318 signalId = SIGNAL_ID_QZSS_L5Q;
319 break;
320 case GNSS_SIGNAL_BEIDOU_B1I:
321 signalId = SIGNAL_ID_BDS_B1I;
322 break;
323 case GNSS_SIGNAL_BEIDOU_B1C:
324 signalId = SIGNAL_ID_BDS_B1C;
325 break;
326 case GNSS_SIGNAL_BEIDOU_B2I:
327 signalId = SIGNAL_ID_BDS_B2I;
328 break;
329 case GNSS_SIGNAL_BEIDOU_B2AI:
330 case GNSS_SIGNAL_BEIDOU_B2AQ:
331 signalId = SIGNAL_ID_BDS_B2A;
332 break;
333 case GNSS_SIGNAL_NAVIC_L5:
334 signalId = SIGNAL_ID_NAVIC_L5SPS;
335 break;
336 default:
337 signalId = SIGNAL_ID_ALL_SIGNALS;
338 }
339
340 return signalId;
341
342 }
343
344 /*===========================================================================
345 FUNCTION get_sv_count_from_mask
346
347 DESCRIPTION
348 get the sv count from bit mask
349
350 DEPENDENCIES
351 NONE
352
353 RETURN VALUE
354 value of sv count
355
356 SIDE EFFECTS
357 N/A
358
359 ===========================================================================*/
get_sv_count_from_mask(uint64_t svMask,int totalSvCount)360 static uint32_t get_sv_count_from_mask(uint64_t svMask, int totalSvCount)
361 {
362 int index = 0;
363 uint32_t svCount = 0;
364
365 if(totalSvCount > MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION) {
366 LOC_LOGE("total SV count in this constellation %d exceeded limit %d",
367 totalSvCount, MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION);
368 }
369 for(index = 0; index < totalSvCount; index++) {
370 if(svMask & 0x1)
371 svCount += 1;
372 svMask >>= 1;
373 }
374 return svCount;
375 }
376
377 /*===========================================================================
378 FUNCTION loc_nmea_sv_meta_init
379
380 DESCRIPTION
381 Init loc_nmea_sv_meta passed in
382
383 DEPENDENCIES
384 NONE
385
386 RETURN VALUE
387 Pointer to loc_nmea_sv_meta
388
389 SIDE EFFECTS
390 N/A
391
392 ===========================================================================*/
loc_nmea_sv_meta_init(loc_nmea_sv_meta & sv_meta,loc_sv_cache_info & sv_cache_info,GnssSvType svType,GnssSignalTypeMask signalType,bool needCombine)393 static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
394 loc_sv_cache_info& sv_cache_info,
395 GnssSvType svType,
396 GnssSignalTypeMask signalType,
397 bool needCombine)
398 {
399 memset(&sv_meta, 0, sizeof(sv_meta));
400 sv_meta.svType = svType;
401
402 switch (svType)
403 {
404 case GNSS_SV_TYPE_GPS:
405 sv_meta.talker[0] = 'G';
406 sv_meta.talker[1] = 'P';
407 sv_meta.mask = sv_cache_info.gps_used_mask;
408 sv_meta.systemId = SYSTEM_ID_GPS;
409 if (GNSS_SIGNAL_GPS_L1CA == signalType) {
410 sv_meta.svCount = sv_cache_info.gps_l1_count;
411 } else if (GNSS_SIGNAL_GPS_L5 == signalType) {
412 sv_meta.svCount = sv_cache_info.gps_l5_count;
413 }
414 break;
415 case GNSS_SV_TYPE_GLONASS:
416 sv_meta.talker[0] = 'G';
417 sv_meta.talker[1] = 'L';
418 sv_meta.mask = sv_cache_info.glo_used_mask;
419 // GLONASS SV ids are from 65-96
420 sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET;
421 sv_meta.systemId = SYSTEM_ID_GLONASS;
422 if (GNSS_SIGNAL_GLONASS_G1 == signalType) {
423 sv_meta.svCount = sv_cache_info.glo_g1_count;
424 } else if (GNSS_SIGNAL_GLONASS_G2 == signalType) {
425 sv_meta.svCount = sv_cache_info.glo_g2_count;
426 }
427 break;
428 case GNSS_SV_TYPE_GALILEO:
429 sv_meta.talker[0] = 'G';
430 sv_meta.talker[1] = 'A';
431 sv_meta.mask = sv_cache_info.gal_used_mask;
432 sv_meta.systemId = SYSTEM_ID_GALILEO;
433 if (GNSS_SIGNAL_GALILEO_E1 == signalType) {
434 sv_meta.svCount = sv_cache_info.gal_e1_count;
435 } else if (GNSS_SIGNAL_GALILEO_E5A == signalType) {
436 sv_meta.svCount = sv_cache_info.gal_e5_count;
437 }
438 break;
439 case GNSS_SV_TYPE_QZSS:
440 sv_meta.talker[0] = 'G';
441 sv_meta.talker[1] = 'Q';
442 sv_meta.mask = sv_cache_info.qzss_used_mask;
443 // QZSS SV ids are from 193-199. So keep svIdOffset -192
444 sv_meta.svIdOffset = QZSS_SV_ID_OFFSET;
445 sv_meta.systemId = SYSTEM_ID_QZSS;
446 if (GNSS_SIGNAL_QZSS_L1CA == signalType) {
447 sv_meta.svCount = sv_cache_info.qzss_l1_count;
448 } else if (GNSS_SIGNAL_QZSS_L5 == signalType) {
449 sv_meta.svCount = sv_cache_info.qzss_l5_count;
450 }
451 break;
452 case GNSS_SV_TYPE_BEIDOU:
453 sv_meta.talker[0] = 'G';
454 sv_meta.talker[1] = 'B';
455 sv_meta.mask = sv_cache_info.bds_used_mask;
456 // BDS SV ids are from 201-235. So keep svIdOffset 0
457 sv_meta.systemId = SYSTEM_ID_BDS;
458 if (GNSS_SIGNAL_BEIDOU_B1I == signalType) {
459 sv_meta.svCount = sv_cache_info.bds_b1_count;
460 } else if (GNSS_SIGNAL_BEIDOU_B2AI == signalType) {
461 sv_meta.svCount = sv_cache_info.bds_b2_count;
462 }
463 break;
464 case GNSS_SV_TYPE_NAVIC:
465 sv_meta.talker[0] = 'G';
466 sv_meta.talker[1] = 'I';
467 sv_meta.mask = sv_cache_info.navic_used_mask;
468 // NAVIC SV ids are from 401-414. So keep svIdOffset 0
469 sv_meta.systemId = SYSTEM_ID_NAVIC;
470 if (GNSS_SIGNAL_NAVIC_L5 == signalType) {
471 sv_meta.svCount = sv_cache_info.navic_l5_count;
472 }
473 break;
474 default:
475 LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
476 return NULL;
477 }
478 sv_meta.signalId = convert_signalType_to_signalId(signalType);
479 sv_meta.totalSvUsedCount =
480 get_sv_count_from_mask(sv_cache_info.gps_used_mask,
481 GPS_SV_PRN_MAX - GPS_SV_PRN_MIN + 1) +
482 get_sv_count_from_mask(sv_cache_info.glo_used_mask,
483 GLO_SV_PRN_MAX - GLO_SV_PRN_MIN + 1) +
484 get_sv_count_from_mask(sv_cache_info.gal_used_mask,
485 GAL_SV_PRN_MAX - GAL_SV_PRN_MIN + 1) +
486 get_sv_count_from_mask(sv_cache_info.qzss_used_mask,
487 QZSS_SV_PRN_MAX - QZSS_SV_PRN_MIN + 1) +
488 get_sv_count_from_mask(sv_cache_info.bds_used_mask,
489 BDS_SV_PRN_MAX - BDS_SV_PRN_MIN + 1) +
490 get_sv_count_from_mask(sv_cache_info.navic_used_mask,
491 NAVIC_SV_PRN_MAX - NAVIC_SV_PRN_MIN + 1);
492 if (needCombine &&
493 (sv_cache_info.gps_used_mask ? 1 : 0) +
494 (sv_cache_info.glo_used_mask ? 1 : 0) +
495 (sv_cache_info.gal_used_mask ? 1 : 0) +
496 (sv_cache_info.qzss_used_mask ? 1 : 0) +
497 (sv_cache_info.bds_used_mask ? 1 : 0) +
498 (sv_cache_info.navic_used_mask ? 1 : 0) > 1)
499 {
500 // If GPS, GLONASS, Galileo, QZSS, BDS etc. are combined
501 // to obtain the reported position solution,
502 // talker shall be set to GN, to indicate that
503 // the satellites are used in a combined solution
504 sv_meta.talker[0] = 'G';
505 sv_meta.talker[1] = 'N';
506 }
507 return &sv_meta;
508 }
509
510 /*===========================================================================
511 FUNCTION loc_nmea_put_checksum
512
513 DESCRIPTION
514 Generate NMEA sentences generated based on position report
515
516 DEPENDENCIES
517 NONE
518
519 RETURN VALUE
520 Total length of the nmea sentence
521
522 SIDE EFFECTS
523 N/A
524
525 ===========================================================================*/
loc_nmea_put_checksum(char * pNmea,int maxSize)526 static int loc_nmea_put_checksum(char *pNmea, int maxSize)
527 {
528 uint8_t checksum = 0;
529 int length = 0;
530 if(NULL == pNmea)
531 return 0;
532
533 pNmea++; //skip the $
534 while (*pNmea != '\0')
535 {
536 checksum ^= *pNmea++;
537 length++;
538 }
539
540 // length now contains nmea sentence string length not including $ sign.
541 int checksumLength = snprintf(pNmea,(maxSize-length-1),"*%02X\r\n", checksum);
542
543 // total length of nmea sentence is length of nmea sentence inc $ sign plus
544 // length of checksum (+1 is to cover the $ character in the length).
545 return (length + checksumLength + 1);
546 }
547
548 /*===========================================================================
549 FUNCTION loc_nmea_generate_GSA
550
551 DESCRIPTION
552 Generate NMEA GSA sentences generated based on position report
553 Currently below sentences are generated:
554 - $GPGSA : GPS DOP and active SVs
555 - $GLGSA : GLONASS DOP and active SVs
556 - $GAGSA : GALILEO DOP and active SVs
557 - $GNGSA : GNSS DOP and active SVs
558
559 DEPENDENCIES
560 NONE
561
562 RETURN VALUE
563 Number of SVs used
564
565 SIDE EFFECTS
566 N/A
567
568 ===========================================================================*/
loc_nmea_generate_GSA(const GpsLocationExtended & locationExtended,char * sentence,int bufSize,loc_nmea_sv_meta * sv_meta_p,std::vector<std::string> & nmeaArraystr)569 static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended,
570 char* sentence,
571 int bufSize,
572 loc_nmea_sv_meta* sv_meta_p,
573 std::vector<std::string> &nmeaArraystr)
574 {
575 if (!sentence || bufSize <= 0 || !sv_meta_p)
576 {
577 LOC_LOGE("NMEA Error invalid arguments.");
578 return 0;
579 }
580
581 char* pMarker = sentence;
582 int lengthRemaining = bufSize;
583 int length = 0;
584
585 uint32_t svUsedCount = 0;
586 uint32_t svUsedList[64] = {0};
587
588 char fixType = '\0';
589
590 const char* talker = sv_meta_p->talker;
591 uint32_t svIdOffset = sv_meta_p->svIdOffset;
592 uint64_t mask = sv_meta_p->mask;
593
594 if(sv_meta_p->svType != GNSS_SV_TYPE_GLONASS) {
595 svIdOffset = 0;
596 }
597
598 for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++)
599 {
600 if (mask & 1)
601 svUsedList[svUsedCount++] = i + svIdOffset;
602 mask = mask >> 1;
603 }
604
605 if (svUsedCount == 0)
606 return 0;
607
608 if (sv_meta_p->totalSvUsedCount == 0)
609 fixType = '1'; // no fix
610 else if (sv_meta_p->totalSvUsedCount <= 3)
611 fixType = '2'; // 2D fix
612 else
613 fixType = '3'; // 3D fix
614
615 // Start printing the sentence
616 // Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v,s*cc
617 // a : Mode : A : Automatic, allowed to automatically switch 2D/3D
618 // x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix)
619 // xx : 12 SV ID
620 // p.p : Position DOP (Dilution of Precision)
621 // h.h : Horizontal DOP
622 // v.v : Vertical DOP
623 // s : GNSS System Id
624 // cc : Checksum value
625 length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType);
626
627 if (length < 0 || length >= lengthRemaining)
628 {
629 LOC_LOGE("NMEA Error in string formatting");
630 return 0;
631 }
632 pMarker += length;
633 lengthRemaining -= length;
634
635 // Add first 12 satellite IDs
636 for (uint8_t i = 0; i < 12; i++)
637 {
638 if (i < svUsedCount)
639 length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[i]);
640 else
641 length = snprintf(pMarker, lengthRemaining, ",");
642
643 if (length < 0 || length >= lengthRemaining)
644 {
645 LOC_LOGE("NMEA Error in string formatting");
646 return 0;
647 }
648 pMarker += length;
649 lengthRemaining -= length;
650 }
651
652 // Add the position/horizontal/vertical DOP values
653 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
654 {
655 length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,",
656 locationExtended.pdop,
657 locationExtended.hdop,
658 locationExtended.vdop);
659 }
660 else
661 { // no dop
662 length = snprintf(pMarker, lengthRemaining, ",,,");
663 }
664 pMarker += length;
665 lengthRemaining -= length;
666
667 // system id
668 length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId);
669 pMarker += length;
670 lengthRemaining -= length;
671
672 /* Sentence is ready, add checksum and broadcast */
673 length = loc_nmea_put_checksum(sentence, bufSize);
674 nmeaArraystr.push_back(sentence);
675
676 return svUsedCount;
677 }
678
679 /*===========================================================================
680 FUNCTION loc_nmea_generate_GSV
681
682 DESCRIPTION
683 Generate NMEA GSV sentences generated based on sv report
684 Currently below sentences are generated:
685 - $GPGSV: GPS Satellites in View
686 - $GLGSV: GLONASS Satellites in View
687 - $GAGSV: GALILEO Satellites in View
688
689 DEPENDENCIES
690 NONE
691
692 RETURN VALUE
693 NONE
694
695 SIDE EFFECTS
696 N/A
697
698 ===========================================================================*/
loc_nmea_generate_GSV(const GnssSvNotification & svNotify,char * sentence,int bufSize,loc_nmea_sv_meta * sv_meta_p,std::vector<std::string> & nmeaArraystr)699 static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
700 char* sentence,
701 int bufSize,
702 loc_nmea_sv_meta* sv_meta_p,
703 std::vector<std::string> &nmeaArraystr)
704 {
705 if (!sentence || bufSize <= 0)
706 {
707 LOC_LOGE("NMEA Error invalid argument.");
708 return;
709 }
710
711 char* pMarker = sentence;
712 int lengthRemaining = bufSize;
713 int length = 0;
714 int sentenceCount = 0;
715 int sentenceNumber = 1;
716 size_t svNumber = 1;
717
718 const char* talker = sv_meta_p->talker;
719 uint32_t svIdOffset = sv_meta_p->svIdOffset;
720 int svCount = sv_meta_p->svCount;
721 if (svCount <= 0)
722 {
723 LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId);
724 return;
725 }
726
727 svNumber = 1;
728 sentenceNumber = 1;
729 sentenceCount = svCount / 4 + (svCount % 4 != 0);
730
731 while (sentenceNumber <= sentenceCount)
732 {
733 pMarker = sentence;
734 lengthRemaining = bufSize;
735
736 length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d",
737 talker, sentenceCount, sentenceNumber, svCount);
738
739 if (length < 0 || length >= lengthRemaining)
740 {
741 LOC_LOGE("NMEA Error in string formatting");
742 return;
743 }
744 pMarker += length;
745 lengthRemaining -= length;
746
747 for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++)
748 {
749 GnssSignalTypeMask signalType = svNotify.gnssSvs[svNumber-1].gnssSignalTypeMask;
750 if (0 == signalType) {
751 // If no signal type in report, it means default L1,G1,E1,B1I
752 switch (svNotify.gnssSvs[svNumber - 1].type)
753 {
754 case GNSS_SV_TYPE_GPS:
755 signalType = GNSS_SIGNAL_GPS_L1CA;
756 break;
757 case GNSS_SV_TYPE_GLONASS:
758 signalType = GNSS_SIGNAL_GLONASS_G1;
759 break;
760 case GNSS_SV_TYPE_GALILEO:
761 signalType = GNSS_SIGNAL_GALILEO_E1;
762 break;
763 case GNSS_SV_TYPE_QZSS:
764 signalType = GNSS_SIGNAL_QZSS_L1CA;
765 break;
766 case GNSS_SV_TYPE_BEIDOU:
767 signalType = GNSS_SIGNAL_BEIDOU_B1I;
768 break;
769 case GNSS_SV_TYPE_SBAS:
770 signalType = GNSS_SIGNAL_SBAS_L1;
771 break;
772 case GNSS_SV_TYPE_NAVIC:
773 signalType = GNSS_SIGNAL_NAVIC_L5;
774 break;
775 default:
776 LOC_LOGE("NMEA Error unknow constellation type: %d",
777 svNotify.gnssSvs[svNumber - 1].type);
778 continue;
779 }
780 }
781
782 if (sv_meta_p->svType == svNotify.gnssSvs[svNumber - 1].type &&
783 sv_meta_p->signalId == convert_signalType_to_signalId(signalType))
784 {
785 length = snprintf(pMarker, lengthRemaining,",%02d,%02d,%03d,",
786 svNotify.gnssSvs[svNumber - 1].svId + svIdOffset,
787 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
788 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
789
790 if (length < 0 || length >= lengthRemaining)
791 {
792 LOC_LOGE("NMEA Error in string formatting");
793 return;
794 }
795 pMarker += length;
796 lengthRemaining -= length;
797
798 if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0)
799 {
800 length = snprintf(pMarker, lengthRemaining,"%02d",
801 (int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int
802
803 if (length < 0 || length >= lengthRemaining)
804 {
805 LOC_LOGE("NMEA Error in string formatting");
806 return;
807 }
808 pMarker += length;
809 lengthRemaining -= length;
810 }
811
812 i++;
813 }
814
815 }
816
817 // append signalId
818 length = snprintf(pMarker, lengthRemaining,",%X",sv_meta_p->signalId);
819 pMarker += length;
820 lengthRemaining -= length;
821
822 length = loc_nmea_put_checksum(sentence, bufSize);
823 nmeaArraystr.push_back(sentence);
824 sentenceNumber++;
825
826 } //while
827 }
828
829 /*===========================================================================
830 FUNCTION loc_nmea_generate_DTM
831
832 DESCRIPTION
833 Generate NMEA DTM sentences generated based on position report
834
835 DEPENDENCIES
836 NONE
837
838 RETURN VALUE
839 NONE
840
841 SIDE EFFECTS
842 N/A
843
844 ===========================================================================*/
loc_nmea_generate_DTM(const LocLla & ref_lla,const LocLla & local_lla,char * talker,char * sentence,int bufSize)845 static void loc_nmea_generate_DTM(const LocLla &ref_lla,
846 const LocLla &local_lla,
847 char *talker,
848 char *sentence,
849 int bufSize)
850 {
851 char* pMarker = sentence;
852 int lengthRemaining = bufSize;
853 int length = 0;
854 int datum_type;
855 char ref_datum[4] = {0};
856 char local_datum[4] = {0};
857 double lla_offset[3] = {0};
858 char latHem, longHem;
859 double latMins, longMins;
860
861
862
863 datum_type = loc_get_datum_type();
864 switch (datum_type) {
865 case LOC_GNSS_DATUM_WGS84:
866 ref_datum[0] = 'W';
867 ref_datum[1] = '8';
868 ref_datum[2] = '4';
869 local_datum[0] = 'P';
870 local_datum[1] = '9';
871 local_datum[2] = '0';
872 break;
873 case LOC_GNSS_DATUM_PZ90:
874 ref_datum[0] = 'P';
875 ref_datum[1] = '9';
876 ref_datum[2] = '0';
877 local_datum[0] = 'W';
878 local_datum[1] = '8';
879 local_datum[2] = '4';
880 break;
881 default:
882 break;
883 }
884 length = snprintf(pMarker , lengthRemaining , "$%sDTM,%s,," , talker, local_datum);
885 if (length < 0 || length >= lengthRemaining) {
886 LOC_LOGE("NMEA Error in string formatting");
887 return;
888 }
889 pMarker += length;
890 lengthRemaining -= length;
891
892 lla_offset[0] = local_lla.lat - ref_lla.lat;
893 lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0);
894 if (lla_offset[1] < -180.0) {
895 lla_offset[1] += 360.0;
896 } else if ( lla_offset[1] > 180.0) {
897 lla_offset[1] -= 360.0;
898 }
899 lla_offset[2] = local_lla.alt - ref_lla.alt;
900 if (lla_offset[0] > 0.0) {
901 latHem = 'N';
902 } else {
903 latHem = 'S';
904 lla_offset[0] *= -1.0;
905 }
906 latMins = fmod(lla_offset[0] * 60.0, 60.0);
907 if (lla_offset[1] < 0.0) {
908 longHem = 'W';
909 lla_offset[1] *= -1.0;
910 }else {
911 longHem = 'E';
912 }
913 longMins = fmod(lla_offset[1] * 60.0, 60.0);
914 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,",
915 (uint8_t)floor(lla_offset[0]), latMins, latHem,
916 (uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]);
917 if (length < 0 || length >= lengthRemaining) {
918 LOC_LOGE("NMEA Error in string formatting");
919 return;
920 }
921 pMarker += length;
922 lengthRemaining -= length;
923 length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum);
924 if (length < 0 || length >= lengthRemaining) {
925 LOC_LOGE("NMEA Error in string formatting");
926 return;
927 }
928 pMarker += length;
929 lengthRemaining -= length;
930
931 length = loc_nmea_put_checksum(sentence, bufSize);
932 }
933
934 /*===========================================================================
935 FUNCTION get_utctime_with_leapsecond_transition
936
937 DESCRIPTION
938 This function returns true if the position report is generated during
939 leap second transition period. If not, then the utc timestamp returned
940 will be set to the timestamp in the position report. If it is,
941 then the utc timestamp returned will need to take into account
942 of the leap second transition so that proper calendar year/month/date
943 can be calculated from the returned utc timestamp.
944
945 DEPENDENCIES
946 NONE
947
948 RETURN VALUE
949 true: position report is generated in leap second transition period.
950
951 SIDE EFFECTS
952 N/A
953
954 ===========================================================================*/
get_utctime_with_leapsecond_transition(const UlpLocation & location,const GpsLocationExtended & locationExtended,const LocationSystemInfo & systemInfo,LocGpsUtcTime & utcPosTimestamp)955 static bool get_utctime_with_leapsecond_transition(
956 const UlpLocation &location,
957 const GpsLocationExtended &locationExtended,
958 const LocationSystemInfo &systemInfo,
959 LocGpsUtcTime &utcPosTimestamp)
960 {
961 bool inTransition = false;
962
963 // position report is not generated during leap second transition,
964 // we can use the UTC timestamp from position report as is
965 utcPosTimestamp = location.gpsLocation.timestamp;
966
967 // Check whether we are in leap second transition.
968 // If so, per NMEA spec, we need to display the extra second in format of 23:59:60
969 // with year/month/date not getting advanced.
970 if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) &&
971 ((systemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) &&
972 (systemInfo.leapSecondSysInfo.leapSecondInfoMask &
973 LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT))) {
974
975 const LeapSecondChangeInfo &leapSecondChangeInfo =
976 systemInfo.leapSecondSysInfo.leapSecondChangeInfo;
977 const GnssSystemTimeStructType &gpsTimestampLsChange =
978 leapSecondChangeInfo.gpsTimestampLsChange;
979
980 uint64_t gpsTimeLsChange = gpsTimestampLsChange.systemWeek * MSEC_IN_ONE_WEEK +
981 gpsTimestampLsChange.systemMsec;
982 uint64_t gpsTimePosReport = locationExtended.gpsTime.gpsWeek * MSEC_IN_ONE_WEEK +
983 locationExtended.gpsTime.gpsTimeOfWeekMs;
984 // we are only dealing with positive leap second change, as negative
985 // leap second change has never occurred and should not occur in future
986 if (leapSecondChangeInfo.leapSecondsAfterChange >
987 leapSecondChangeInfo.leapSecondsBeforeChange) {
988 // leap second adjustment is always 1 second at a time. It can happen
989 // every quarter end and up to four times per year.
990 if ((gpsTimePosReport >= gpsTimeLsChange) &&
991 (gpsTimePosReport < (gpsTimeLsChange + 1000))) {
992 inTransition = true;
993 utcPosTimestamp = gpsTimeLsChange + UTC_GPS_OFFSET_MSECS -
994 leapSecondChangeInfo.leapSecondsBeforeChange * 1000;
995
996 // we substract 1000 milli-seconds from UTC timestmap in order to calculate the
997 // proper year, month and date during leap second transtion.
998 // Let us give an example, assuming leap second transition is scheduled on 2019,
999 // Dec 31st mid night. When leap second transition is happening,
1000 // instead of outputting the time as 2020, Jan, 1st, 00 hour, 00 min, and 00 sec.
1001 // The time need to be displayed as 2019, Dec, 31st, 23 hour, 59 min and 60 sec.
1002 utcPosTimestamp -= 1000;
1003 }
1004 }
1005 }
1006 return inTransition;
1007 }
1008
1009 /*===========================================================================
1010 FUNCTION loc_nmea_get_fix_quality
1011
1012 DESCRIPTION
1013 This function obtains the fix quality for GGA sentence, mode indicator
1014 for RMC and VTG sentence based on nav solution mask and tech mask in
1015 the postion report.
1016
1017 DEPENDENCIES
1018 NONE
1019
1020 Output parameter
1021 ggaGpsQuality: gps quality field in GGA sentence
1022 rmcModeIndicator: mode indicator field in RMC sentence
1023 vtgModeIndicator: mode indicator field in VTG sentence
1024
1025 SIDE EFFECTS
1026 N/A
1027
1028 ===========================================================================*/
loc_nmea_get_fix_quality(const UlpLocation & location,const GpsLocationExtended & locationExtended,char & ggaGpsQuality,char & rmcModeIndicator,char & vtgModeIndicator)1029 static void loc_nmea_get_fix_quality(const UlpLocation & location,
1030 const GpsLocationExtended & locationExtended,
1031 char & ggaGpsQuality,
1032 char & rmcModeIndicator,
1033 char & vtgModeIndicator) {
1034
1035 ggaGpsQuality = '0';
1036 rmcModeIndicator = 'N';
1037 vtgModeIndicator = 'N';
1038
1039 do {
1040 if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){
1041 ggaGpsQuality = '0'; // 0 means no fix
1042 rmcModeIndicator = 'N';
1043 vtgModeIndicator = 'N';
1044 break;
1045 }
1046 // NOTE: Order of the check is important
1047 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
1048 if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
1049 ggaGpsQuality = '2'; // 2 means DGPS fix
1050 rmcModeIndicator = 'P'; // P means precise
1051 vtgModeIndicator = 'P'; // P means precise
1052 break;
1053 } else if (LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask){
1054 ggaGpsQuality = '4'; // 4 means RTK Fixed fix
1055 rmcModeIndicator = 'R'; // use R (RTK fixed)
1056 vtgModeIndicator = 'D'; // use D (differential) as
1057 // no RTK fixed defined for VTG in NMEA 183 spec
1058 break;
1059 } else if (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask){
1060 ggaGpsQuality = '5'; // 5 means RTK float fix
1061 rmcModeIndicator = 'F'; // F means RTK float fix
1062 vtgModeIndicator = 'D'; // use D (differential) as
1063 // no RTK float defined for VTG in NMEA 183 spec
1064 break;
1065 } else if (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask){
1066 ggaGpsQuality = '2'; // 2 means DGPS fix
1067 rmcModeIndicator = 'D'; // D means differential
1068 vtgModeIndicator = 'D'; // D means differential
1069 break;
1070 } else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask){
1071 ggaGpsQuality = '2'; // 2 means DGPS fix
1072 rmcModeIndicator = 'D'; // D means differential
1073 vtgModeIndicator = 'D'; // D means differential
1074 break;
1075 }
1076 }
1077 // NOTE: Order of the check is important
1078 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
1079 if (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask){
1080 ggaGpsQuality = '1'; // 1 means GPS
1081 rmcModeIndicator = 'A'; // A means autonomous
1082 vtgModeIndicator = 'A'; // A means autonomous
1083 break;
1084 } else if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){
1085 ggaGpsQuality = '6'; // 6 means estimated (dead reckoning)
1086 rmcModeIndicator = 'E'; // E means estimated (dead reckoning)
1087 vtgModeIndicator = 'E'; // E means estimated (dead reckoning)
1088 break;
1089 }
1090 }
1091 } while (0);
1092
1093 LOC_LOGv("gps quality: %c, rmc mode indicator: %c, vtg mode indicator: %c",
1094 ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
1095 }
1096
1097 /*===========================================================================
1098 FUNCTION loc_nmea_generate_pos
1099
1100 DESCRIPTION
1101 Generate NMEA sentences generated based on position report
1102 Currently below sentences are generated within this function:
1103 - $GPGSA : GPS DOP and active SVs
1104 - $GLGSA : GLONASS DOP and active SVs
1105 - $GAGSA : GALILEO DOP and active SVs
1106 - $GNGSA : GNSS DOP and active SVs
1107 - $--VTG : Track made good and ground speed
1108 - $--RMC : Recommended minimum navigation information
1109 - $--GGA : Time, position and fix related data
1110
1111 DEPENDENCIES
1112 NONE
1113
1114 RETURN VALUE
1115 0
1116
1117 SIDE EFFECTS
1118 N/A
1119
1120 ===========================================================================*/
loc_nmea_generate_pos(const UlpLocation & location,const GpsLocationExtended & locationExtended,const LocationSystemInfo & systemInfo,unsigned char generate_nmea,std::vector<std::string> & nmeaArraystr)1121 void loc_nmea_generate_pos(const UlpLocation &location,
1122 const GpsLocationExtended &locationExtended,
1123 const LocationSystemInfo &systemInfo,
1124 unsigned char generate_nmea,
1125 std::vector<std::string> &nmeaArraystr)
1126 {
1127 ENTRY_LOG();
1128
1129 LocGpsUtcTime utcPosTimestamp = 0;
1130 bool inLsTransition = false;
1131
1132 inLsTransition = get_utctime_with_leapsecond_transition
1133 (location, locationExtended, systemInfo, utcPosTimestamp);
1134
1135 time_t utcTime(utcPosTimestamp/1000);
1136 tm * pTm = gmtime(&utcTime);
1137 if (NULL == pTm) {
1138 LOC_LOGE("gmtime failed");
1139 return;
1140 }
1141
1142 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
1143 char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0};
1144 char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0};
1145 char sentence_GNS[NMEA_SENTENCE_MAX_LENGTH] = {0};
1146 char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0};
1147 char* pMarker = sentence;
1148 int lengthRemaining = sizeof(sentence);
1149 int length = 0;
1150 int utcYear = pTm->tm_year % 100; // 2 digit year
1151 int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero
1152 int utcDay = pTm->tm_mday;
1153 int utcHours = pTm->tm_hour;
1154 int utcMinutes = pTm->tm_min;
1155 int utcSeconds = pTm->tm_sec;
1156 int utcMSeconds = (location.gpsLocation.timestamp)%1000;
1157 int datum_type = loc_get_datum_type();
1158 LocEcef ecef_w84;
1159 LocEcef ecef_p90;
1160 LocLla lla_w84;
1161 LocLla lla_p90;
1162 LocLla ref_lla;
1163 LocLla local_lla;
1164
1165 if (inLsTransition) {
1166 // During leap second transition, we need to display the extra
1167 // leap second of hour, minute, second as (23:59:60)
1168 utcHours = 23;
1169 utcMinutes = 59;
1170 utcSeconds = 60;
1171 // As UTC timestamp is freezing during leap second transition,
1172 // retrieve milli-seconds portion from GPS timestamp.
1173 utcMSeconds = locationExtended.gpsTime.gpsTimeOfWeekMs % 1000;
1174 }
1175
1176 loc_sv_cache_info sv_cache_info = {};
1177
1178 if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
1179 sv_cache_info.gps_used_mask =
1180 locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask;
1181 sv_cache_info.glo_used_mask =
1182 locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask;
1183 sv_cache_info.gal_used_mask =
1184 locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask;
1185 sv_cache_info.bds_used_mask =
1186 locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
1187 sv_cache_info.qzss_used_mask =
1188 locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask;
1189 sv_cache_info.navic_used_mask =
1190 locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask;
1191 }
1192
1193 if (generate_nmea) {
1194 char talker[3] = {'G', 'P', '\0'};
1195 char modeIndicator[7] = {0};
1196 uint32_t svUsedCount = 0;
1197 uint32_t count = 0;
1198 loc_nmea_sv_meta sv_meta;
1199 // -------------------
1200 // ---$GPGSA/$GNGSA---
1201 // -------------------
1202
1203 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
1204 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
1205 GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr);
1206 if (count > 0)
1207 {
1208 svUsedCount += count;
1209 talker[0] = sv_meta.talker[0];
1210 talker[1] = sv_meta.talker[1];
1211 }
1212
1213 // -------------------
1214 // ---$GLGSA/$GNGSA---
1215 // -------------------
1216
1217 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
1218 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
1219 GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr);
1220 if (count > 0)
1221 {
1222 svUsedCount += count;
1223 talker[0] = sv_meta.talker[0];
1224 talker[1] = sv_meta.talker[1];
1225 }
1226
1227 // -------------------
1228 // ---$GAGSA/$GNGSA---
1229 // -------------------
1230
1231 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
1232 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
1233 GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr);
1234 if (count > 0)
1235 {
1236 svUsedCount += count;
1237 talker[0] = sv_meta.talker[0];
1238 talker[1] = sv_meta.talker[1];
1239 }
1240
1241 // ----------------------------
1242 // ---$GBGSA/$GNGSA (BEIDOU)---
1243 // ----------------------------
1244 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
1245 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
1246 GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr);
1247 if (count > 0)
1248 {
1249 svUsedCount += count;
1250 talker[0] = sv_meta.talker[0];
1251 talker[1] = sv_meta.talker[1];
1252 }
1253
1254 // --------------------------
1255 // ---$GQGSA/$GNGSA (QZSS)---
1256 // --------------------------
1257
1258 count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
1259 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
1260 GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr);
1261 if (count > 0)
1262 {
1263 svUsedCount += count;
1264 talker[0] = sv_meta.talker[0];
1265 talker[1] = sv_meta.talker[1];
1266 }
1267
1268 char ggaGpsQuality = '0';
1269 char rmcModeIndicator = 'N';
1270 char vtgModeIndicator = 'N';
1271 loc_nmea_get_fix_quality(location, locationExtended,
1272 ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
1273
1274 // -------------------
1275 // ------$--VTG-------
1276 // -------------------
1277
1278 pMarker = sentence;
1279 lengthRemaining = sizeof(sentence);
1280
1281 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
1282 {
1283 float magTrack = location.gpsLocation.bearing;
1284 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
1285 {
1286 float magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation;
1287 if (magTrack < 0.0)
1288 magTrack += 360.0;
1289 else if (magTrack > 360.0)
1290 magTrack -= 360.0;
1291 }
1292
1293 length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,", talker, location.gpsLocation.bearing, magTrack);
1294 }
1295 else
1296 {
1297 length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker);
1298 }
1299
1300 if (length < 0 || length >= lengthRemaining)
1301 {
1302 LOC_LOGE("NMEA Error in string formatting");
1303 return;
1304 }
1305 pMarker += length;
1306 lengthRemaining -= length;
1307
1308 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
1309 {
1310 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
1311 float speedKmPerHour = location.gpsLocation.speed * 3.6;
1312
1313 length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,", speedKnots, speedKmPerHour);
1314 }
1315 else
1316 {
1317 length = snprintf(pMarker, lengthRemaining, ",N,,K,");
1318 }
1319
1320 if (length < 0 || length >= lengthRemaining)
1321 {
1322 LOC_LOGE("NMEA Error in string formatting");
1323 return;
1324 }
1325 pMarker += length;
1326 lengthRemaining -= length;
1327
1328 length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator);
1329
1330 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1331 nmeaArraystr.push_back(sentence);
1332
1333 memset(&ecef_w84, 0, sizeof(ecef_w84));
1334 memset(&ecef_p90, 0, sizeof(ecef_p90));
1335 memset(&lla_w84, 0, sizeof(lla_w84));
1336 memset(&lla_p90, 0, sizeof(lla_p90));
1337 memset(&ref_lla, 0, sizeof(ref_lla));
1338 memset(&local_lla, 0, sizeof(local_lla));
1339 lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI;
1340 lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI;
1341 lla_w84.alt = location.gpsLocation.altitude;
1342
1343 convert_Lla_to_Ecef(lla_w84, ecef_w84);
1344 convert_WGS84_to_PZ90(ecef_w84, ecef_p90);
1345 convert_Ecef_to_Lla(ecef_p90, lla_p90);
1346
1347 switch (datum_type) {
1348 case LOC_GNSS_DATUM_WGS84:
1349 ref_lla.lat = location.gpsLocation.latitude;
1350 ref_lla.lon = location.gpsLocation.longitude;
1351 ref_lla.alt = location.gpsLocation.altitude;
1352 local_lla.lat = lla_p90.lat / M_PI * 180.0;
1353 local_lla.lon = lla_p90.lon / M_PI * 180.0;
1354 local_lla.alt = lla_p90.alt;
1355 break;
1356 case LOC_GNSS_DATUM_PZ90:
1357 ref_lla.lat = lla_p90.lat / M_PI * 180.0;
1358 ref_lla.lon = lla_p90.lon / M_PI * 180.0;
1359 ref_lla.alt = lla_p90.alt;
1360 local_lla.lat = location.gpsLocation.latitude;
1361 local_lla.lon = location.gpsLocation.longitude;
1362 local_lla.alt = location.gpsLocation.altitude;
1363 break;
1364 default:
1365 break;
1366 }
1367
1368 // -------------------
1369 // ------$--DTM-------
1370 // -------------------
1371 loc_nmea_generate_DTM(ref_lla, local_lla, talker, sentence_DTM, sizeof(sentence_DTM));
1372
1373 // -------------------
1374 // ------$--RMC-------
1375 // -------------------
1376
1377 pMarker = sentence_RMC;
1378 lengthRemaining = sizeof(sentence_RMC);
1379
1380 length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A," ,
1381 talker, utcHours, utcMinutes, utcSeconds,utcMSeconds/10);
1382
1383 if (length < 0 || length >= lengthRemaining)
1384 {
1385 LOC_LOGE("NMEA Error in string formatting");
1386 return;
1387 }
1388 pMarker += length;
1389 lengthRemaining -= length;
1390
1391 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
1392 {
1393 double latitude = ref_lla.lat;
1394 double longitude = ref_lla.lon;
1395 char latHemisphere;
1396 char lonHemisphere;
1397 double latMinutes;
1398 double lonMinutes;
1399
1400 if (latitude > 0)
1401 {
1402 latHemisphere = 'N';
1403 }
1404 else
1405 {
1406 latHemisphere = 'S';
1407 latitude *= -1.0;
1408 }
1409
1410 if (longitude < 0)
1411 {
1412 lonHemisphere = 'W';
1413 longitude *= -1.0;
1414 }
1415 else
1416 {
1417 lonHemisphere = 'E';
1418 }
1419
1420 latMinutes = fmod(latitude * 60.0 , 60.0);
1421 lonMinutes = fmod(longitude * 60.0 , 60.0);
1422
1423 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
1424 (uint8_t)floor(latitude), latMinutes, latHemisphere,
1425 (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
1426 }
1427 else
1428 {
1429 length = snprintf(pMarker, lengthRemaining,",,,,");
1430 }
1431
1432 if (length < 0 || length >= lengthRemaining)
1433 {
1434 LOC_LOGE("NMEA Error in string formatting");
1435 return;
1436 }
1437 pMarker += length;
1438 lengthRemaining -= length;
1439
1440 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
1441 {
1442 float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
1443 length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
1444 }
1445 else
1446 {
1447 length = snprintf(pMarker, lengthRemaining, ",");
1448 }
1449
1450 if (length < 0 || length >= lengthRemaining)
1451 {
1452 LOC_LOGE("NMEA Error in string formatting");
1453 return;
1454 }
1455 pMarker += length;
1456 lengthRemaining -= length;
1457
1458 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
1459 {
1460 length = snprintf(pMarker, lengthRemaining, "%.1lf,", location.gpsLocation.bearing);
1461 }
1462 else
1463 {
1464 length = snprintf(pMarker, lengthRemaining, ",");
1465 }
1466
1467 if (length < 0 || length >= lengthRemaining)
1468 {
1469 LOC_LOGE("NMEA Error in string formatting");
1470 return;
1471 }
1472 pMarker += length;
1473 lengthRemaining -= length;
1474
1475 length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
1476 utcDay, utcMonth, utcYear);
1477
1478 if (length < 0 || length >= lengthRemaining)
1479 {
1480 LOC_LOGE("NMEA Error in string formatting");
1481 return;
1482 }
1483 pMarker += length;
1484 lengthRemaining -= length;
1485
1486 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
1487 {
1488 float magneticVariation = locationExtended.magneticDeviation;
1489 char direction;
1490 if (magneticVariation < 0.0)
1491 {
1492 direction = 'W';
1493 magneticVariation *= -1.0;
1494 }
1495 else
1496 {
1497 direction = 'E';
1498 }
1499
1500 length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
1501 magneticVariation, direction);
1502 }
1503 else
1504 {
1505 length = snprintf(pMarker, lengthRemaining, ",,");
1506 }
1507
1508 if (length < 0 || length >= lengthRemaining)
1509 {
1510 LOC_LOGE("NMEA Error in string formatting");
1511 return;
1512 }
1513 pMarker += length;
1514 lengthRemaining -= length;
1515
1516 length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator);
1517 pMarker += length;
1518 lengthRemaining -= length;
1519
1520 // hardcode Navigation Status field to 'V'
1521 length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
1522 pMarker += length;
1523 lengthRemaining -= length;
1524
1525 length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC));
1526
1527 // -------------------
1528 // ------$--GNS-------
1529 // -------------------
1530
1531 pMarker = sentence_GNS;
1532 lengthRemaining = sizeof(sentence_GNS);
1533
1534 length = snprintf(pMarker, lengthRemaining, "$%sGNS,%02d%02d%02d.%02d," ,
1535 talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
1536
1537 if (length < 0 || length >= lengthRemaining)
1538 {
1539 LOC_LOGE("NMEA Error in string formatting");
1540 return;
1541 }
1542 pMarker += length;
1543 lengthRemaining -= length;
1544
1545 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
1546 {
1547 double latitude = ref_lla.lat;
1548 double longitude = ref_lla.lon;
1549 char latHemisphere;
1550 char lonHemisphere;
1551 double latMinutes;
1552 double lonMinutes;
1553
1554 if (latitude > 0)
1555 {
1556 latHemisphere = 'N';
1557 }
1558 else
1559 {
1560 latHemisphere = 'S';
1561 latitude *= -1.0;
1562 }
1563
1564 if (longitude < 0)
1565 {
1566 lonHemisphere = 'W';
1567 longitude *= -1.0;
1568 }
1569 else
1570 {
1571 lonHemisphere = 'E';
1572 }
1573
1574 latMinutes = fmod(latitude * 60.0 , 60.0);
1575 lonMinutes = fmod(longitude * 60.0 , 60.0);
1576
1577 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
1578 (uint8_t)floor(latitude), latMinutes, latHemisphere,
1579 (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
1580 }
1581 else
1582 {
1583 length = snprintf(pMarker, lengthRemaining,",,,,");
1584 }
1585
1586 if (length < 0 || length >= lengthRemaining)
1587 {
1588 LOC_LOGE("NMEA Error in string formatting");
1589 return;
1590 }
1591 pMarker += length;
1592 lengthRemaining -= length;
1593
1594 if(!(sv_cache_info.gps_used_mask ? 1 : 0))
1595 modeIndicator[0] = 'N';
1596 else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)
1597 modeIndicator[0] = 'D';
1598 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1599 modeIndicator[0] = 'E';
1600 else
1601 modeIndicator[0] = 'A';
1602 if(!(sv_cache_info.glo_used_mask ? 1 : 0))
1603 modeIndicator[1] = 'N';
1604 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1605 modeIndicator[1] = 'E';
1606 else
1607 modeIndicator[1] = 'A';
1608 if(!(sv_cache_info.gal_used_mask ? 1 : 0))
1609 modeIndicator[2] = 'N';
1610 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1611 modeIndicator[2] = 'E';
1612 else
1613 modeIndicator[2] = 'A';
1614 if(!(sv_cache_info.bds_used_mask ? 1 : 0))
1615 modeIndicator[3] = 'N';
1616 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1617 modeIndicator[3] = 'E';
1618 else
1619 modeIndicator[3] = 'A';
1620 if(!(sv_cache_info.qzss_used_mask ? 1 : 0))
1621 modeIndicator[4] = 'N';
1622 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1623 modeIndicator[4] = 'E';
1624 else
1625 modeIndicator[4] = 'A';
1626 if(!(sv_cache_info.navic_used_mask ? 1 : 0))
1627 modeIndicator[5] = 'N';
1628 else if (LOC_POS_TECH_MASK_SENSORS == locationExtended.tech_mask)
1629 modeIndicator[5] = 'E';
1630 else
1631 modeIndicator[5] = 'A';
1632 modeIndicator[6] = '\0';
1633 for(int index = 5; index > 0 && 'N' == modeIndicator[index]; index--) {
1634 modeIndicator[index] = '\0';
1635 }
1636 length = snprintf(pMarker, lengthRemaining,"%s,", modeIndicator);
1637
1638 pMarker += length;
1639 lengthRemaining -= length;
1640
1641 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
1642 length = snprintf(pMarker, lengthRemaining, "%02d,%.1f,",
1643 svUsedCount, locationExtended.hdop);
1644 }
1645 else { // no hdop
1646 length = snprintf(pMarker, lengthRemaining, "%02d,,",
1647 svUsedCount);
1648 }
1649
1650 if (length < 0 || length >= lengthRemaining)
1651 {
1652 LOC_LOGE("NMEA Error in string formatting");
1653 return;
1654 }
1655 pMarker += length;
1656 lengthRemaining -= length;
1657
1658 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
1659 {
1660 length = snprintf(pMarker, lengthRemaining, "%.1lf,",
1661 locationExtended.altitudeMeanSeaLevel);
1662 }
1663 else
1664 {
1665 length = snprintf(pMarker, lengthRemaining,",");
1666 }
1667
1668 if (length < 0 || length >= lengthRemaining)
1669 {
1670 LOC_LOGE("NMEA Error in string formatting");
1671 return;
1672 }
1673 pMarker += length;
1674 lengthRemaining -= length;
1675
1676 if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
1677 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
1678 {
1679 length = snprintf(pMarker, lengthRemaining, "%.1lf,,",
1680 ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
1681 }
1682 else
1683 {
1684 length = snprintf(pMarker, lengthRemaining,",,");
1685 }
1686
1687 pMarker += length;
1688 lengthRemaining -= length;
1689
1690 // hardcode Navigation Status field to 'V'
1691 length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
1692 pMarker += length;
1693 lengthRemaining -= length;
1694
1695 length = loc_nmea_put_checksum(sentence_GNS, sizeof(sentence_GNS));
1696
1697
1698 // -------------------
1699 // ------$--GGA-------
1700 // -------------------
1701
1702 pMarker = sentence_GGA;
1703 lengthRemaining = sizeof(sentence_GGA);
1704
1705 length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d," ,
1706 talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
1707
1708 if (length < 0 || length >= lengthRemaining)
1709 {
1710 LOC_LOGE("NMEA Error in string formatting");
1711 return;
1712 }
1713 pMarker += length;
1714 lengthRemaining -= length;
1715
1716 if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
1717 {
1718 double latitude = ref_lla.lat;
1719 double longitude = ref_lla.lon;
1720 char latHemisphere;
1721 char lonHemisphere;
1722 double latMinutes;
1723 double lonMinutes;
1724
1725 if (latitude > 0)
1726 {
1727 latHemisphere = 'N';
1728 }
1729 else
1730 {
1731 latHemisphere = 'S';
1732 latitude *= -1.0;
1733 }
1734
1735 if (longitude < 0)
1736 {
1737 lonHemisphere = 'W';
1738 longitude *= -1.0;
1739 }
1740 else
1741 {
1742 lonHemisphere = 'E';
1743 }
1744
1745 latMinutes = fmod(latitude * 60.0 , 60.0);
1746 lonMinutes = fmod(longitude * 60.0 , 60.0);
1747
1748 length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
1749 (uint8_t)floor(latitude), latMinutes, latHemisphere,
1750 (uint8_t)floor(longitude),lonMinutes, lonHemisphere);
1751 }
1752 else
1753 {
1754 length = snprintf(pMarker, lengthRemaining,",,,,");
1755 }
1756
1757 if (length < 0 || length >= lengthRemaining)
1758 {
1759 LOC_LOGE("NMEA Error in string formatting");
1760 return;
1761 }
1762 pMarker += length;
1763 lengthRemaining -= length;
1764
1765 // Number of satellites in use, 00-12
1766 if (svUsedCount > MAX_SATELLITES_IN_USE)
1767 svUsedCount = MAX_SATELLITES_IN_USE;
1768 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
1769 {
1770 length = snprintf(pMarker, lengthRemaining, "%c,%02d,%.1f,",
1771 ggaGpsQuality, svUsedCount, locationExtended.hdop);
1772 }
1773 else
1774 { // no hdop
1775 length = snprintf(pMarker, lengthRemaining, "%c,%02d,,",
1776 ggaGpsQuality, svUsedCount);
1777 }
1778
1779 if (length < 0 || length >= lengthRemaining)
1780 {
1781 LOC_LOGE("NMEA Error in string formatting");
1782 return;
1783 }
1784 pMarker += length;
1785 lengthRemaining -= length;
1786
1787 if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
1788 {
1789 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
1790 locationExtended.altitudeMeanSeaLevel);
1791 }
1792 else
1793 {
1794 length = snprintf(pMarker, lengthRemaining,",,");
1795 }
1796
1797 if (length < 0 || length >= lengthRemaining)
1798 {
1799 LOC_LOGE("NMEA Error in string formatting");
1800 return;
1801 }
1802 pMarker += length;
1803 lengthRemaining -= length;
1804
1805 if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
1806 (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
1807 {
1808 length = snprintf(pMarker, lengthRemaining, "%.1lf,M,,",
1809 ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
1810 }
1811 else
1812 {
1813 length = snprintf(pMarker, lengthRemaining,",,,");
1814 }
1815
1816 length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA));
1817
1818 // ------$--DTM-------
1819 nmeaArraystr.push_back(sentence_DTM);
1820 // ------$--RMC-------
1821 nmeaArraystr.push_back(sentence_RMC);
1822 if(LOC_GNSS_DATUM_PZ90 == datum_type) {
1823 // ------$--DTM-------
1824 nmeaArraystr.push_back(sentence_DTM);
1825 }
1826 // ------$--GNS-------
1827 nmeaArraystr.push_back(sentence_GNS);
1828 if(LOC_GNSS_DATUM_PZ90 == datum_type) {
1829 // ------$--DTM-------
1830 nmeaArraystr.push_back(sentence_DTM);
1831 }
1832 // ------$--GGA-------
1833 nmeaArraystr.push_back(sentence_GGA);
1834
1835 }
1836 //Send blank NMEA reports for non-final fixes
1837 else {
1838 strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
1839 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1840 nmeaArraystr.push_back(sentence);
1841
1842 strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
1843 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1844 nmeaArraystr.push_back(sentence);
1845
1846 strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence));
1847 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1848 nmeaArraystr.push_back(sentence);
1849
1850 strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence));
1851 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1852 nmeaArraystr.push_back(sentence);
1853
1854 strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence));
1855 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1856 nmeaArraystr.push_back(sentence);
1857
1858 strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
1859 length = loc_nmea_put_checksum(sentence, sizeof(sentence));
1860 nmeaArraystr.push_back(sentence);
1861 }
1862
1863 EXIT_LOG(%d, 0);
1864 }
1865
1866
1867
1868 /*===========================================================================
1869 FUNCTION loc_nmea_generate_sv
1870
1871 DESCRIPTION
1872 Generate NMEA sentences generated based on sv report
1873
1874 DEPENDENCIES
1875 NONE
1876
1877 RETURN VALUE
1878 0
1879
1880 SIDE EFFECTS
1881 N/A
1882
1883 ===========================================================================*/
loc_nmea_generate_sv(const GnssSvNotification & svNotify,std::vector<std::string> & nmeaArraystr)1884 void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
1885 std::vector<std::string> &nmeaArraystr)
1886 {
1887 ENTRY_LOG();
1888
1889 char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
1890 int svCount = svNotify.count;
1891 int svNumber = 1;
1892 loc_sv_cache_info sv_cache_info = {};
1893
1894 //Count GPS SVs for saparating GPS from GLONASS and throw others
1895 for(svNumber=1; svNumber <= svCount; svNumber++) {
1896 if (GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svNumber - 1].type)
1897 {
1898 // cache the used in fix mask, as it will be needed to send $GPGSA
1899 // during the position report
1900 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1901 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1902 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1903 {
1904 sv_cache_info.gps_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1905 }
1906 if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
1907 sv_cache_info.gps_l5_count++;
1908 } else {
1909 // GNSS_SIGNAL_GPS_L1CA or default
1910 // If no signal type in report, it means default L1
1911 sv_cache_info.gps_l1_count++;
1912 }
1913 }
1914 else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type)
1915 {
1916 // cache the used in fix mask, as it will be needed to send $GNGSA
1917 // during the position report
1918 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1919 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1920 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1921 {
1922 sv_cache_info.glo_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1923 }
1924 if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
1925 sv_cache_info.glo_g2_count++;
1926 } else {
1927 // GNSS_SIGNAL_GLONASS_G1 or default
1928 // If no signal type in report, it means default G1
1929 sv_cache_info.glo_g1_count++;
1930 }
1931 }
1932 else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svNumber - 1].type)
1933 {
1934 // cache the used in fix mask, as it will be needed to send $GAGSA
1935 // during the position report
1936 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1937 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1938 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1939 {
1940 sv_cache_info.gal_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1941 }
1942 if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
1943 sv_cache_info.gal_e5_count++;
1944 } else {
1945 // GNSS_SIGNAL_GALILEO_E1 or default
1946 // If no signal type in report, it means default E1
1947 sv_cache_info.gal_e1_count++;
1948 }
1949 }
1950 else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svNumber - 1].type)
1951 {
1952 // cache the used in fix mask, as it will be needed to send $PQGSA
1953 // during the position report
1954 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1955 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1956 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1957 {
1958 sv_cache_info.qzss_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1959 }
1960 if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask) {
1961 sv_cache_info.qzss_l5_count++;
1962 } else {
1963 // GNSS_SIGNAL_QZSS_L1CA or default
1964 // If no signal type in report, it means default L1
1965 sv_cache_info.qzss_l1_count++;
1966 }
1967 }
1968 else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svNumber - 1].type)
1969 {
1970 // cache the used in fix mask, as it will be needed to send $PQGSA
1971 // during the position report
1972 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1973 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1974 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1975 {
1976 sv_cache_info.bds_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1977 }
1978 if(GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svNumber - 1].gnssSignalTypeMask){
1979 sv_cache_info.bds_b2_count++;
1980 } else {
1981 // GNSS_SIGNAL_BEIDOU_B1I or default
1982 // If no signal type in report, it means default B1I
1983 sv_cache_info.bds_b1_count++;
1984 }
1985 }
1986 else if (GNSS_SV_TYPE_NAVIC == svNotify.gnssSvs[svNumber - 1].type)
1987 {
1988 // cache the used in fix mask, as it will be needed to send $PQGSA
1989 // during the position report
1990 if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
1991 (svNotify.gnssSvs[svNumber - 1].gnssSvOptionsMask &
1992 GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
1993 {
1994 sv_cache_info.navic_used_mask |= (1 << (svNotify.gnssSvs[svNumber - 1].svId - 1));
1995 }
1996 // GNSS_SIGNAL_NAVIC_L5 is the only signal type for NAVIC
1997 sv_cache_info.navic_l5_count++;
1998 }
1999 }
2000
2001 loc_nmea_sv_meta sv_meta;
2002 // ---------------------
2003 // ------$GPGSV:L1CA----
2004 // ---------------------
2005
2006 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2007 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
2008 GNSS_SIGNAL_GPS_L1CA, false), nmeaArraystr);
2009
2010 // ---------------------
2011 // ------$GPGSV:L5------
2012 // ---------------------
2013
2014 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2015 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
2016 GNSS_SIGNAL_GPS_L5, false), nmeaArraystr);
2017 // ---------------------
2018 // ------$GLGSV:G1------
2019 // ---------------------
2020
2021 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2022 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
2023 GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr);
2024
2025 // ---------------------
2026 // ------$GLGSV:G2------
2027 // ---------------------
2028
2029 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2030 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
2031 GNSS_SIGNAL_GLONASS_G2, false), nmeaArraystr);
2032
2033 // ---------------------
2034 // ------$GAGSV:E1------
2035 // ---------------------
2036
2037 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2038 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
2039 GNSS_SIGNAL_GALILEO_E1, false), nmeaArraystr);
2040
2041 // -------------------------
2042 // ------$GAGSV:E5A---------
2043 // -------------------------
2044 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2045 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
2046 GNSS_SIGNAL_GALILEO_E5A, false), nmeaArraystr);
2047
2048 // -----------------------------
2049 // ------$PQGSV (QZSS):L1CA-----
2050 // -----------------------------
2051
2052 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2053 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
2054 GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr);
2055
2056 // -----------------------------
2057 // ------$PQGSV (QZSS):L5-------
2058 // -----------------------------
2059
2060 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2061 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
2062 GNSS_SIGNAL_QZSS_L5, false), nmeaArraystr);
2063 // -----------------------------
2064 // ------$PQGSV (BEIDOU:B1I)----
2065 // -----------------------------
2066
2067 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2068 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
2069 GNSS_SIGNAL_BEIDOU_B1I,false), nmeaArraystr);
2070
2071 // -----------------------------
2072 // ------$PQGSV (BEIDOU:B2AI)---
2073 // -----------------------------
2074
2075 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2076 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
2077 GNSS_SIGNAL_BEIDOU_B2AI,false), nmeaArraystr);
2078
2079 // -----------------------------
2080 // ------$GIGSV (NAVIC:L5)------
2081 // -----------------------------
2082
2083 loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
2084 loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC,
2085 GNSS_SIGNAL_NAVIC_L5,false), nmeaArraystr);
2086
2087 EXIT_LOG(%d, 0);
2088 }
2089