-
Notifications
You must be signed in to change notification settings - Fork 963
/
GPS.cpp
1753 lines (1574 loc) · 67.4 KB
/
GPS.cpp
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
#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_GPS
#include "Default.h"
#include "GPS.h"
#include "GpioLogic.h"
#include "NodeDB.h"
#include "PowerMon.h"
#include "RTC.h"
#include "Throttle.h"
#include "buzz.h"
#include "meshUtils.h"
#include "main.h" // pmu_found
#include "sleep.h"
#include "GPSUpdateScheduling.h"
#include "cas.h"
#include "ubx.h"
#ifdef ARCH_PORTDUINO
#include "PortduinoGlue.h"
#include "meshUtils.h"
#include <algorithm>
#include <ctime>
#endif
#ifndef GPS_RESET_MODE
#define GPS_RESET_MODE HIGH
#endif
// Not all platforms have std::size().
template <typename T, std::size_t N> std::size_t array_count(const T (&)[N])
{
return N;
}
#if defined(NRF52840_XXAA) || defined(NRF52833_XXAA) || defined(ARCH_ESP32) || defined(ARCH_PORTDUINO)
HardwareSerial *GPS::_serial_gps = &Serial1;
#elif defined(ARCH_RP2040)
SerialUART *GPS::_serial_gps = &Serial1;
#else
HardwareSerial *GPS::_serial_gps = nullptr;
#endif
GPS *gps = nullptr;
static const char *ACK_SUCCESS_MESSAGE = "Get ack success!";
static GPSUpdateScheduling scheduling;
/// Multiple GPS instances might use the same serial port (in sequence), but we can
/// only init that port once.
static bool didSerialInit;
static struct uBloxGnssModelInfo {
char swVersion[30];
char hwVersion[10];
uint8_t extensionNo;
char extension[10][30];
uint8_t protocol_version;
} ublox_info;
#define GPS_SOL_EXPIRY_MS 5000 // in millis. give 1 second time to combine different sentences. NMEA Frequency isn't higher anyway
#define NMEA_MSG_GXGSA "GNGSA" // GSA message (GPGSA, GNGSA etc)
// For logging
static const char *getGPSPowerStateString(GPSPowerState state)
{
switch (state) {
case GPS_ACTIVE:
return "ACTIVE";
case GPS_IDLE:
return "IDLE";
case GPS_SOFTSLEEP:
return "SOFTSLEEP";
case GPS_HARDSLEEP:
return "HARDSLEEP";
case GPS_OFF:
return "OFF";
default:
assert(false); // Unhandled enum value..
return "FALSE"; // to make new ESP-IDF happy
}
}
static void UBXChecksum(uint8_t *message, size_t length)
{
uint8_t CK_A = 0, CK_B = 0;
// Calculate the checksum, starting from the CLASS field (which is message[2])
for (size_t i = 2; i < length - 2; i++) {
CK_A = (CK_A + message[i]) & 0xFF;
CK_B = (CK_B + CK_A) & 0xFF;
}
// Place the calculated checksum values in the message
message[length - 2] = CK_A;
message[length - 1] = CK_B;
}
// Calculate the checksum for a CAS packet
static void CASChecksum(uint8_t *message, size_t length)
{
uint32_t cksum = ((uint32_t)message[5] << 24); // Message ID
cksum += ((uint32_t)message[4]) << 16; // Class
cksum += message[2]; // Payload Len
// Iterate over the payload as a series of uint32_t's and
// accumulate the cksum
for (size_t i = 0; i < (length - 10) / 4; i++) {
uint32_t pl = 0;
memcpy(&pl, (message + 6) + (i * sizeof(uint32_t)), sizeof(uint32_t)); // avoid pointer dereference
cksum += pl;
}
// Place the checksum values in the message
message[length - 4] = (cksum & 0xFF);
message[length - 3] = (cksum & (0xFF << 8)) >> 8;
message[length - 2] = (cksum & (0xFF << 16)) >> 16;
message[length - 1] = (cksum & (0xFF << 24)) >> 24;
}
// Function to create a ublox packet for editing in memory
uint8_t GPS::makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
{
// Construct the UBX packet
UBXscratch[0] = 0xB5; // header
UBXscratch[1] = 0x62; // header
UBXscratch[2] = class_id; // class
UBXscratch[3] = msg_id; // id
UBXscratch[4] = payload_size; // length
UBXscratch[5] = 0x00;
UBXscratch[6 + payload_size] = 0x00; // CK_A
UBXscratch[7 + payload_size] = 0x00; // CK_B
for (int i = 0; i < payload_size; i++) {
UBXscratch[6 + i] = pgm_read_byte(&msg[i]);
}
UBXChecksum(UBXscratch, (payload_size + 8));
return (payload_size + 8);
}
// Function to create a CAS packet for editing in memory
uint8_t GPS::makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
{
// General CAS structure
// | H1 | H2 | payload_len | cls | msg | Payload ... | Checksum |
// Size: | 1 | 1 | 2 | 1 | 1 | payload_len | 4 |
// Pos: | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 ... | 6 + payload_len ... |
// |------|------|-------------|------|------|------|--------------|---------------------------|
// | 0xBA | 0xCE | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX ... | 0xXX | 0xXX | 0xXX | 0xXX |
// Construct the CAS packet
UBXscratch[0] = 0xBA; // header 1 (0xBA)
UBXscratch[1] = 0xCE; // header 2 (0xCE)
UBXscratch[2] = payload_size; // length 1
UBXscratch[3] = 0; // length 2
UBXscratch[4] = class_id; // class
UBXscratch[5] = msg_id; // id
UBXscratch[6 + payload_size] = 0x00; // Checksum
UBXscratch[7 + payload_size] = 0x00;
UBXscratch[8 + payload_size] = 0x00;
UBXscratch[9 + payload_size] = 0x00;
for (int i = 0; i < payload_size; i++) {
UBXscratch[6 + i] = pgm_read_byte(&msg[i]);
}
CASChecksum(UBXscratch, (payload_size + 10));
#if defined(GPS_DEBUG) && defined(DEBUG_PORT)
LOG_DEBUG("CAS packet: ");
DEBUG_PORT.hexDump(MESHTASTIC_LOG_LEVEL_DEBUG, UBXscratch, payload_size + 10);
#endif
return (payload_size + 10);
}
GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
{
uint8_t buffer[768] = {0};
uint8_t b;
int bytesRead = 0;
uint32_t startTimeout = millis() + waitMillis;
#ifdef GPS_DEBUG
std::string debugmsg = "";
#endif
while (millis() < startTimeout) {
if (_serial_gps->available()) {
b = _serial_gps->read();
#ifdef GPS_DEBUG
debugmsg += vformat("%c", (b >= 32 && b <= 126) ? b : '.');
#endif
buffer[bytesRead] = b;
bytesRead++;
if ((bytesRead == 767) || (b == '\r')) {
if (strnstr((char *)buffer, message, bytesRead) != nullptr) {
#ifdef GPS_DEBUG
LOG_DEBUG("Found: %s", message); // Log the found message
#endif
return GNSS_RESPONSE_OK;
} else {
bytesRead = 0;
#ifdef GPS_DEBUG
LOG_DEBUG(debugmsg.c_str());
#endif
}
}
}
}
return GNSS_RESPONSE_NONE;
}
GPS_RESPONSE GPS::getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
{
uint32_t startTime = millis();
uint8_t buffer[CAS_ACK_NACK_MSG_SIZE] = {0};
uint8_t bufferPos = 0;
// CAS-ACK-(N)ACK structure
// | H1 | H2 | Payload Len | cls | msg | Payload | Checksum (4) |
// | | | | | | Cls | Msg | Reserved | |
// |------|------|-------------|------|------|------|------|-------------|---------------------------|
// ACK-NACK| 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x00 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |
// ACK-ACK | 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x01 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |
while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
if (_serial_gps->available()) {
buffer[bufferPos++] = _serial_gps->read();
// keep looking at the first two bytes of buffer until
// we have found the CAS frame header (0xBA, 0xCE), if not
// keep reading bytes until we find a frame header or we run
// out of time.
if ((bufferPos == 2) && !(buffer[0] == 0xBA && buffer[1] == 0xCE)) {
buffer[0] = buffer[1];
buffer[1] = 0;
bufferPos = 1;
}
}
// we have read all the bytes required for the Ack/Nack (14-bytes)
// and we must have found a frame to get this far
if (bufferPos == sizeof(buffer) - 1) {
uint8_t msg_cls = buffer[4]; // message class should be 0x05
uint8_t msg_msg_id = buffer[5]; // message id should be 0x00 or 0x01
uint8_t payload_cls = buffer[6]; // payload class id
uint8_t payload_msg = buffer[7]; // payload message id
// Check for an ACK-ACK for the specified class and message id
if ((msg_cls == 0x05) && (msg_msg_id == 0x01) && payload_cls == class_id && payload_msg == msg_id) {
#ifdef GPS_DEBUG
LOG_INFO("Got ACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
return GNSS_RESPONSE_OK;
}
// Check for an ACK-NACK for the specified class and message id
if ((msg_cls == 0x05) && (msg_msg_id == 0x00) && payload_cls == class_id && payload_msg == msg_id) {
#ifdef GPS_DEBUG
LOG_WARN("Got NACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
return GNSS_RESPONSE_NAK;
}
// This isn't the frame we are looking for, clear the buffer
// and try again until we run out of time.
memset(buffer, 0x0, sizeof(buffer));
bufferPos = 0;
}
}
return GNSS_RESPONSE_NONE;
}
GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
{
uint8_t b;
uint8_t ack = 0;
const uint8_t ackP[2] = {class_id, msg_id};
uint8_t buf[10] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00};
uint32_t startTime = millis();
const char frame_errors[] = "More than 100 frame errors";
int sCounter = 0;
#ifdef GPS_DEBUG
std::string debugmsg = "";
#endif
for (int j = 2; j < 6; j++) {
buf[8] += buf[j];
buf[9] += buf[8];
}
for (int j = 0; j < 2; j++) {
buf[6 + j] = ackP[j];
buf[8] += buf[6 + j];
buf[9] += buf[8];
}
while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
if (ack > 9) {
#ifdef GPS_DEBUG
LOG_INFO("Got ACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
#endif
return GNSS_RESPONSE_OK; // ACK received
}
if (_serial_gps->available()) {
b = _serial_gps->read();
if (b == frame_errors[sCounter]) {
sCounter++;
if (sCounter == 26) {
#ifdef GPS_DEBUG
LOG_DEBUG(debugmsg.c_str());
#endif
return GNSS_RESPONSE_FRAME_ERRORS;
}
} else {
sCounter = 0;
}
#ifdef GPS_DEBUG
debugmsg += vformat("%02X", b);
#endif
if (b == buf[ack]) {
ack++;
} else {
if (ack == 3 && b == 0x00) { // UBX-ACK-NAK message
#ifdef GPS_DEBUG
LOG_DEBUG(debugmsg.c_str());
#endif
LOG_WARN("Got NAK for class %02X message %02X", class_id, msg_id);
return GNSS_RESPONSE_NAK; // NAK received
}
ack = 0; // Reset the acknowledgement counter
}
}
}
#ifdef GPS_DEBUG
LOG_DEBUG(debugmsg.c_str());
LOG_WARN("No response for class %02X message %02X", class_id, msg_id);
#endif
return GNSS_RESPONSE_NONE; // No response received within timeout
}
/**
* @brief
* @note New method, this method can wait for the specified class and message ID, and return the payload
* @param *buffer: The message buffer, if there is a response payload message, it will be returned through the buffer parameter
* @param size: size of buffer
* @param requestedClass: request class constant
* @param requestedID: request message ID constant
* @retval length of payload message
*/
int GPS::getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, uint32_t waitMillis)
{
uint16_t ubxFrameCounter = 0;
uint32_t startTime = millis();
uint16_t needRead = 0;
while (Throttle::isWithinTimespanMs(startTime, waitMillis)) {
if (_serial_gps->available()) {
int c = _serial_gps->read();
switch (ubxFrameCounter) {
case 0:
// ubxFrame 'μ'
if (c == 0xB5) {
ubxFrameCounter++;
}
break;
case 1:
// ubxFrame 'b'
if (c == 0x62) {
ubxFrameCounter++;
} else {
ubxFrameCounter = 0;
}
break;
case 2:
// Class
if (c == requestedClass) {
ubxFrameCounter++;
} else {
ubxFrameCounter = 0;
}
break;
case 3:
// Message ID
if (c == requestedID) {
ubxFrameCounter++;
} else {
ubxFrameCounter = 0;
}
break;
case 4:
// Payload length lsb
needRead = c;
ubxFrameCounter++;
break;
case 5:
// Payload length msb
needRead |= (c << 8);
ubxFrameCounter++;
// Check for buffer overflow
if (needRead >= size) {
ubxFrameCounter = 0;
break;
}
if (_serial_gps->readBytes(buffer, needRead) != needRead) {
ubxFrameCounter = 0;
} else {
// return payload length
#ifdef GPS_DEBUG
LOG_INFO("Got ACK for class %02X message %02X in %dms", requestedClass, requestedID, millis() - startTime);
#endif
return needRead;
}
break;
default:
break;
}
}
}
return 0;
}
#if GPS_BAUDRATE_FIXED
// if GPS_BAUDRATE is specified in variant, only try that.
static const int serialSpeeds[1] = {GPS_BAUDRATE};
static const int rareSerialSpeeds[1] = {GPS_BAUDRATE};
#else
static const int serialSpeeds[3] = {9600, 115200, 38400};
static const int rareSerialSpeeds[3] = {4800, 57600, GPS_BAUDRATE};
#endif
/**
* @brief Setup the GPS based on the model detected.
* We detect the GPS by cycling through a set of baud rates, first common then rare.
* For each baud rate, we run GPS::Probe to send commands and match the responses
* to known GPS responses.
* @retval Whether setup reached the end of its potential to configure the GPS.
*/
bool GPS::setup()
{
if (!didSerialInit) {
int msglen = 0;
if (tx_gpio && gnssModel == GNSS_MODEL_UNKNOWN) {
if (probeTries < 2) {
LOG_DEBUG("Probe for GPS at %d", serialSpeeds[speedSelect]);
gnssModel = probe(serialSpeeds[speedSelect]);
if (gnssModel == GNSS_MODEL_UNKNOWN) {
if (++speedSelect == array_count(serialSpeeds)) {
speedSelect = 0;
++probeTries;
}
}
}
// Rare Serial Speeds
if (probeTries == 2) {
LOG_DEBUG("Probe for GPS at %d", rareSerialSpeeds[speedSelect]);
gnssModel = probe(rareSerialSpeeds[speedSelect]);
if (gnssModel == GNSS_MODEL_UNKNOWN) {
if (++speedSelect == array_count(rareSerialSpeeds)) {
LOG_WARN("Give up on GPS probe and set to %d", GPS_BAUDRATE);
return true;
}
}
}
}
if (gnssModel != GNSS_MODEL_UNKNOWN) {
setConnected();
} else {
return false;
}
if (gnssModel == GNSS_MODEL_MTK) {
/*
* t-beam-s3-core uses the same L76K GNSS module as t-echo.
* Unlike t-echo, L76K uses 9600 baud rate for communication by default.
* */
// Initialize the L76K Chip, use GPS + GLONASS + BEIDOU
_serial_gps->write("$PCAS04,7*1E\r\n");
delay(250);
// only ask for RMC and GGA
_serial_gps->write("$PCAS03,1,0,0,0,1,0,0,0,0,0,,,0,0*02\r\n");
delay(250);
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
_serial_gps->write("$PCAS11,3*1E\r\n");
delay(250);
} else if (gnssModel == GNSS_MODEL_MTK_L76B) {
// Waveshare Pico-GPS hat uses the L76B with 9600 baud
// Initialize the L76B Chip, use GPS + GLONASS
// See note in L76_Series_GNSS_Protocol_Specification, chapter 3.29
_serial_gps->write("$PMTK353,1,1,0,0,0*2B\r\n");
// Above command will reset the GPS and takes longer before it will accept new commands
delay(1000);
// only ask for RMC and GGA (GNRMC and GNGGA)
// See note in L76_Series_GNSS_Protocol_Specification, chapter 2.1
_serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
delay(250);
// Enable SBAS
_serial_gps->write("$PMTK301,2*2E\r\n");
delay(250);
// Enable PPS for 2D/3D fix only
_serial_gps->write("$PMTK285,3,100*3F\r\n");
delay(250);
// Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
_serial_gps->write("$PMTK886,1*29\r\n");
delay(250);
} else if (gnssModel == GNSS_MODEL_MTK_PA1616S) {
// PA1616S is used in some GPS breakout boards from Adafruit
// PA1616S does not have GLONASS capability. PA1616D does, but is not implemented here.
_serial_gps->write("$PMTK353,1,0,0,0,0*2A\r\n");
// Above command will reset the GPS and takes longer before it will accept new commands
delay(1000);
// Only ask for RMC and GGA (GNRMC and GNGGA)
_serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
delay(250);
// Enable SBAS / WAAS
_serial_gps->write("$PMTK301,2*2E\r\n");
delay(250);
} else if (gnssModel == GNSS_MODEL_ATGM336H) {
// Set the intial configuration of the device - these _should_ work for most AT6558 devices
msglen = makeCASPacket(0x06, 0x07, sizeof(_message_CAS_CFG_NAVX_CONF), _message_CAS_CFG_NAVX_CONF);
_serial_gps->write(UBXscratch, msglen);
if (getACKCas(0x06, 0x07, 250) != GNSS_RESPONSE_OK) {
LOG_WARN("ATGM336H: Could not set Config");
}
// Set the update frequence to 1Hz
msglen = makeCASPacket(0x06, 0x04, sizeof(_message_CAS_CFG_RATE_1HZ), _message_CAS_CFG_RATE_1HZ);
_serial_gps->write(UBXscratch, msglen);
if (getACKCas(0x06, 0x04, 250) != GNSS_RESPONSE_OK) {
LOG_WARN("ATGM336H: Could not set Update Frequency");
}
// Set the NEMA output messages
// Ask for only RMC and GGA
uint8_t fields[] = {CAS_NEMA_RMC, CAS_NEMA_GGA};
for (unsigned int i = 0; i < sizeof(fields); i++) {
// Construct a CAS-CFG-MSG packet
uint8_t cas_cfg_msg_packet[] = {0x4e, fields[i], 0x01, 0x00};
msglen = makeCASPacket(0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet);
_serial_gps->write(UBXscratch, msglen);
if (getACKCas(0x06, 0x01, 250) != GNSS_RESPONSE_OK) {
LOG_WARN("ATGM336H: Could not enable NMEA MSG: %d", fields[i]);
}
}
} else if (gnssModel == GNSS_MODEL_UC6580) {
// The Unicore UC6580 can use a lot of sat systems, enable it to
// use GPS L1 & L5 + BDS B1I & B2a + GLONASS L1 + GALILEO E1 & E5a + SBAS + QZSS
// This will reset the receiver, so wait a bit afterwards
// The paranoid will wait for the OK*04 confirmation response after each command.
_serial_gps->write("$CFGSYS,h35155\r\n");
delay(750);
// Must be done after the CFGSYS command
// Turn off GSV messages, we don't really care about which and where the sats are, maybe someday.
_serial_gps->write("$CFGMSG,0,3,0\r\n");
delay(250);
// Turn off GSA messages, TinyGPS++ doesn't use this message.
_serial_gps->write("$CFGMSG,0,2,0\r\n");
delay(250);
// Turn off NOTICE __TXT messages, these may provide Unicore some info but we don't care.
_serial_gps->write("$CFGMSG,6,0,0\r\n");
delay(250);
_serial_gps->write("$CFGMSG,6,1,0\r\n");
delay(250);
} else if (IS_ONE_OF(gnssModel, GNSS_MODEL_AG3335, GNSS_MODEL_AG3352)) {
_serial_gps->write("$PAIR066,1,0,1,0,0,1*3B\r\n"); // Enable GPS+GALILEO+NAVIC
// Configure NMEA (sentences will output once per fix)
_serial_gps->write("$PAIR062,0,1*3F\r\n"); // GGA ON
_serial_gps->write("$PAIR062,1,0*3F\r\n"); // GLL OFF
_serial_gps->write("$PAIR062,2,0*3C\r\n"); // GSA OFF
_serial_gps->write("$PAIR062,3,0*3D\r\n"); // GSV OFF
_serial_gps->write("$PAIR062,4,1*3B\r\n"); // RMC ON
_serial_gps->write("$PAIR062,5,0*3B\r\n"); // VTG OFF
_serial_gps->write("$PAIR062,6,0*38\r\n"); // ZDA ON
delay(250);
_serial_gps->write("$PAIR513*3D\r\n"); // save configuration
} else if (gnssModel == GNSS_MODEL_UBLOX6) {
clearBuffer();
SEND_UBX_PACKET(0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
SEND_UBX_PACKET(0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
// Turn off unwanted NMEA messages, set update rate
SEND_UBX_PACKET(0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
clearBuffer();
SEND_UBX_PACKET(0x06, 0x11, _message_CFG_RXM_ECO, "enable powersave ECO mode for Neo-6", 500);
SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_AID, "disable UBX-AID", 500);
msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE);
_serial_gps->write(UBXscratch, msglen);
if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
LOG_WARN("Unable to save GNSS module config");
} else {
LOG_INFO("GNSS module config saved!");
}
} else if (IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9)) {
if (gnssModel == GNSS_MODEL_UBLOX7) {
LOG_DEBUG("Set GPS+SBAS");
msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_7), _message_GNSS_7);
_serial_gps->write(UBXscratch, msglen);
} else { // 8,9
msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_8), _message_GNSS_8);
_serial_gps->write(UBXscratch, msglen);
}
if (getACK(0x06, 0x3e, 800) == GNSS_RESPONSE_NAK) {
// It's not critical if the module doesn't acknowledge this configuration.
LOG_DEBUG("reconfigure GNSS - defaults maintained. Is this module GPS-only?");
} else {
if (gnssModel == GNSS_MODEL_UBLOX7) {
LOG_INFO("GPS+SBAS configured");
} else { // 8,9
LOG_INFO("GPS+SBAS+GLONASS+Galileo configured");
}
// Documentation say, we need wait atleast 0.5s after reconfiguration of GNSS module, before sending next
// commands for the M8 it tends to be more... 1 sec should be enough ;>)
delay(1000);
}
// Disable Text Info messages //6,7,8,9
clearBuffer();
SEND_UBX_PACKET(0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
if (gnssModel == GNSS_MODEL_UBLOX8) { // 8
clearBuffer();
SEND_UBX_PACKET(0x06, 0x39, _message_JAM_8, "enable interference resistance", 500);
clearBuffer();
SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5_8, "configure NAVX5_8 settings", 500);
} else { // 6,7,9
SEND_UBX_PACKET(0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
SEND_UBX_PACKET(0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
}
// Turn off unwanted NMEA messages, set update rate
SEND_UBX_PACKET(0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
SEND_UBX_PACKET(0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
if (ublox_info.protocol_version >= 18) {
clearBuffer();
SEND_UBX_PACKET(0x06, 0x86, _message_PMS, "enable powersave for GPS", 500);
SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
// For M8 we want to enable NMEA vserion 4.10 so we can see the additional sats.
if (gnssModel == GNSS_MODEL_UBLOX8) {
clearBuffer();
SEND_UBX_PACKET(0x06, 0x17, _message_NMEA, "enable NMEA 4.10", 500);
}
} else {
SEND_UBX_PACKET(0x06, 0x11, _message_CFG_RXM_PSM, "enable powersave mode for GPS", 500);
SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
}
msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE);
_serial_gps->write(UBXscratch, msglen);
if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
LOG_WARN("Unable to save GNSS module config");
} else {
LOG_INFO("GNSS module configuration saved!");
}
} else if (gnssModel == GNSS_MODEL_UBLOX10) {
delay(1000);
clearBuffer();
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_NMEA_RAM, "disable NMEA messages in M10 RAM", 300);
delay(750);
clearBuffer();
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_NMEA_BBR, "disable NMEA messages in M10 BBR", 300);
delay(750);
clearBuffer();
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_RAM, "disable Info messages for M10 GPS RAM", 300);
delay(750);
// Next disable Info txt messages in BBR layer
clearBuffer();
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_BBR, "disable Info messages for M10 GPS BBR", 300);
delay(750);
// Do M10 configuration for Power Management.
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_PM_RAM, "enable powersave for M10 GPS RAM", 300);
delay(750);
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_PM_BBR, "enable powersave for M10 GPS BBR", 300);
delay(750);
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ITFM_RAM, "enable jam detection M10 GPS RAM", 300);
delay(750);
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ITFM_BBR, "enable jam detection M10 GPS BBR", 300);
delay(750);
// Here is where the init commands should go to do further M10 initialization.
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_SBAS_RAM, "disable SBAS M10 GPS RAM", 300);
delay(750); // will cause a receiver restart so wait a bit
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_DISABLE_SBAS_BBR, "disable SBAS M10 GPS BBR", 300);
delay(750); // will cause a receiver restart so wait a bit
// Done with initialization, Now enable wanted NMEA messages in BBR layer so they will survive a periodic
// sleep.
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ENABLE_NMEA_BBR, "enable messages for M10 GPS BBR", 300);
delay(750);
// Next enable wanted NMEA messages in RAM layer
SEND_UBX_PACKET(0x06, 0x8A, _message_VALSET_ENABLE_NMEA_RAM, "enable messages for M10 GPS RAM", 500);
delay(750);
// As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
// BBR will survive a restart, and power off for a while, but modules with small backup
// batteries or super caps will not retain the config for a long power off time.
msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE_10), _message_SAVE_10);
_serial_gps->write(UBXscratch, msglen);
if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) {
LOG_WARN("Unable to save GNSS module config");
} else {
LOG_INFO("GNSS module configuration saved!");
}
}
didSerialInit = true;
}
notifyDeepSleepObserver.observe(¬ifyDeepSleep);
return true;
}
GPS::~GPS()
{
// we really should unregister our sleep observer
notifyDeepSleepObserver.unobserve(¬ifyDeepSleep);
}
// Put the GPS hardware into a specified state
void GPS::setPowerState(GPSPowerState newState, uint32_t sleepTime)
{
// Update the stored GPSPowerstate, and create local copies
GPSPowerState oldState = powerState;
powerState = newState;
LOG_INFO("GPS power state move from %s to %s", getGPSPowerStateString(oldState), getGPSPowerStateString(newState));
#ifdef HELTEC_MESH_NODE_T114
if ((oldState == GPS_OFF || oldState == GPS_HARDSLEEP) && (newState != GPS_OFF && newState != GPS_HARDSLEEP)) {
_serial_gps->begin(serialSpeeds[speedSelect]);
} else if ((newState == GPS_OFF || newState == GPS_HARDSLEEP) && (oldState != GPS_OFF && oldState != GPS_HARDSLEEP)) {
_serial_gps->end();
}
#endif
switch (newState) {
case GPS_ACTIVE:
case GPS_IDLE:
if (oldState == GPS_ACTIVE || oldState == GPS_IDLE) // If hardware already awake, no changes needed
break;
if (oldState != GPS_ACTIVE && oldState != GPS_IDLE) // If hardware just waking now, clear buffer
clearBuffer();
powerMon->setState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
writePinEN(true); // Power (EN pin): on
setPowerPMU(true); // Power (PMU): on
writePinStandby(false); // Standby (pin): awake (not standby)
setPowerUBLOX(true); // Standby (UBLOX): awake
break;
case GPS_SOFTSLEEP:
powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
writePinEN(true); // Power (EN pin): on
setPowerPMU(true); // Power (PMU): on
writePinStandby(true); // Standby (pin): asleep (not awake)
setPowerUBLOX(false, sleepTime); // Standby (UBLOX): asleep, timed
break;
case GPS_HARDSLEEP:
powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
writePinEN(false); // Power (EN pin): off
setPowerPMU(false); // Power (PMU): off
writePinStandby(true); // Standby (pin): asleep (not awake)
setPowerUBLOX(false, sleepTime); // Standby (UBLOX): asleep, timed
#ifdef GNSS_AIROHA
if (config.position.gps_update_interval * 1000 >= GPS_FIX_HOLD_TIME * 2) {
digitalWrite(PIN_GPS_EN, LOW);
}
#endif
break;
case GPS_OFF:
assert(sleepTime == 0); // This is an indefinite sleep
powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
writePinEN(false); // Power (EN pin): off
setPowerPMU(false); // Power (PMU): off
writePinStandby(true); // Standby (pin): asleep
setPowerUBLOX(false, 0); // Standby (UBLOX): asleep, indefinitely
#ifdef GNSS_AIROHA
if (config.position.gps_update_interval * 1000 >= GPS_FIX_HOLD_TIME * 2) {
digitalWrite(PIN_GPS_EN, LOW);
}
#endif
break;
}
}
// Set power with EN pin, if relevant
void GPS::writePinEN(bool on)
{
// Abort: if conflict with Canned Messages when using Wisblock(?)
if ((HW_VENDOR == meshtastic_HardwareModel_RAK4631 || HW_VENDOR == meshtastic_HardwareModel_WISMESH_TAP) &&
(rotaryEncoderInterruptImpl1 || upDownInterruptImpl1))
return;
// Write and log
enablePin->set(on);
#ifdef GPS_DEBUG
LOG_DEBUG("Pin EN %s", on == HIGH ? "HI" : "LOW");
#endif
}
// Set the value of the STANDBY pin, if relevant
// true for standby state, false for awake
void GPS::writePinStandby(bool standby)
{
#ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76B, L76K and clones
// Determine the new value for the pin
// Normally: active HIGH for awake
#ifdef PIN_GPS_STANDBY_INVERTED
bool val = standby;
#else
bool val = !standby;
#endif
// Write and log
pinMode(PIN_GPS_STANDBY, OUTPUT);
digitalWrite(PIN_GPS_STANDBY, val);
#ifdef GPS_DEBUG
LOG_DEBUG("Pin STANDBY %s", val == HIGH ? "HI" : "LOW");
#endif
#endif
}
// Enable / Disable GPS with PMU, if present
void GPS::setPowerPMU(bool on)
{
// We only have PMUs on the T-Beam, and that board has a tiny battery to save GPS ephemera,
// so treat as a standby.
#ifdef HAS_PMU
// Abort: if no PMU
if (!pmu_found)
return;
// Abort: if PMU not initialized
if (!PMU)
return;
uint8_t model = PMU->getChipModel();
if (model == XPOWERS_AXP2101) {
if (HW_VENDOR == meshtastic_HardwareModel_TBEAM) {
// t-beam v1.2 GNSS power channel
on ? PMU->enablePowerOutput(XPOWERS_ALDO3) : PMU->disablePowerOutput(XPOWERS_ALDO3);
} else if (HW_VENDOR == meshtastic_HardwareModel_LILYGO_TBEAM_S3_CORE) {
// t-beam-s3-core GNSS power channel
on ? PMU->enablePowerOutput(XPOWERS_ALDO4) : PMU->disablePowerOutput(XPOWERS_ALDO4);
}
} else if (model == XPOWERS_AXP192) {
// t-beam v1.1 GNSS power channel
on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3);
}
#ifdef GPS_DEBUG
LOG_DEBUG("PMU %s", on ? "on" : "off");
#endif
#endif
}
// Set UBLOX power, if relevant
void GPS::setPowerUBLOX(bool on, uint32_t sleepMs)
{
// Abort: if not UBLOX hardware
if (!IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX6, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9, GNSS_MODEL_UBLOX10))
return;
// If waking
if (on) {
gps->_serial_gps->write(0xFF);
clearBuffer(); // This often returns old data, so drop it
}
// If putting to sleep
else {
uint8_t msglen;
// If we're being asked to sleep indefinitely, make *sure* we're awake first, to process the new sleep command
if (sleepMs == 0) {
setPowerUBLOX(true);
delay(500);
}
// Determine hardware version
if (gnssModel != GNSS_MODEL_UBLOX10) {
// Encode the sleep time in millis into the packet
for (int i = 0; i < 4; i++)
_message_PMREQ[0 + i] = sleepMs >> (i * 8);
// Record the message length
msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), _message_PMREQ);
} else {
// Encode the sleep time in millis into the packet
for (int i = 0; i < 4; i++)
_message_PMREQ_10[4 + i] = sleepMs >> (i * 8);
// Record the message length
msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), _message_PMREQ_10);
}
// Send the UBX packet
gps->_serial_gps->write(gps->UBXscratch, msglen);
#ifdef GPS_DEBUG
LOG_DEBUG("UBLOX: sleep for %dmS", sleepMs);
#endif
}
}
/// Record that we have a GPS
void GPS::setConnected()
{
if (!hasGPS) {
hasGPS = true;
shouldPublish = true;
}
}
// We want a GPS lock. Wake the hardware
void GPS::up()
{
scheduling.informSearching();
setPowerState(GPS_ACTIVE);
}
// We've got a GPS lock. Enter a low power state, potentially.
void GPS::down()
{
scheduling.informGotLock();
uint32_t predictedSearchDuration = scheduling.predictedSearchDurationMs();
uint32_t sleepTime = scheduling.msUntilNextSearch();
uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval);
LOG_DEBUG("%us until next search", sleepTime / 1000);
// If update interval less than 10 seconds, no attempt to sleep
if (updateInterval <= 10 * 1000UL || sleepTime == 0)
setPowerState(GPS_IDLE);
else {
// Check whether the GPS hardware is capable of GPS_SOFTSLEEP
// If not, fallback to GPS_HARDSLEEP instead
bool softsleepSupported = false;
// U-blox is supported via PMREQ
if (IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX6, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9, GNSS_MODEL_UBLOX10))
softsleepSupported = true;
#ifdef PIN_GPS_STANDBY // L76B, L76K and clones have a standby pin
softsleepSupported = true;
#endif
if (softsleepSupported) {
// How long does gps_update_interval need to be, for GPS_HARDSLEEP to become more efficient than
// GPS_SOFTSLEEP? Heuristic equation. A compromise manually fitted to power observations from U-blox NEO-6M
// and M10050 https://www.desmos.com/calculator/6gvjghoumr This is not particularly accurate, but probably an
// improvement over a single, fixed threshold
uint32_t hardsleepThreshold = (2750 * pow(predictedSearchDuration / 1000, 1.22));
LOG_DEBUG("gps_update_interval >= %us needed to justify hardsleep", hardsleepThreshold / 1000);
// If update interval too short: softsleep (if supported by hardware)
if (updateInterval < hardsleepThreshold) {
setPowerState(GPS_SOFTSLEEP, sleepTime);
return;
}
}
// If update interval long enough (or softsleep unsupported): hardsleep instead
setPowerState(GPS_HARDSLEEP, sleepTime);
}
}
void GPS::publishUpdate()
{
if (shouldPublish) {
shouldPublish = false;
// In debug logs, identify position by @timestamp:stage (stage 2 = publish)
LOG_DEBUG("Publish pos@%x:2, hasVal=%d, Sats=%d, GPSlock=%d", p.timestamp, hasValidLocation, p.sats_in_view, hasLock());
// Notify any status instances that are observing us
const meshtastic::GPSStatus status = meshtastic::GPSStatus(hasValidLocation, isConnected(), isPowerSaving(), p);
newStatus.notifyObservers(&status);
if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) {