forked from SlightlyLoony/gpsctl
-
Notifications
You must be signed in to change notification settings - Fork 4
/
ublox.c
1292 lines (1061 loc) · 55.2 KB
/
ublox.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
//
// Created by Tom Dilatush on 10/20/17.
//
// these defines allow compiling on both an OS X development machine and the target Raspberry Pi. If different
// development environments or target machines are needed, these will likely need to be tweaked.
#define _XOPEN_SOURCE 700
#define _DARWIN_C_SOURCE
#define _POSIX_C_SOURCE 199309L
#include "ublox.h"
// for best time pulse:
// - disable SBAS, IMES, and QZSS (UBX-CFG-GNSS)
// - stationary mode (UBX-CFG-NAV5)
// - calibrate antenna delay (UBX-CFG-TP5)
// - set measurement rate (UBX-CFG-RATE) and time pulse frequency (UBX-CFG-TP5) to 1Hz
// - set UTC variant to GPS (UBX-CFG-NAV5)
#define UBX_ACK 0x05
#define UBX_ACK_ACK 0x01
#define UBX_ACK_NAK 0x00
#define UBX_CFG 0x06
#define UBX_CFG_PRT 0x00
#define UBX_CFG_ANT 0x13
#define UBX_CFG_NAV5 0x24
#define UBX_CFG_GNSS 0x3E
#define UBX_CFG_TP5 0x31
#define UBX_CFG_PMS 0x86
#define UBX_CFG_RATE 0x08
#define UBX_CFG_RST 0x04
#define UBX_CFG_CFG 0x09
#define UBX_CFG_NMEA 0x17
#define UBX_CFG_MSG 0x01
#define UBX_MON 0x0A
#define UBX_MON_VER 0x04
#define UBX_NAV 0x01
#define UBX_NAV_PVT 0x07
#define UBX_NAV_SAT 0x35
#define NAV_PVT_MAX_MS 2000
#define NAV_SAT_MAX_MS 2000
#define CFG_NAV5_MAX_MS 2000
#define MON_VER_MAX_MS 2000
#define MON_VER_SYNC_MAX_MS 1200
#define CFG_GNSS_MAX_MS 1000
#define CFG_ANT_MAX_MS 1000
#define CFG_PRT_MAX_MS 1000
#define CFG_NMEA_MAX_MS 1000
#define CFG_TP5_MAX_MS 1000
#define CFG_RATE_MAX_MS 1000
#define CFG_PMS_MAX_MS 1000
static ubxType ut_ACK_NAK = { UBX_ACK, UBX_ACK_NAK };
static ubxType ut_CFG_PRT = { UBX_CFG, UBX_CFG_PRT };
static ubxType ut_NAV_PVT = { UBX_NAV, UBX_NAV_PVT };
static ubxType ut_CFG_CFG = { UBX_CFG, UBX_CFG_CFG };
static ubxType ut_CFG_MSG = { UBX_CFG, UBX_CFG_MSG };
typedef struct ubxMsg {
ubxType type;
slBuffer *body;
ubxChecksum checksum;
} ubxMsg;
static void updateUbxChecksum(byte b, ubxChecksum *checksum) {
checksum->ck_a += b;
checksum->ck_b += checksum->ck_a;
}
// Calculate and return the Fletcher checksum for the given UBX message, without modifying the message at all.
static ubxChecksum calcFletcherChecksum(const ubxMsg msg) {
ubxChecksum result = {0, 0};
// first we get the class and id...
updateUbxChecksum(msg.type.class, &result);
updateUbxChecksum(msg.type.id, &result);
// then the length, little-endian...
updateUbxChecksum((byte) (size_slBuffer(msg.body)), &result);
updateUbxChecksum((byte) (size_slBuffer(msg.body) >> 8), &result);
// and finally the body itself...
for (int i = 0; i < size_slBuffer(msg.body); i++) {
updateUbxChecksum(buffer_slBuffer(msg.body)[i], &result);
}
return result;
}
ubxMsg createUbxMsg(ubxType type, slBuffer *body) {
ubxMsg result;
result.type = type;
result.body = body;
result.checksum = calcFletcherChecksum(result);
return result;
}
// Returns if all the requested bytes were successfully read.
static slReturn readUbxBytes(int fdPort, byte *buffer, int *count, size_t max, int max_ms, long long startTime, long long charWaitMs) {
*count = 0;
while (((currentTimeMs() - startTime) < max_ms) && (*count < max)) {
slReturn rscResp = readSerialChar(fdPort, charWaitMs);
if (isErrorReturn(rscResp))
makeErrorMsgReturn(ERR_CAUSE(rscResp), "couldn't read serial character");
if (isWarningReturn(rscResp))
makeErrorMsgReturn(ERR_ROOT, "timed out while trying to read UBX message");
int c = getReturnInfoChar(rscResp);
if (c >= 0) {
buffer[*count] = (byte) c;
(*count)++;
}
}
return (*count >= max) ? makeOkReturn() : makeErrorMsgReturn(ERR_ROOT, "insufficient bytes read");
}
// Waits up to one second for a UBX message to be received.
// The body (an slBuffer) is only allocated if returns Ok.
#define UBX_SYNC1 (0xB5)
#define UBX_SYNC2 (0x62)
static slReturn rcvUbxMsg(int fdPort, ubxMsg* msg, int max_ms) {
ubxMsg nomsg = { {0, 0}, NULL, {0, 0}};
*msg = nomsg;
long long startTime = currentTimeMs();
while (((currentTimeMs() - startTime) < max_ms)) {
// first we synchronize on the 0xB5, 0x62 byte pair that starts every UBX message...
int count = 0;
while (((currentTimeMs() - startTime) < max_ms) && (count < 2)) {
slReturn rscResp = readSerialChar(fdPort, startTime + max_ms - currentTimeMs());
if (isErrorReturn(rscResp))
return makeErrorMsgReturn(ERR_CAUSE(rscResp), "couldn't read serial character");
if (isWarningReturn(rscResp))
break; // if we timed out...
int c= getReturnInfoChar(rscResp);
if (c >= 0) {
if ((count == 0) && (c == UBX_SYNC1)) {
count++;
continue;
}
if ((count == 1) && (c == UBX_SYNC2)) {
count++;
continue;
}
count = 0;
}
}
if (count < 2) continue;
// we're synchronized, so it's time to get the message type...
byte buff[2];
slReturn rubResp = readUbxBytes(fdPort, buff, &count, 2, max_ms, startTime, startTime + max_ms - currentTimeMs());
if (isErrorReturn(rubResp))
return makeErrorMsgReturn(ERR_CAUSE(rubResp), "couldn't read serial characters while receiving UBX message type");
if (count < 2) continue;
ubxType type = {buff[0], buff[1]};
msg->type = type;
// get the body length, little-endian...
rubResp = readUbxBytes(fdPort, buff, &count, 2, max_ms, startTime, startTime + max_ms - currentTimeMs());
if (isErrorReturn(rubResp))
return makeErrorMsgReturn(ERR_CAUSE(rubResp), "couldn't read serial characters while receiving UBX message body length");
if (count < 2) continue;
size_t length = buff[0] | (buff[1] << 8);
// now get the body...
msg->body = create_slBuffer(length, LittleEndian);
rubResp = readUbxBytes(fdPort, buffer_slBuffer(msg->body), &count, length, max_ms, startTime, startTime + max_ms - currentTimeMs());
if (isErrorReturn(rubResp))
return makeErrorFmtMsgReturn(ERR_CAUSE(rubResp),
"couldn't read serial characters while receiving UBX message body (0x%02x/0x%02x, %d bytes)",
type.class, type.id, length);
msg->checksum = calcFletcherChecksum(*msg); // this calculates what the checksum SHOULD be...
if (count >= length) {
// finally, get the checksum...
rubResp = readUbxBytes(fdPort, buff, &count, 2, max_ms, startTime, startTime + max_ms - currentTimeMs());
if (isErrorReturn(rubResp))
return makeErrorMsgReturn(ERR_CAUSE(rubResp), "couldn't read serial characters while receiving UBX message checksum");
if (count >= 2) {
if ((buff[0] == msg->checksum.ck_a) && (buff[1] == msg->checksum.ck_b))
return makeOkReturn();
else {
free(msg->body);
return makeErrorMsgReturn(ERR_ROOT, "UBX received message with checksum error");
}
}
}
free(msg->body);
}
return makeErrorMsgReturn(ERR_ROOT, "timed out while waiting to receive UBX message");
}
// Waits for an ACK/NAK message.
#define ACK_WAIT_MS 1500
static slReturn ubxAckWait(int fdPort) {
ubxMsg ackMsg;
slReturn result = rcvUbxMsg(fdPort, &ackMsg, ACK_WAIT_MS);
if (isErrorReturn(result)) return makeErrorMsgReturn(ERR_CAUSE(result), "Problem receiving ACK message");
free(ackMsg.body);
if (ackMsg.type.class != UBX_ACK)
return makeErrorFmtMsgReturn(ERR_ROOT, "Unexpected message received while waiting for ACK: %02x/%02x", ackMsg.type.class, ackMsg.type.id);
else
if (ackMsg.type.id == UBX_ACK_ACK)
return makeOkReturn();
else
return makeErrorMsgReturn(ERR_ROOT, "Received a NAK");
}
// Transmits the given message to the UBX GPS, waiting for completion.
static slReturn sendUbxMsg(int fdPort, ubxMsg msg) {
// first we send out our sync bytes, message type, and body length...
byte buff[6] = { UBX_SYNC1,
UBX_SYNC2,
msg.type.class,
msg.type.id,
(byte) size_slBuffer(msg.body),
(byte)(size_slBuffer(msg.body) >> 8)
};
if (sizeof(buff) != write(fdPort, buff, sizeof(buff)))
return makeErrorFmtMsgReturn(ERR_ROOT, "error writing UBX message header: %s", strerror(errno));
// then we send out the body itself...
if (size_slBuffer(msg.body) != write(fdPort, buffer_slBuffer(msg.body), size_slBuffer(msg.body)))
return makeErrorFmtMsgReturn(ERR_ROOT, "error writing UBX message body: %s", strerror(errno));
// and finally, out goes the checksum...
byte buffChk[2] = {msg.checksum.ck_a, msg.checksum.ck_b};
if (sizeof(buffChk) != write(fdPort, buffChk, sizeof(buffChk)))
return makeErrorFmtMsgReturn(ERR_ROOT, "error writing UBX message checksum: %s", strerror(errno));
// now we wait for everything to actually be sent...
if (tcdrain(fdPort))
return makeErrorFmtMsgReturn(ERR_ROOT, "error waiting for tcdrain: %s", strerror(errno));
return makeOkReturn();
}
// Send the given ubxMsg, then wait for an ACK and return the result.
static slReturn sendUbxAckedMsg(int fdPort, ubxMsg msg) {
slReturn result = sendUbxMsg(fdPort, msg);
if (isErrorReturn(result)) return result;
return ubxAckWait(fdPort);
}
// Returns true if the two given ubxType arguments are equal.
static bool cmpUbxType(ubxType a, ubxType b) {
return (a.class == b.class) && (a.id == b.id);
}
// Polls the GPX with the given message, then waits for the response and possibly an ACK/NAK. The response message
// body is only allocated on an ok response.
static slReturn pollUbx(int fdPort, ubxMsg pollMsg, int max_ms, ubxMsg* answer) {
// send the poll message...
slReturn txResult = sendUbxMsg(fdPort, pollMsg);
free(pollMsg.body);
if (isErrorReturn(txResult)) return makeErrorMsgReturn(ERR_CAUSE(txResult), "Problem sending poll message");
// now get the poll response message...
slReturn resp = rcvUbxMsg(fdPort, answer, max_ms);
if (isErrorReturn(resp)) return makeErrorMsgReturn(ERR_CAUSE(resp), "Problem receiving poll response");
// make sure it's the response type we expected...
if (cmpUbxType(pollMsg.type, answer->type)) {
// certain poll messages (notably configuration) ALSO send an ACK-ACK; for those, get it...
if (pollMsg.type.class == UBX_CFG) {
// get our ack, if our class of messages so requires...
slReturn ackResp = ubxAckWait(fdPort);
if (isErrorReturn(ackResp))
return makeErrorMsgReturn(ERR_CAUSE(ackResp), "Problem receiving expected ACK for poll");
}
return makeOkReturn();
}
else {
// we might have received a NAK (in the case of an invalid poll message being sent),
// or maybe we got complete garbage, so we distinguish between them...
slReturn errResp;
if (cmpUbxType(answer->type, ut_ACK_NAK))
errResp = makeErrorMsgReturn(ERR_ROOT, "received NAK in response to UBX poll");
else
errResp = makeErrorFmtMsgReturn(ERR_ROOT, "received unexpected message type (0x%02x-0x%02x) in response to UBX poll",
answer->type.class, answer->type.id);
free(answer->body);
return errResp;
}
}
// Corrects the raw time as reported by the GPS.
static void correctTime(ubxFix *fix) {
// no matter what, we're gonna add the nanoseconds correction to the seconds...
fix->second += ((int64_t) fix->nanoseconds_correction) * 1e-9;
// if the result is positive, we're done...
if (fix->second >= 0.0) return;
// otherwise, the time was rounded up and now we have to undo that...
fix->second += 60; // fix the seconds wrap-around - note this is wrong for leap-seconds, but how to fix that?
// now fix the minutes...
fix->minute--;
if (fix->minute >= 0) return;
fix->minute += 60;
// then the hours...
fix->hour--;
if (fix->hour >= 0) return;
fix->hour += 24;
// we rounded to the next day - sheesh, fix it...
fix->day--;
if (fix->day >= 1) return;
// oh oh - we rounded up from the preceding month!
// now this starts to get a little complicated - we have to know the days in a month, which means leap years...
fix->month--;
if (fix->month >= 1) {
fix->day = daysInMonth((unsigned) fix->year, (unsigned) fix->month);
return;
}
// sheesh - we even rounded up the year!
fix->year--;
fix->month = 12;
fix->day = daysInMonth((unsigned) fix->year, (unsigned) fix->month);
}
// Resets the U-Blox GPS (hardware reset).
extern slReturn ubxReset(int fdPort, int verbosity) {
// construct our reset message (which won't be acknowledged in any way)...
slBuffer* body = create_slBuffer(4, LittleEndian);
put_uint16_slBuffer(body, 0xffff, 0); // we're doing a cold start...
put_uint8_slBuffer(body, 2, 0); // immediate restart...
ubxType type = { UBX_CFG, UBX_CFG_RST };
ubxMsg msg = createUbxMsg(type, body);
slReturn sumResp = sendUbxMsg(fdPort, msg);
return sumResp;
}
// Query NMEA Message config
extern slReturn isNMEAmsgEnabled(int fdPort, int verbosity, nmeaMSG messageID) {
// poll for the current configuration...
ubxMsg poll = createUbxMsg(ut_CFG_MSG, init_slBuffer(LittleEndian, 0xf0, messageID));
ubxMsg current;
slReturn resp = pollUbx(fdPort, poll, CFG_NMEA_MAX_MS, ¤t);
if (isErrorReturn(resp)) return resp;
bool state = get_uint8_slBuffer(current.body, 3);
return makeOkInfoReturn(bool2info(state));
}
// Enable / disable NMEA Message types
extern slReturn ubxEnableNMEAMsg(int fdPort, int verbosity, nmeaMSG messageID, bool enable) {
// construct the configuration message...
slBuffer* body = create_slBuffer(8, LittleEndian);
put_uint8_slBuffer(body, 0, 0xf0); // Message type NMEA
put_uint8_slBuffer(body, 1, messageID);
put_uint8_slBuffer(body, 2, 0x00); // other ports
put_uint8_slBuffer(body, 3, (enable) ? 1 : 0); // Serial port
put_uint8_slBuffer(body, 4, 0x00); // other ports
put_uint8_slBuffer(body, 5, 0x00); // other ports
put_uint8_slBuffer(body, 6, 0x00); // other ports
put_uint8_slBuffer(body, 7, 0x00); // other ports
ubxMsg nmeaMsg = createUbxMsg(ut_CFG_MSG, body);
// now send it, and wait for our ack...
slReturn nmeaResp = sendUbxAckedMsg(fdPort, nmeaMsg);
if (isErrorReturn(nmeaResp))
return makeErrorMsgReturn(ERR_CAUSE(nmeaResp), "failed to enable /disable NMEA MSG");
free(body);
return makeOkReturn();
}
extern slReturn ubxConfigNMEAVersion(int fdPort, int verbosity, uint8_t nmeaVersion) {
// configure the NMEA version...
// get the NMEA configuration...
ubxType nmeaType = { UBX_CFG, UBX_CFG_NMEA };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(nmeaType, body);
ubxMsg nmeaMsg;
slReturn nmeaResp = pollUbx(fdPort, msg, CFG_NMEA_MAX_MS, &nmeaMsg);
if (isErrorReturn(nmeaResp))
return makeErrorMsgReturn(ERR_CAUSE(nmeaResp), "problem getting NMEA Version information from GPS");
// make the changes we need to make only if we need to...
slBuffer* b = nmeaMsg.body;
if (get_uint8_slBuffer(b, 1) != nmeaVersion) {
put_uint8_slBuffer(b, 1, nmeaVersion);
// now send it back to the GPS...
ubxMsg newnmeaMsg = createUbxMsg(nmeaMsg.type, b);
nmeaResp = sendUbxAckedMsg(fdPort, newnmeaMsg);
if (isErrorReturn(nmeaResp))
return makeErrorMsgReturn(ERR_CAUSE(nmeaResp), "problem sending NMEA Version configuration to GPS");
}
free(body);
free(nmeaMsg.body);
return makeOkReturn();
}
// Configure satellites
extern slReturn ubxConfigSatellites(int fdPort, int verbosity) {
// configure the GNSS for GPS, GLONASS, BeiDou, and Galileo, with no SBAS.
// first read the current configuration...
int minch, maxch, enabled, beidou_enabled, galileo_enabled, glonass_enabled, gps_enabled, nmeaver;
double dnmeaver;
gps_enabled = iniparser_getboolean(gpsctlConf, "gps:enabled", true);
galileo_enabled = iniparser_getboolean(gpsctlConf, "galileo:enabled", true);
// only one of BeiDou and Glonass can be enabled at the same time if GPS or Galileo are enabled
beidou_enabled = iniparser_getboolean(gpsctlConf, "beidou:enabled", false);
glonass_enabled = iniparser_getboolean(gpsctlConf, "glonass:enabled", true);
if (beidou_enabled && glonass_enabled && (gps_enabled || galileo_enabled)) {
if (iniparser_getboolean(gpsctlConf, "gpsctl:prefer beidou to glonass if both enabled", false))
glonass_enabled = false;
else
beidou_enabled = false;
}
ubxType gnssType = { UBX_CFG, UBX_CFG_GNSS };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(gnssType, body);
ubxMsg gnssMsg;
slReturn gnssResp = pollUbx(fdPort, msg, CFG_GNSS_MAX_MS, &gnssMsg);
if (isErrorReturn(gnssResp))
return makeErrorMsgReturn(ERR_CAUSE(gnssResp), "problem getting GNSS information from GPS");
// make the changes we need to make...
uint8_t gnssRecs = get_uint8_slBuffer(gnssMsg.body, 3); // number of configuration blocks...
slBuffer* b = gnssMsg.body;
for (int i = 0; i < gnssRecs; i++) {
size_t o = 4 + 8 * (size_t)i;
gnssID id = (gnssID) get_uint8_slBuffer(b, o + 0);
uint32_t flags;
switch (id) {
case GPS:
// enable it...
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, gps_enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "gps:minimum channels", 8);
maxch = iniparser_getint(gpsctlConf, "gps:maximum channels", 16);
put_uint8_slBuffer(b, o + 1, minch); // min of 8...
put_uint8_slBuffer(b, o + 2, maxch); // max of 16...
break;
case SBAS:
// disable everything else...
enabled = iniparser_getboolean(gpsctlConf, "sbas:enabled", false);
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "sbas:minimum channels", 1);
maxch = iniparser_getint(gpsctlConf, "sbas:maximum channels", 3);
put_uint8_slBuffer(b, o + 1, minch); // min of 8...
put_uint8_slBuffer(b, o + 2, maxch); // max of 16...
break;
case Galileo:
// enable it...
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, galileo_enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "galileo:minimum channels", 4);
maxch = iniparser_getint(gpsctlConf, "galileo:maximum channels", 8);
put_uint8_slBuffer(b, o + 1, minch); // min of 4...
put_uint8_slBuffer(b, o + 2, maxch); // max of 8...
break;
case BeiDou:
// disable everything else...
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, beidou_enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "beidou:minimum channels", 8);
maxch = iniparser_getint(gpsctlConf, "beidou:maximum channels", 16);
put_uint8_slBuffer(b, o + 1, minch); // min of 8...
put_uint8_slBuffer(b, o + 2, maxch); // max of 16...
break;
case IMES:
// disable everything else...
enabled = iniparser_getboolean(gpsctlConf, "imes:enabled", false);
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "imes:minimum channels", 0);
maxch = iniparser_getint(gpsctlConf, "imes:maximum channels", 8);
put_uint8_slBuffer(b, o + 1, minch); // min of 0...
put_uint8_slBuffer(b, o + 2, maxch); // max of 8...
break;
case QZSS:
// disable everything else...
enabled = iniparser_getboolean(gpsctlConf, "qzss:enabled", false);
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "qzss:minimum channels", 0);
maxch = iniparser_getint(gpsctlConf, "qzss:maximum channels", 3);
put_uint8_slBuffer(b, o + 1, minch); // min of 0...
put_uint8_slBuffer(b, o + 2, maxch); // max of 3...
break;
case GLONASS:
// enable it...
flags = (uint32_t) setBit_slBits(get_uint32_slBuffer(b, o + 4), 0, glonass_enabled);
put_uint32_slBuffer(b, o + 4, flags);
// set our min and max channels...
minch = iniparser_getint(gpsctlConf, "glonass:minimum channels", 8);
maxch = iniparser_getint(gpsctlConf, "glonass:maximum channels", 14);
put_uint8_slBuffer(b, o + 1, minch); // min of 8...
put_uint8_slBuffer(b, o + 2, maxch); // max of 14...
break;
}
}
// now send it back to the GPS...
ubxMsg newGnssMsg = createUbxMsg(gnssMsg.type, b); // this recomputes the checksum...
slReturn suamResp = sendUbxAckedMsg(fdPort, newGnssMsg);
if (isErrorReturn(suamResp))
return makeErrorMsgReturn(ERR_CAUSE(suamResp), "problem sending GNSS configuration to GPS");
free(body);
free(gnssMsg.body);
// We don't care if NMEA version is 4.0, 40, 4.1, or 41, etc, we do the right thing anyway
dnmeaver = iniparser_getdouble(gpsctlConf, "nmea:version", 41) + 0.005;
if (dnmeaver < 20.0) {
dnmeaver = dnmeaver * 10.0;
}
nmeaver = (int) dnmeaver;
// force nmea version 41 or higher if BeiDou or Galileo are enabled
if ((beidou_enabled || galileo_enabled) && nmeaver < 41) {
nmeaver = 41;
}
// convert to from integer 40, 41, etc to hex 0x40, 0x41
nmeaver = (nmeaver / 10) * 16 + (nmeaver % 10);
ubxConfigNMEAVersion(fdPort, verbosity, nmeaver);
return makeOkReturn();
}
// Configure Time Pulse
extern slReturn ubxConfigTimePulse(int fdPort, int verbosity) {
// configure the time pulse...
// get the time pulse configuration...
ubxType tp5Type = { UBX_CFG, UBX_CFG_TP5 };
slBuffer* body = create_slBuffer(1, LittleEndian);
*buffer_slBuffer(body) = 0; // we only look at time pulse zero...
ubxMsg msg = createUbxMsg(tp5Type, body);
ubxMsg tp5Msg;
slReturn tp5Resp = pollUbx(fdPort, msg, CFG_TP5_MAX_MS, &tp5Msg);
if (isErrorReturn(tp5Resp))
return makeErrorMsgReturn(ERR_CAUSE(tp5Resp), "problem getting time pulse information from GPS");
// make the changes we need to make...
slBuffer* b = tp5Msg.body;
put_uint16_slBuffer(b, 4, iniparser_getint(gpsctlConf, "time pulse:antenna cable delay", 56)); // set the antenna cable delay to 56 ns...
put_uint16_slBuffer(b, 6, iniparser_getint(gpsctlConf, "time pulse:rf group delay", 20)); // set the RF group delay to 20 ns...
put_uint32_slBuffer(b, 8, iniparser_getint(gpsctlConf, "time pulse:unlocked pulse period", 1000000)); // set the unlocked period to 1 second (1,000,000 microseconds)...
put_uint32_slBuffer(b, 16, iniparser_getint(gpsctlConf, "time pulse:unlocked pulse length", 0)); // set the unlocked pulse length to 0 seconds...
put_uint32_slBuffer(b, 12, iniparser_getint(gpsctlConf, "time pulse:locked pulse period", 1000000)); // set the locked period to 1 second (1,000,000 microseconds)...
put_uint32_slBuffer(b, 20, iniparser_getint(gpsctlConf, "time pulse:locked pulse length", 500000)); // set the locked pulse length to 0.5 seconds (500,000 microseconds)...
put_uint32_slBuffer(b, 24, iniparser_getint(gpsctlConf, "time pulse:user configurable delay", 0)); // set the user-configurable delay to zero...
put_uint32_slBuffer(b, 28, 0x77); // set the flags...
// now send it back to the GPS...
ubxMsg newTp5Msg = createUbxMsg(tp5Msg.type, b);
slReturn suamResp = sendUbxAckedMsg(fdPort, newTp5Msg);
if (isErrorReturn(suamResp))
return makeErrorMsgReturn(ERR_CAUSE(suamResp), "problem sending time pulse configuration to GPS");
free(body);
free(tp5Msg.body);
return makeOkReturn();
}
extern slReturn ubxConfigNavEngine(int fdPort, int verbosity) {
// get the navigation engine configuration...
ubxType nav5Type = { UBX_CFG, UBX_CFG_NAV5 };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(nav5Type, body);
ubxMsg nav5Msg;
slReturn nav5Resp = pollUbx(fdPort, msg, CFG_NAV5_MAX_MS, &nav5Msg);
if (isErrorReturn(nav5Resp))
return makeErrorMsgReturn(ERR_CAUSE(nav5Resp), "problem getting navigation engine information from GPS");
// make our changes...
slBuffer* b = nav5Msg.body;
put_uint16_slBuffer(b, 0, 0x0577); // configure all settings...
put_uint8_slBuffer(b, 2, iniparser_getint(gpsctlConf, "navigation engine:dynamic model", Stationary)); // use stationary mode...
put_uint8_slBuffer(b, 3, iniparser_getint(gpsctlConf, "navigation engine:fix mode", Only3D)); // use any mode...
put_uint32_slBuffer(b, 4, (int) (iniparser_getdouble(gpsctlConf, "navigation engine:fixed altitude (2d)", 0) * 100.0)); // fixed altitude for 2D (not used)...
put_uint32_slBuffer(b, 8, (int) (iniparser_getdouble(gpsctlConf, "navigation engine:fixed altitude variance (2d)", 0) * 10000.0)); // fixed altitude variance for 2D (1 m^2)...
put_uint8_slBuffer(b, 12, iniparser_getint(gpsctlConf, "navigation engine:minimum elevation", 20)); // minimum elevation is 5 degrees...
put_uint16_slBuffer(b, 14, (int) (iniparser_getdouble(gpsctlConf, "navigation engine:position dop mask", 10) * 10.0)); // position DoP mask is 10.0...
put_uint16_slBuffer(b, 16, (int) (iniparser_getdouble(gpsctlConf, "navigation engine:time dop mask", 10) * 10.0)); // time DoP mask is 10.0...
put_uint16_slBuffer(b, 18, iniparser_getint(gpsctlConf, "navigation engine:position accuracy mask", 40)); // position accuracy mask is 100 meters...
put_uint16_slBuffer(b, 20, iniparser_getint(gpsctlConf, "navigation engine:time accuracy mask", 40)); // time accuracy mask is 300 m...
put_uint8_slBuffer(b, 22, iniparser_getint(gpsctlConf, "navigation engine:static hold threshold", 0)); // static hold threshold is 0 cm/s...
put_uint8_slBuffer(b, 23, iniparser_getint(gpsctlConf, "navigation engine:dynamic gnss timeout", 60)); // dynamic GNSS timeout is 60 seconds (not used)...
put_uint8_slBuffer(b, 24, iniparser_getint(gpsctlConf, "navigation engine:threshold above c/no", 8)); // number of satellites that must be above CNo threshold...
put_uint8_slBuffer(b, 25, iniparser_getint(gpsctlConf, "navigation engine:c/no threshold", 20)); // CNo threshold...
put_uint16_slBuffer(b, 28, iniparser_getint(gpsctlConf, "navigation engine:static hold max distance", 0)); // static hold distance threshold...
put_uint8_slBuffer(b, 30, iniparser_getint(gpsctlConf, "navigation engine:utc standard", USNO_UTC)); // use USNO UTC...
// send it back to the GPS...
ubxMsg newNav5Msg = createUbxMsg(nav5Msg.type, b);
slReturn suamResp = sendUbxAckedMsg(fdPort, newNav5Msg);
if (isErrorReturn(suamResp))
return makeErrorMsgReturn(ERR_CAUSE(suamResp), "problem sending navigation engine configuration to GPS");
free(body);
free(nav5Msg.body);
return makeOkReturn();
}
// Configures the GPS for maximum timing accuracy...
extern slReturn ubxConfigForTiming(int fdPort, int verbosity) {
// configure the GNSS for GPS, GLONASS, BeiDou, and Galileo, with no SBAS.
ubxConfigSatellites(fdPort, verbosity);
// configure the time pulse...
ubxConfigTimePulse(fdPort, verbosity);
// configure the navigation engine...
ubxConfigNavEngine(fdPort, verbosity);
// Suppress NMEA output except for ZDA messages
ubxEnableNMEAMsg(fdPort, verbosity, GGA, iniparser_getboolean(gpsctlConf, "nmea:gga", false));
ubxEnableNMEAMsg(fdPort, verbosity, GLL, iniparser_getboolean(gpsctlConf, "nmea:gll", false));
ubxEnableNMEAMsg(fdPort, verbosity, GSA, iniparser_getboolean(gpsctlConf, "nmea:gsa", false));
ubxEnableNMEAMsg(fdPort, verbosity, GSV, iniparser_getboolean(gpsctlConf, "nmea:gsv", false));
ubxEnableNMEAMsg(fdPort, verbosity, RMC, iniparser_getboolean(gpsctlConf, "nmea:rmc", true));
ubxEnableNMEAMsg(fdPort, verbosity, VTG, iniparser_getboolean(gpsctlConf, "nmea:vtg", false));
ubxEnableNMEAMsg(fdPort, verbosity, GRS, iniparser_getboolean(gpsctlConf, "nmea:grs", false));
ubxEnableNMEAMsg(fdPort, verbosity, GST, iniparser_getboolean(gpsctlConf, "nmea:gst", false));
ubxEnableNMEAMsg(fdPort, verbosity, ZDA, iniparser_getboolean(gpsctlConf, "nmea:zda", true));
return makeOkReturn();
}
// Fills the fix structure at the given pointer with information about a time and position fix obtained from the
// GPS system.
#define NAV_PVT_BODY_SIZE 92
extern slReturn ubxGetFix(int fdPort, int verbosity, ubxFix* fix) {
// get a fix from the GPS...
ubxMsg fixMsg;
slReturn pollResp = pollUbx(fdPort, createUbxMsg(ut_NAV_PVT, create_slBuffer(0, LittleEndian)), NAV_PVT_MAX_MS, &fixMsg);
if (isErrorReturn(pollResp)) return pollResp;
if (size_slBuffer(fixMsg.body) < NAV_PVT_BODY_SIZE)
return makeErrorFmtMsgReturn(ERR_ROOT, "unexpected NAV_PVT message body size: %d bytes", size_slBuffer(fixMsg.body));
// convenience variable...
slBuffer *body = fixMsg.body;
fix->year = get_uint16_slBuffer(body, 4);
fix->month = get_uint8_slBuffer(body, 6);
fix->day = get_uint8_slBuffer(body, 7);
fix->hour = get_uint8_slBuffer(body, 8);
fix->minute = get_uint8_slBuffer(body, 9);
fix->second = get_uint8_slBuffer(body, 10);
fix->nanoseconds_correction = get_int32_slBuffer(body, 16);
fix->time_accuracy_ns = get_uint32_slBuffer(body, 12);
fix->date_valid = isBitSet_slBits(get_uint8_slBuffer(body, 11), 0);
fix->time_valid = isBitSet_slBits(get_uint8_slBuffer(body, 11), 1);
fix->time_resolved = isBitSet_slBits(get_uint8_slBuffer(body, 11), 2);
fix->fix_is_3d = (get_uint8_slBuffer(body, 20) == 3);
fix->fix_valid = isBitSet_slBits(get_uint8_slBuffer(body, 21), 0);
fix->number_of_satellites_used = get_uint8_slBuffer(body, 23);
fix->latitude_deg = get_int32_slBuffer(body, 28) * 1e-7;
fix->longitude_deg = get_int32_slBuffer(body, 24) * 1e-7;
fix->height_above_ellipsoid_mm = get_int32_slBuffer(body, 32);
fix->height_above_sea_level_mm = get_int32_slBuffer(body, 36);
fix->height_accuracy_mm = get_uint32_slBuffer(body, 44);
fix->horizontal_accuracy_mm = get_uint32_slBuffer(body, 40);
fix->ground_speed_mm_s = get_int32_slBuffer(body, 60);
fix->ground_speed_accuracy_mm_s = get_uint32_slBuffer(body, 68);
fix->heading_deg = get_int32_slBuffer(body, 64) * 1e-5;
fix->heading_accuracy_deg = get_uint32_slBuffer(body, 72) * 1e-5;
#undef body
correctTime(fix);
return makeOkReturn();
}
// Fills the ubxVersion structure at the given pointer with information about the GPS's version.
extern slReturn ubxGetVersion(int fdPort, int verbosity, ubxVersion* version) {
ubxType type = { UBX_MON, UBX_MON_VER };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(type, body);
ubxMsg versionMsg;
slReturn result = pollUbx(fdPort, msg, MON_VER_MAX_MS, &versionMsg);
if (isErrorReturn(result)) return result;
// decode the message and print the results...
char* buf = (char *) buffer_slBuffer(versionMsg.body);
version->software = getAllocatedStringCopy(buf);
version->hardware = getAllocatedStringCopy(buf + 30);
// calculate the number of extension entries we have...
int ne = ((int) size_slBuffer(versionMsg.body) - 40) / 30;
version->number_of_extensions = ne;
if (ne == 0)
version->extensions = NULL;
else {
version->extensions = safeMalloc((size_t) (sizeof(buf) * ne));
char** ptr = version->extensions;
for (int i = 40; i < (int) size_slBuffer(versionMsg.body); i += 30) {
*ptr = getAllocatedStringCopy(buf + i);
ptr++;
}
*ptr = NULL;
}
return makeOkReturn();
}
extern char* getSignalQuality(signalQuality qual) {
switch (qual) {
case signalNone: return "None";
case signalAcquired: return "Acquired";
case signalCodeCarrierLocked: return "Code/carrier locked";
case signalCodeLocked: return "Code locked";
case signalSearching: return "Searching";
case signalUnusable: return "Unusable";
default: return "Unknown";
}
}
extern char* getSatelliteHealth(satelliteHealth health) {
switch (health) {
case healthUnknown: return "Unknown";
case healthOk: return "Ok";
case healthBad: return "Bad";
default: return "Unknown";
}
}
extern char* getOrbitSource(orbitSource source) {
switch (source) {
case osNone: return "None";
case osAlmanac: return "Almanac";
case osAssistNowAutonomous: return "AssistNow auto";
case osAssistNowOffline: return "AssistNow off";
case osEphemeris: return "Ephemeris";
case osOther: return "Other";
default: return "Unknown";
}
}
// Fills the ubxSatellites structure at the given pointer with information about the GPS satellites.
extern slReturn ubxGetSatellites(int fdPort, int verbosity, ubxSatellites* satellites) {
ubxType type = { UBX_NAV, UBX_NAV_SAT };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(type, body);
ubxMsg satMsg;
slReturn nsResp = pollUbx(fdPort, msg, MON_VER_MAX_MS, &satMsg);
if (isErrorReturn(nsResp)) return nsResp;
// now decode our message, starting with the number of satellites...
void* b = satMsg.body;
ubxSatellites* ss = satellites;
ss->numberOfSatellites = get_uint8_slBuffer(b, 5);
// make a place to store our results...
ss->satellites = safeMalloc(ss->numberOfSatellites * sizeof(ubxSatellite));
// now examine each of our satellites...
for (int i = 0; i < ss->numberOfSatellites; i++) {
ubxSatellite* s = ss->satellites + i;
size_t o = (unsigned) (8 + 12 * i);
s->gnssID = get_uint8_slBuffer(b, o + 0);
s->satelliteID = get_uint8_slBuffer(b, o + 1);
s->cno = get_uint8_slBuffer(b, o + 2);
s->elevation = get_int8_slBuffer(b, o + 3);
s->azimuth = get_int16_slBuffer(b, o + 4);
s->pseudoRangeResidualM = get_int8_slBuffer(b, o + 6) * 0.1;
uint32_t flags = get_uint32_slBuffer(b, o + 8);
s->signalQuality = (signalQuality) min_i((int) getBitField_slBits(flags, 0x07), 5);
s->used = isBitSet_slBits(flags, 3);
s->health = (satelliteHealth) getBitField_slBits(flags, 0x30);
s->diffCorr = isBitSet_slBits(flags, 6);
s->smoothed = isBitSet_slBits(flags, 7);
s->orbitSource = (orbitSource) min_i((int) getBitField_slBits(flags, 0x700), 5);
s->haveEphemeris = isBitSet_slBits(flags, 11);
s->haveAlmanac = isBitSet_slBits(flags, 12);
s->haveAssistNowOff = isBitSet_slBits(flags, 13);
s->haveAssistNowAuto = isBitSet_slBits(flags, 14);
s->sbasCorrUsed = isBitSet_slBits(flags, 16);
s->rtcmCorrUsed = isBitSet_slBits(flags, 17);
s->prCorrUsed = isBitSet_slBits(flags, 20);
s->crCorrUsed = isBitSet_slBits(flags, 21);
s->doCorrUsed = isBitSet_slBits(flags, 22);
}
return makeOkReturn();
}
extern char* getGnssName(gnssID id) {
switch (id) {
case GPS: return "GPS";
case SBAS: return "SBAS";
case Galileo: return "Galileo";
case BeiDou: return "BeiDou";
case IMES: return "IMES";
case QZSS: return "QZSS";
case GLONASS: return "GLONASS";
default: return "unknown";
}
}
extern char* getDynamicModelName(dynModel model) {
switch (model) {
case Portable: return "Portable";
case Stationary: return "Stationary";
case Pedestrian: return "Pedestrian";
case Automotive: return "Automotive";
case Sea: return "Sea";
case Air1G: return "Air (less than 1G acceleration)";
case Air2G: return "Air (less than 2G acceleration)";
case Air4G: return "Air (less than 4G acceleration)";
case Watch: return "Watch";
default: return "unknown";
}
}
extern char* getFixModeName(fixMode mode) {
switch (mode) {
case Only2D: return "2D only";
case Only3D: return "3D only";
case Auto2D3D: return "Auto 2D/3D";
default: return "unknown";
}
}
extern char* getUTCTypeName(utcType utc) {
switch (utc) {
case AutoUTC: return "Auto";
case USNO_UTC: return "USNO";
case GLONASS_UTC: return "GLONASS";
case BeiDou_UTC: return "BeiDou";
default: return "unknown";
}
}
extern char* getFixTimeRefName(fixTimeRefType type) {
switch (type) {
case fixUTC: return "UTC";
case fixGPS: return "GPS";
case fixGLONASS: return "GLONASS";
case fixBeiDou: return "BeiDou";
case fixGalileo: return "Galileo";
default: return "unknown";
}
}
extern char* getPowerModeName(powerModeType type) {
switch (type) {
case pmFull: return "Full";
case pmBalanced: return "Balanced";
case pmInterval: return "Interval";
case pmAggressive1Hz: return "Aggressive (1 Hz)";
case pmAggressive2Hz: return "Aggressive (2 Hz)";
case pmAggressive4Hz: return "Aggressive (3 Hz)";
case pmInvalid: return "Invalid";
default: return "unknown";
}
}
// Fills the ubxVersion structure at the given pointer with information about the GPS's version. Returns the
// appropriate reportResponse value.
extern char* getTimeGridTypeName(timegridType type) {
switch (type) {
case tgUTC: return "UTC";
case tgGPS: return "GPS";
case tgGLONASS: return "GLONASS";
case tgBeiDou: return "BeiDou";
case tgGalileo: return "Galileo";
default: return "unknown";
}
}
// Returns the current state of NMEA data on the GPS's UART. If the return value is ok, the result is stored
// as a bool in the information field.
static slReturn isNmeaOn(int fdPort) {
// poll for the current configuration...
ubxMsg poll = createUbxMsg(ut_CFG_PRT, init_slBuffer(LittleEndian, 1 /* UBX_CFG_PRT_UART_ID */));
ubxMsg current;
slReturn resp = pollUbx(fdPort, poll, CFG_PRT_MAX_MS, ¤t);
if (isErrorReturn(resp)) return resp;
bool state = isBitSet_slBits(get_uint16_slBuffer(current.body, 14), 1);
return makeOkInfoReturn(bool2info(state));
}
// Fills the ubxConfig structure at the given pointer with information about the GPS's configuration.
extern slReturn ubxGetConfig(int fdPort, int verbosity, ubxConfig* config) {
// get the antenna configuration...
ubxType antType = { UBX_CFG, UBX_CFG_ANT };
slBuffer* body = create_slBuffer(0, LittleEndian);
ubxMsg msg = createUbxMsg(antType, body);
ubxMsg antMsg;
slReturn antResp = pollUbx(fdPort, msg, CFG_ANT_MAX_MS, &antMsg);
if (isErrorReturn(antResp))
return makeErrorMsgReturn(ERR_CAUSE(antResp), "problem getting antenna information from GPS");
uint16_t antMask = get_uint16_slBuffer(antMsg.body, 0);
free(body);
free(antMsg.body);
config->antPwr = isBitSet_slBits(antMask, 0);