-
Notifications
You must be signed in to change notification settings - Fork 2
/
gps.c
5475 lines (4796 loc) · 182 KB
/
gps.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
/*
* Copyright (c) 2015 Thomas Roell. All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to
* deal with the Software without restriction, including without limitation the
* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
* sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimers.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimers in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of Thomas Roell, nor the names of its contributors
* may be used to endorse or promote products derived from this Software
* without specific prior written permission.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
* WITH THE SOFTWARE.
*/
#include <stdint.h>
#include <string.h>
#include "kitty.h"
#include "gps.h"
/************************************************************************************/
#define GPS_PORT_UART_SEND(_data, _count) tm4c123_uart_send((_data),(_count))
#define GPS_PORT_UART_CONFIGURE(_speed) tm4c123_uart_configure((_speed))
/************************************************************************************/
#define NMEA_TALKER_MASK_GPS 0x01
#define NMEA_TALKER_MASK_GLONASS 0x02
#define NMEA_TALKER_MASK_GNSS 0x04
#define NMEA_SENTENCE_MASK_GPGGA 0x00000001
#define NMEA_SENTENCE_MASK_GPGSA 0x00000002
#define NMEA_SENTENCE_MASK_GPGSV 0x00000004
#define NMEA_SENTENCE_MASK_GPRMC 0x00000008
#define NMEA_SENTENCE_MASK_GLGSA 0x00000010
#define NMEA_SENTENCE_MASK_GLGSV 0x00000020
#define NMEA_SENTENCE_MASK_SOLUTION 0x00008000
#define NMEA_FIELD_SEQUENCE_START 0
#define NMEA_FIELD_SEQUENCE_SKIP 1
#define NMEA_FIELD_SEQUENCE_GGA_TIME 2
#define NMEA_FIELD_SEQUENCE_GGA_LATITUDE 3
#define NMEA_FIELD_SEQUENCE_GGA_LATITUDE_NS 4
#define NMEA_FIELD_SEQUENCE_GGA_LONGITUDE 5
#define NMEA_FIELD_SEQUENCE_GGA_LONGITUDE_EW 6
#define NMEA_FIELD_SEQUENCE_GGA_QUALITY 7
#define NMEA_FIELD_SEQUENCE_GGA_NUMSV 8
#define NMEA_FIELD_SEQUENCE_GGA_HDOP 9
#define NMEA_FIELD_SEQUENCE_GGA_ALTITUDE 10
#define NMEA_FIELD_SEQUENCE_GGA_ALTITUDE_UNIT 11
#define NMEA_FIELD_SEQUENCE_GGA_SEPARATION 12
#define NMEA_FIELD_SEQUENCE_GGA_SEPARATION_UNIT 13
#define NMEA_FIELD_SEQUENCE_GGA_DIFFERENTIAL_AGE 14
#define NMEA_FIELD_SEQUENCE_GGA_DIFFERENTIAL_STATION 15
#define NMEA_FIELD_SEQUENCE_GSA_OPERATION 16
#define NMEA_FIELD_SEQUENCE_GSA_NAVIGATION 17
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_1 18
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_2 19
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_3 20
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_4 21
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_5 22
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_6 23
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_7 24
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_8 25
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_9 26
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_10 27
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_11 28
#define NMEA_FIELD_SEQUENCE_GSA_SV_USED_PRN_12 29
#define NMEA_FIELD_SEQUENCE_GSA_PDOP 30
#define NMEA_FIELD_SEQUENCE_GSA_HDOP 31
#define NMEA_FIELD_SEQUENCE_GSA_VDOP 32
#define NMEA_FIELD_SEQUENCE_GSV_SENTENCES 33
#define NMEA_FIELD_SEQUENCE_GSV_CURRENT 34
#define NMEA_FIELD_SEQUENCE_GSV_SV_IN_VIEW_COUNT 35
#define NMEA_FIELD_SEQUENCE_GSV_SV_IN_VIEW_ID 36
#define NMEA_FIELD_SEQUENCE_GSV_SV_IN_VIEW_ELEV 37
#define NMEA_FIELD_SEQUENCE_GSV_SV_IN_VIEW_AZIM 38
#define NMEA_FIELD_SEQUENCE_GSV_SV_IN_VIEW_SNR 39
#define NMEA_FIELD_SEQUENCE_RMC_TIME 40
#define NMEA_FIELD_SEQUENCE_RMC_STATUS 41
#define NMEA_FIELD_SEQUENCE_RMC_LATITUDE 42
#define NMEA_FIELD_SEQUENCE_RMC_LATITUDE_NS 43
#define NMEA_FIELD_SEQUENCE_RMC_LONGITUDE 44
#define NMEA_FIELD_SEQUENCE_RMC_LONGITUDE_EW 45
#define NMEA_FIELD_SEQUENCE_RMC_SPEED 46
#define NMEA_FIELD_SEQUENCE_RMC_COURSE 47
#define NMEA_FIELD_SEQUENCE_RMC_DATE 48
#define NMEA_FIELD_SEQUENCE_RMC_VARIATION 49
#define NMEA_FIELD_SEQUENCE_RMC_VARIATION_UNIT 50
#define NMEA_FIELD_SEQUENCE_RMC_MODE 51
#define NMEA_FIELD_SEQUENCE_GGA_END 52
#define NMEA_FIELD_SEQUENCE_GSA_END 53
#define NMEA_FIELD_SEQUENCE_GSV_END 54
#define NMEA_FIELD_SEQUENCE_RMC_END 55
#if (GPS_CONFIG_MEDIATEK >= 1)
#define NMEA_FIELD_SEQUENCE_PMTK001_COMMAND 56
#define NMEA_FIELD_SEQUENCE_PMTK001_STATUS 57
#define NMEA_FIELD_SEQUENCE_PMTK010_STATUS 58
#define NMEA_FIELD_SEQUENCE_PMTK869_MODE 59
#define NMEA_FIELD_SEQUENCE_PMTK869_STATUS 60
#define NMEA_FIELD_SEQUENCE_PMTK001_END 61
#define NMEA_FIELD_SEQUENCE_PMTK010_END 62
#define NMEA_FIELD_SEQUENCE_PMTK869_END 63
#endif /* GPS_CONFIG_MEDIATEK >= 1 */
#define NMEA_FIELD_MASK_TIME 0x0001
#define NMEA_FIELD_MASK_POSITION 0x0002
#define NMEA_FIELD_MASK_ALTITUDE 0x0004
#define NMEA_FIELD_MASK_SPEED 0x0008
#define NMEA_FIELD_MASK_COURSE 0x0010
#define NMEA_FIELD_MASK_HDOP 0x0020
#define NMEA_FIELD_MASK_VDOP 0x0040
#define NMEA_OPERATION_MANUAL 0
#define NMEA_OPERATION_AUTOMATIC 1
#define NMEA_NAVIGATION_NONE 0
#define NMEA_NAVIGATION_2D 1
#define NMEA_NAVIGATION_3D 2
#define NMEA_STATUS_RECEIVER_WARNING 0
#define NMEA_STATUS_DATA_VALID 1
typedef struct _nmea_context_t {
uint8_t talker; /* NMEA TALKER */
uint8_t sequence; /* FIELD SEQUENCE */
uint16_t mask; /* FIELD MASK */
uint8_t navigation; /* GSA */
uint8_t status; /* RMC */
#if (GPS_CONFIG_SATELLITES >= 1)
uint8_t sv_in_view_sentences; /* GSV */
uint8_t sv_in_view_count; /* GSV */
uint8_t sv_in_view_index; /* GSV */
uint8_t sv_used_count; /* GSA */
uint32_t sv_used_mask[3]; /* GSA */
#endif /* GPS_CONFIG_SATELLITES >= 1 */
#if (GPS_CONFIG_MEDIATEK >= 1)
uint16_t mtk_command;
uint16_t mtk_status;
#endif /* GPS_CONFIG_MEDIATEK >= 1 */
} nmea_context_t;
/************************************************************************************/
#define SRF_MESSAGE_MASK_MEASURED_TRACKER 0x00010000
#define SRF_MESSAGE_MASK_CLOCK_STATUS 0x00020000
#define SRF_MESSAGE_MASK_GEODETIC_NAVIGATION 0x00040000
#define SRF_MESSAGE_MASK_SOLUTION 0x00008000
typedef struct _srf_context_t {
uint32_t week;
uint32_t tow;
#if (GPS_CONFIG_SATELLITES >= 1)
uint32_t sv_used_mask;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
} srf_context_t;
/************************************************************************************/
#define UBX_MESSAGE_MASK_NAV_CLOCK 0x00010000
#define UBX_MESSAGE_MASK_NAV_DOP 0x00020000
#define UBX_MESSAGE_MASK_NAV_POSLLH 0x00040000
#define UBX_MESSAGE_MASK_NAV_PVT 0x00080000
#define UBX_MESSAGE_MASK_NAV_SOL 0x00100000
#define UBX_MESSAGE_MASK_NAV_SVINFO 0x00200000
#define UBX_MESSAGE_MASK_NAV_TIMEGPS 0x00400000
#define UBX_MESSAGE_MASK_NAV_TIMEUTC 0x00800000
#define UBX_MESSAGE_MASK_NAV_VELNED 0x01000000
#define UBX_MESSAGE_MASK_SOLUTION 0x00008000
typedef struct _ubx_context_t {
uint32_t week;
uint32_t tow;
uint32_t itow;
} ubx_context_t;
/************************************************************************************/
#define GPS_PROTOCOL_MASK_NMEA 0x01
#define GPS_PROTOCOL_MASK_SRF 0x02
#define GPS_PROTOCOL_MASK_UBX 0x04
#define GPS_STATE_START 0
#define GPS_STATE_NMEA_PAYLOAD 1
#define GPS_STATE_NMEA_CHECKSUM_1 2
#define GPS_STATE_NMEA_CHECKSUM_2 3
#define GPS_STATE_NMEA_END_CR 4
#define GPS_STATE_NMEA_END_LF 5
#define GPS_STATE_SRF_START_2 6
#define GPS_STATE_SRF_LENGTH_1 7
#define GPS_STATE_SRF_LENGTH_2 8
#define GPS_STATE_SRF_MESSAGE 9
#define GPS_STATE_SRF_PAYLOAD 10
#define GPS_STATE_SRF_CHECKSUM_1 11
#define GPS_STATE_SRF_CHECKSUM_2 12
#define GPS_STATE_SRF_END_1 13
#define GPS_STATE_SRF_END_2 14
#define GPS_STATE_UBX_SYNC_2 15
#define GPS_STATE_UBX_MESSAGE_1 16
#define GPS_STATE_UBX_MESSAGE_2 17
#define GPS_STATE_UBX_LENGTH_1 18
#define GPS_STATE_UBX_LENGTH_2 19
#define GPS_STATE_UBX_PAYLOAD 20
#define GPS_STATE_UBX_CK_A 21
#define GPS_STATE_UBX_CK_B 22
#define GPS_INIT_DONE 0
#define GPS_INIT_MTK_BAUD_RATE 1
#define GPS_INIT_MTK_STARTUP 2
#define GPS_INIT_MTK_INIT_TABLE 128
#define GPS_INIT_SRF_BAUD_RATE 3
#define GPS_INIT_SRF_STARTUP 4
#define GPS_INIT_SRF_INIT_TABLE 128
#define GPS_INIT_UBX_BAUD_RATE 5
#define GPS_INIT_UBX_STARTUP 6
#define GPS_INIT_UBX_INIT_TABLE 128
#define GPS_RESPONSE_NONE 0
#define GPS_RESPONSE_ACK 1
#define GPS_RESPONSE_NACK 2
#define GPS_RESPONSE_STARTUP 3
#define GPS_RESPONSE_NMEA_SENTENCE 4
#define GPS_RESPONSE_SRF_MESSAGE 5
#define GPS_RESPONSE_UBX_MESSAGE 6
#define GPS_DATA_SIZE 96
typedef struct _gps_device_t {
uint8_t reset;
uint8_t protocol;
uint8_t init;
uint8_t state;
uint32_t seen;
uint32_t expected;
uint16_t checksum;
uint8_t ck_a;
uint8_t ck_b;
uint16_t count;
uint16_t length;
uint16_t first;
uint16_t last;
uint16_t message;
uint8_t data[GPS_DATA_SIZE];
#if (GPS_CONFIG_SIRF == 0) && (GPS_CONFIG_UBLOX == 0)
nmea_context_t nmea;
#endif /* GPS_CONFIG_SIRF == 0 && GPS_CONFIG_UBLOX == 0 */
#if (GPS_CONFIG_SIRF >= 1)
srf_context_t srf;
#endif /* GPS_CONFIG_SIRF >= 1 */
#if (GPS_CONFIG_UBLOX >= 1)
ubx_context_t ubx;
#endif /* GPS_CONFIG_UBLOX >= 1 */
gps_location_t location;
#if (GPS_CONFIG_SATELLITES >= 1)
gps_satellites_t satellites;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
volatile uint32_t command;
uint64_t tick; /* CPU time @ last solution */
utc_time_t pps_time; /* UTC time @ last PPS pulse */
uint8_t pps_correction; /* GPS/UTC offset @ last PPS pulse */
volatile uint8_t pps_pending; /* pending PPS pulse */
volatile uint64_t pps_reference; /* CPU ticks @ pending PPS pulse */
uint64_t pps_tick; /* CPU ticks @ accepted PPS pulse */
uint32_t pps_period; /* ticks per second */
gps_ini_callback_t ini_callback;
gps_location_callback_t location_callback;
gps_satellites_callback_t satellites_callback;
} gps_device_t;
static gps_device_t gps_device;
#if (GPS_CONFIG_MEDIATEK >= 1)
static void mtk_initialize(gps_device_t *device, unsigned int response, uint32_t command);
#endif /* GPS_CONFIG_MEDIATEK >= 1 */
#if (GPS_CONFIG_SIRF >= 1)
static void srf_initialize(gps_device_t *device, unsigned int response, uint32_t command);
#endif /* GPS_CONFIG_UBLOX >= 1 */
#if (GPS_CONFIG_UBLOX >= 1)
static void ubx_initialize(gps_device_t *device, unsigned int response, uint32_t command);
#endif /* GPS_CONFIG_UBLOX >= 1 */
/************************************************************************************/
static const uint16_t utc_days_since_month[2][12] = {
{ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, },
{ 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, },
};
static int utc_diff_time(const utc_time_t *t0, uint32_t offset0, const utc_time_t *t1, uint32_t offset1)
{
/* The difference between t1 time and the current time is mostly a few seconds,
* but the arithmetic might overflow, so be careful to incrementally compute the difference
* in days, minutes, seconds, milliseconds.
*/
return (((((((((((t0->year * 365 + (1 + ((t0->year -1) / 4))) +
utc_days_since_month[(t0->year & 3) == 0][t0->month -1] +
(t0->day -1)) -
((t1->year * 365 + (1 + ((t1->year -1) / 4))) +
utc_days_since_month[(t1->year & 3) == 0][t1->month -1] +
(t1->day -1))) * 24) +
t0->hour -
t1->hour) * 60) +
t0->min -
t1->min) * 60) +
offset0 -
offset1) * 1000) +
t0->sec -
t1->sec);
}
/* utc_offset_time:
*
* Compute the UTC offset (or GPS leap second) by computing the elapsed UTC seconds
* since 06/01/1980, and subtract that from week/tow (which is ahead by said
* leap seconds).
*/
static int utc_offset_time(const utc_time_t *time, uint16_t week, uint32_t tow)
{
return ((((uint32_t)week * 604800) + ((tow + 5) / 1000)) -
(((((((((time->year * 365 + (1 + ((time->year -1) / 4))) +
utc_days_since_month[(time->year & 3) == 0][time->month -1] +
(time->day -1)) -
((1980 * 365 + (1 + ((1980 -1) / 4))) +
(6 -1))) * 24) +
time->hour) * 60) +
time->min ) * 60) +
((time->sec + 5) / 1000)));
}
/************************************************************************************/
static void gps_configure(gps_device_t *device, uint32_t speed)
{
GPS_PORT_UART_CONFIGURE(speed);
}
static void gps_send(gps_device_t *device, uint32_t command, const uint8_t *data, unsigned int count)
{
device->command = command;
GPS_PORT_UART_SEND(data, count);
}
static void gps_location(gps_device_t *device)
{
int delta;
unsigned int sec, msec;
if ((device->location.type != GPS_LOCATION_TYPE_NONE) && (device->pps_time.year == 0))
{
device->pps_pending = 1;
device->pps_reference = 0;
}
switch (device->location.type) {
case GPS_LOCATION_TYPE_NONE:
/* If there was a reference PPS plus and a fix, then allow the
* UTC time to freewheel.
*/
if (device->pps_time.year != 0)
{
device->location.mask &= (GPS_LOCATION_MASK_TIME |
GPS_LOCATION_MASK_CORRECTION |
GPS_LOCATION_MASK_CLOCK);
}
else
{
device->location.mask = 0;
}
device->location.numsv = 0;
device->location.quality = GPS_LOCATION_QUALITY_NONE;
break;
case GPS_LOCATION_TYPE_TIME:
device->location.mask &= (GPS_LOCATION_MASK_TIME |
GPS_LOCATION_MASK_CORRECTION |
GPS_LOCATION_MASK_CLOCK |
GPS_LOCATION_MASK_TDOP);
device->location.quality = GPS_LOCATION_QUALITY_NONE;
break;
case GPS_LOCATION_TYPE_2D:
device->location.mask &= (GPS_LOCATION_MASK_TIME |
GPS_LOCATION_MASK_CORRECTION |
GPS_LOCATION_MASK_POSITION |
GPS_LOCATION_MASK_SPEED |
GPS_LOCATION_MASK_COURSE |
GPS_LOCATION_MASK_CLOCK |
GPS_LOCATION_MASK_EHPE |
GPS_LOCATION_MASK_HDOP |
GPS_LOCATION_MASK_TDOP);
break;
case GPS_LOCATION_TYPE_3D:
break;
default:
break;
}
if (device->location.mask & GPS_LOCATION_MASK_TIME)
{
if (!(device->location.mask & GPS_LOCATION_MASK_CORRECTION))
{
device->location.correction = 0;
}
sec = device->location.time.sec / 1000;
msec = device->location.time.sec - (sec * 1000);
// Fake out PPS pending
if ((device->location.type != GPS_LOCATION_TYPE_NONE) && device->pps_pending && (msec == 0))
{
device->pps_time = device->location.time;
device->pps_correction = device->location.correction;
device->pps_tick = device->pps_reference;
device->pps_pending = 0;
device->tick = device->pps_tick;
device->location.mask |= GPS_LOCATION_MASK_PPS;
}
else
{
if (msec == 0)
{
device->pps_pending = 0;
}
if (device->pps_time.year != 0)
{
/* Here the time stamping has to be done along the UTC time. The problem
* with that is that leap seconds can pop up, mainly not because the rare response
* of an actual leap second, but because the reciever starts off with a build
* in leap second that is only later on resolved via satellite. So the code has
* to find out about that event by looking at the UTC time going backwards in
* case we don't know the currently used leap second offset. That is all fine and
* dandy if every single solution is visible on the wire, and no data ever gets lost.
*/
delta = utc_diff_time(&device->location.time, device->location.correction, &device->pps_time, device->pps_correction);
while (delta <= 0)
{
delta += 1000;
}
sec = (uint32_t)delta / 1000;
msec = (uint32_t)delta - (sec * 1000);
device->tick = device->pps_tick + ((uint64_t)sec * device->pps_period) + ((msec / (1000 / GPS_CONFIG_RATE)) * (device->pps_period / GPS_CONFIG_RATE));
}
else
{
device->tick = 0;
}
}
}
else
{
device->tick = 0;
device->location.time.year = 0;
device->location.time.month = 0;
device->location.time.day = 0;
device->location.time.hour = 0;
device->location.time.min = 0;
device->location.time.sec = 0;
device->location.correction = 0;
device->location.mask = 0;
device->location.numsv = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_POSITION))
{
device->location.latitude = 0;
device->location.longitude = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_ALTITUDE))
{
device->location.altitude = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_SPEED))
{
device->location.speed = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_COURSE))
{
device->location.course = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_CLIMB))
{
device->location.climb = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_CLOCK))
{
device->location.bias = 0;
device->location.drift = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_EHPE))
{
device->location.ehpe = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_EVPE))
{
device->location.evpe = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_ESVE))
{
device->location.esve = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_ECVE))
{
device->location.ecve = 0;
}
if (!(device->location.mask & GPS_LOCATION_MASK_HDOP))
{
device->location.hdop = 9999;
}
if (!(device->location.mask & GPS_LOCATION_MASK_VDOP))
{
device->location.vdop = 9999;
}
if (!(device->location.mask & GPS_LOCATION_MASK_TDOP))
{
device->location.tdop = 9999;
}
if (device->location_callback)
{
(*device->location_callback)(device->tick, &device->location);
}
#if (GPS_CONFIG_SATELLITES >= 1)
device->satellites.time = device->location.time;
device->satellites.correction = device->location.correction;
device->satellites.mask |= (device->location.mask & (GPS_LOCATION_MASK_TIME | GPS_LOCATION_MASK_CORRECTION));
#endif /* GPS_CONFIG_SATELLITES >= 1 */
device->location.type = 0;
device->location.mask = 0;
}
#if (GPS_CONFIG_SATELLITES >= 1)
static void gps_satellites(gps_device_t *device)
{
if (device->satellites.count > GPS_SATELLITES_COUNT_MAX)
{
device->satellites.count = GPS_SATELLITES_COUNT_MAX;
}
if (device->satellites_callback)
{
(*device->satellites_callback)(device->tick, &device->satellites);
}
device->satellites.mask = 0;
}
#endif /* GPS_CONFIG_SATELLITES >= 1 */
/************************************************************************************/
static const char nmea_hex_ascii[16] = "0123456789ABCDEF";
static void nmea_send(gps_device_t *device, const char *data)
{
gps_send(&gps_device, ~0l, (const uint8_t*)data, strlen(data));
}
#if (GPS_CONFIG_SIRF == 0) && (GPS_CONFIG_UBLOX == 0)
static const uint32_t nmea_scale[10] = {
1,
10,
100,
1000,
10000,
100000,
1000000,
10000000,
100000000,
1000000000,
};
static int nmea_same_time(const utc_time_t *t0, const utc_time_t *t1)
{
return ((t0->sec == t1->sec) &&
(t0->min == t1->min) &&
(t0->hour == t1->hour));
}
static int nmea_parse_time(const uint8_t *data, utc_time_t *p_time)
{
uint32_t hour, min, sec, msec, digits;
if ((data[0] >= '0') && (data[0] <= '9') && (data[1] >= '0') && (data[1] <= '9'))
{
hour = (data[0] - '0') * 10 + (data[1] - '0');
data += 2;
if ((hour < 24) && (data[0] >= '0') && (data[0] <= '9') && (data[1] >= '0') && (data[1] <= '9'))
{
min = (data[0] - '0') * 10 + (data[1] - '0');
data += 2;
if ((min < 60) && (data[0] >= '0') && (data[0] <= '9') && (data[1] >= '0') && (data[1] <= '9'))
{
sec = (data[0] - '0') * 10 + (data[1] - '0');
data += 2;
/* A 60 is legal here for leap seconds.
*/
if (sec <= 60)
{
msec = 0;
if (data[0] == '.')
{
digits = 0;
data++;
while ((data[0] >= '0') && (data[0] <= '9'))
{
msec = msec * 10 + (data[0] - '0');
digits++;
data++;
}
if (data[0] == '\0')
{
if (digits < 3)
{
msec = msec * nmea_scale[3 - digits];
}
else if (digits > 3)
{
msec = (msec + (nmea_scale[digits - 3] / 2)) / nmea_scale[digits - 3];
}
}
}
if (data[0] == '\0')
{
p_time->hour = hour;
p_time->min = min;
p_time->sec = (sec * 1000) + msec;
return 1;
}
}
}
}
}
return 0;
}
static int nmea_parse_unsigned(const uint8_t *data, uint32_t *p_unsigned)
{
uint32_t integer;
integer = 0;
while ((data[0] >= '0') && (data[0] <= '9'))
{
integer = integer * 10 + (data[0] - '0');
data++;
}
if (data[0] == '\0')
{
*p_unsigned = integer;
return 1;
}
return 0;
}
static int nmea_parse_fixed(const uint8_t *data, uint32_t *p_fixed, uint32_t scale)
{
uint32_t integer, fraction, digits;
integer = 0;
while ((data[0] >= '0') && (data[0] <= '9'))
{
integer = integer * 10 + (data[0] - '0');
data++;
}
fraction = 0;
if (data[0] == '.')
{
digits = 0;
data++;
while ((data[0] >= '0') && (data[0] <= '9'))
{
fraction = fraction * 10 + (data[0] - '0');
digits++;
data++;
}
if (data[0] == '\0')
{
if (digits < scale)
{
fraction = fraction * nmea_scale[scale - digits];
}
else if (digits > scale)
{
fraction = (fraction + (nmea_scale[digits - scale] / 2)) / nmea_scale[digits - scale];
}
}
}
if (data[0] == '\0')
{
*p_fixed = integer * nmea_scale[scale] + fraction;
return 1;
}
return 0;
}
static int nmea_parse_latitude(const uint8_t *data, uint32_t *p_latitude)
{
uint32_t degrees, minutes;
if ((data[0] >= '0') && (data[0] <= '9') && (data[1] >= '0') && (data[1] <= '9'))
{
degrees = (data[0] - '0') * 10 + (data[1] - '0');
data += 2;
if ((degrees < 90) && (data[0] != '\0') && nmea_parse_fixed(data, &minutes, 7))
{
if (minutes < 600000000)
{
*p_latitude = (uint32_t)(degrees * 10000000u) + (uint32_t)((minutes + 30) / 60);
return 1;
}
}
}
return 0;
}
static int nmea_parse_longitude(const uint8_t *data, uint32_t *p_longitude)
{
uint32_t degrees, minutes;
if ((data[0] >= '0') && (data[0] <= '9') && (data[1] >= '0') && (data[1] <= '9') && (data[2] >= '0') && (data[2] <= '9'))
{
degrees = (data[0] - '0') * 100 + (data[1] - '0') * 10 + (data[2] - '0');
data += 3;
if ((degrees < 180) && (data[0] != '\0') && nmea_parse_fixed(data, &minutes, 7))
{
if (minutes < 600000000)
{
*p_longitude = (uint32_t)(degrees * 10000000u) + (uint32_t)((minutes + 30) / 60);
return 1;
}
}
}
return 0;
}
static void nmea_start_sentence(gps_device_t *device)
{
nmea_context_t *context = &device->nmea;
switch (context->sequence) {
case NMEA_FIELD_SEQUENCE_GGA_END:
break;
case NMEA_FIELD_SEQUENCE_GSA_END:
#if (GPS_CONFIG_SATELLITES >= 1)
context->sv_used_count = 0;
context->sv_used_mask[0] = 0;
context->sv_used_mask[1] = 0;
context->sv_used_mask[2] = 0;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
break;
case NMEA_FIELD_SEQUENCE_GSV_END:
#if (GPS_CONFIG_SATELLITES >= 1)
context->sv_in_view_sentences = 0;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
break;
case NMEA_FIELD_SEQUENCE_RMC_END:
break;
#if (GPS_CONFIG_MEDIATEK >= 1)
case NMEA_FIELD_SEQUENCE_PMTK001_END:
break;
case NMEA_FIELD_SEQUENCE_PMTK010_END:
break;
case NMEA_FIELD_SEQUENCE_PMTK869_END:
break;
#endif /* GPS_CONFIG_MEDIATEK >= 1 */
default:
break;
}
context->sequence = NMEA_FIELD_SEQUENCE_START;
}
static void nmea_parse_sentence(gps_device_t *device, const uint8_t *data, unsigned int length)
{
nmea_context_t *context = &device->nmea;
uint32_t sequence, sequence_next;
uint32_t latitude, longitude, altitude, separation, speed, course;
uint32_t quality, numsv, hdop, vdop, date;
uint32_t command, status;
utc_time_t time;
#if (GPS_CONFIG_SATELLITES >= 1)
uint32_t count, index, sentences, current, svid, elev, azim, snr;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
sequence = context->sequence;
sequence_next = sequence +1;
switch (sequence) {
case NMEA_FIELD_SEQUENCE_START:
sequence_next = NMEA_FIELD_SEQUENCE_SKIP;
if (data[0] == 'P')
{
#if (GPS_CONFIG_MEDIATEK >= 1)
if (!strcmp((const char*)data, "PMTK001"))
{
sequence_next = NMEA_FIELD_SEQUENCE_PMTK001_COMMAND;
}
else if (!strcmp((const char*)data, "PMTK010"))
{
sequence_next = NMEA_FIELD_SEQUENCE_PMTK010_STATUS;
}
else if (!strcmp((const char*)data, "PMTK869"))
{
sequence_next = NMEA_FIELD_SEQUENCE_PMTK869_MODE;
}
#endif /* GPS_CONFIG_MEDIATEK */
}
else
{
context->talker = 0;
if (data[0] == 'G')
{
if (data[1] == 'P')
{
context->talker = NMEA_TALKER_MASK_GPS;
}
else if (data[1] == 'L')
{
context->talker = NMEA_TALKER_MASK_GLONASS;
}
else if (data[1] == 'N')
{
context->talker = NMEA_TALKER_MASK_GNSS;
}
}
/* --GSA is the switch detector in NMEA 0183. If it's GPGSA or GLGSA, then the system
* is set up as single GPS/GLONASS system, and we'd see only either a GPGSV or GLGSV
* later on. If it's a GNGSA, then another GNGSA will follow, one for GPS and one for
* GLONASS. The constellation will be reported as GPGSV and GLGSV.
*/
if ((context->talker & (NMEA_TALKER_MASK_GPS | NMEA_TALKER_MASK_GLONASS | NMEA_TALKER_MASK_GNSS)) && !strcmp((const char*)(data+2), "GSA"))
{
if (device->seen & NMEA_SENTENCE_MASK_GPGGA)
{
sequence_next = NMEA_FIELD_SEQUENCE_GSA_OPERATION;
context->mask = (NMEA_FIELD_MASK_VDOP);
}
}
#if (GPS_CONFIG_SATELLITES >= 1)
/* -- GSV is used to report the satellite constellation with either GPGSV or GLGSV.
* GNGSV is not legal.
*/
else if ((context->talker & (NMEA_TALKER_MASK_GPS | NMEA_TALKER_MASK_GLONASS)) && !strcmp((const char*)(data+2), "GSV"))
{
if (device->seen & (NMEA_SENTENCE_MASK_GPGGA | NMEA_SENTENCE_MASK_SOLUTION))
{
sequence_next = NMEA_FIELD_SEQUENCE_GSV_SENTENCES;
}
}
#endif /* GPS_CONFIG_SATELLITES >= 1 */
/* According to the standard, if a receiver is supporting GPS only, the talker would
* be "GP". If it's a GLONASS only system, or a combined GPS+GLONASS system, then
* the talker should be "GN". However some GLONASS only systems use a "GL" talker,
* and quite a few GPS_GLONASS systems mix "GP" and "GN" randomly. So just make
* sure that a valid talker is used, and simply ignore it. The system detection
* is done via --GSA anyway.
*/
else if (context->talker)
{
if (!strcmp((const char*)(data+2), "GGA"))
{
sequence_next = NMEA_FIELD_SEQUENCE_GGA_TIME;
/* GSA/GSV are subsequent to a GGA */
device->seen &= ~(NMEA_SENTENCE_MASK_GPGGA |
NMEA_SENTENCE_MASK_GPGSA |
NMEA_SENTENCE_MASK_GPGSV |
NMEA_SENTENCE_MASK_GLGSA |
NMEA_SENTENCE_MASK_GLGSV |
NMEA_SENTENCE_MASK_SOLUTION);
context->mask = (NMEA_FIELD_MASK_POSITION |
NMEA_FIELD_MASK_ALTITUDE |
NMEA_FIELD_MASK_HDOP);
#if (GPS_CONFIG_SATELLITES >= 1)
context->sv_in_view_sentences = 0;
context->sv_used_count = 0;
context->sv_used_mask[0] = 0;
context->sv_used_mask[1] = 0;
context->sv_used_mask[2] = 0;
device->satellites.count = 0;
#endif /* GPS_CONFIG_SATELLITES >= 1 */
}
else if (!strcmp((const char*)(data+2), "RMC"))
{
sequence_next = NMEA_FIELD_SEQUENCE_RMC_TIME;
device->seen &= ~(NMEA_SENTENCE_MASK_GPRMC | NMEA_SENTENCE_MASK_SOLUTION);
context->mask = (NMEA_FIELD_MASK_TIME |
NMEA_FIELD_MASK_SPEED |
NMEA_FIELD_MASK_COURSE);
}
}
}
break;
case NMEA_FIELD_SEQUENCE_SKIP:
sequence_next = NMEA_FIELD_SEQUENCE_SKIP;
break;
case NMEA_FIELD_SEQUENCE_GGA_TIME:
case NMEA_FIELD_SEQUENCE_RMC_TIME: