1 #include "gps.h"
2 
decode_gps(uint8_t * data,coordinate * result)3 bool decode_gps(uint8_t* data, coordinate* result) {
4     if ((data[0] & 0x0F) > 9) return false;
5     if ((data[1] & 0x0F) > 9) return false;
6     if ((data[2] & 0x0F) > 9) return false;
7     if ((data[3] & 0x0F) > 9) return false;
8     if ((data[4] & 0x0F) > 9) return false;
9     if ((data[5] & 0x0F) > 9) return false;
10 
11     float lat = (data[0] & 0x0F) * 10 +
12                 (data[1] & 0x0F) +
13         (float) (data[2] & 0x0F) / 6 +
14         (float) (data[3] & 0x0F) / 60 +
15         (float) (data[4] & 0x0F) / 600 +
16         (float) (data[5] & 0x0F) / 6000;
17 
18     uint8_t direction = (data[3] & 0xF0);
19     if (direction == 0x50) {
20         // northern hemisphere. nothing to do here, so pass
21     } else if (direction == 0x30) {
22         // southern hemisphere
23         lat *= -1;
24     } else {
25         return false;
26     }
27 
28     float lon;
29 
30     uint8_t b = data[4] & 0xF0;
31     uint8_t c = data[6];
32     if (b == 0x50) {
33         if (c >= 0x76 && c < 0x7f) {
34             lon = c - 0x76;
35         } else if (c >= 0x6c && c < 0x75) {
36             lon = 100 + (c - 0x6c);
37         } else if (c >= 0x26 && c < 0x6b) {
38             lon = 110 + (c - 0x26);
39         } else {
40             return false;
41         }
42     } else if (b == 0x30) {
43         if (c >= 0x26 && c < 0x7f) {
44             lon = 10 + (c - 0x26);
45         } else {
46             return false;
47         }
48     }
49 
50     b = data[7];
51     if (b > 0x58 && b <= 0x61) {
52         lon += (float) (b - 0x58) / 60;
53     } else if (b >= 0x26 && b <= 0x57) {
54         lon += (float) (10 + (b - 0x26)) / 60;
55     } else {
56         return false;
57     }
58 
59     b = data[8];
60     if (b >= 0x1c && b < 0x7f) {
61         lon += (float) (b - 0x1c) / 6000;
62     } else {
63         return false;
64     }
65 
66     direction = (data[5] & 0xF0);
67     if (direction == 0x50) {
68         // western hemisphere
69         lon *= -1;
70     } else if (direction == 0x30) {
71         // eastern hemisphere. nothing to do here, so pass
72     } else {
73         return false;
74     }
75 
76 
77 
78     if (lat > 90 || lat < -90) return false;
79     if (lon > 180 || lon < -180) return false;
80 
81     result->lat = lat;
82     result->lon = lon;
83 
84     return true;
85 }
86