-
Notifications
You must be signed in to change notification settings - Fork 7
/
Copy pathgps.py
147 lines (114 loc) · 2.8 KB
/
gps.py
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
# part of ysf_bridge
#
import crc
latitude = 0.0
longitude = 0.0
radio_code = 0x0
def GPS_dec(data, ft):
global latitude
global longitude
latitude = 0.0
longitude = 0.0
i = (ft - 5) * 10 - 2
valid = False
while (i >= 0):
if (data[i] == 0x03):
crcd = crc.addCRC(data, i+1)
if (crcd == data[i + 1]):
valid = True
break
i = i-1
if valid:
if ((data[1] == 0x22) and (data[2] == 0x62)):
#print('short')
GPS_string(data)
if ((data[1] == 0x47) and (data[2] == 0x64)):
#print('long')
GPS_string(data)
return valid
def GPS_string(data):
global latitude
global longitude
global radio_code
for i in range(6):
b = data[i+5] & 0xF0
if ((b != 0x50) and (b != 0x30)):
return False
tens = data[5] & 0x0F
units = data[6] & 0x0F
lat_deg = tens * 10 + units
if ((tens > 9) or (units > 9) or (lat_deg > 89)):
return False
tens = data[7] & 0x0F
units = data[8] & 0x0F
lat_min = tens * 10 + units
if ((tens > 9) or (units > 9) or (lat_min > 59)):
return False
tens = data[9] & 0x0F
units = data[10] & 0x0F
lat_min_frac = tens * 10 + units
if ((tens > 9) or (units > 10) or (lat_min_frac > 99)):
return False
b = data[8] & 0xF0
if (b == 0x50):
lat_dir = 1
else:
if (b == 0x30):
lat_dir = -1
else:
return False
b = data[9] & 0xF0
if (b == 0x50):
b = data[11]
if ((b >= 0x76) and (b <= 0x7F)):
lon_deg = b - 0x76
else:
if ((b >= 0x6C) and (b <= 0x75)):
lon_deg = 100 + (b - 0x6C)
else:
if ((b >= 0x26) and (b <= 0x6B)):
lon_deg = 110 + (b - 0x26)
else:
return False
else:
if (b == 0x30):
b = data[11]
if ((b >= 0x26) and (b <= 0x7F)):
lon_deg = 10 + (b - 0x26)
else:
return False
else:
return False
b = data[12]
if ((b >= 0x58) and (b <= 0x61)):
lon_min = b - 0x58
else:
if ((b >= 0x26) and (b <= 0x57)):
lon_min = 10 + (b - 0x26)
else:
return False
b = data[13]
if ((b >= 0x1C) and (b <= 0x7F)):
lon_min_frac = b - 0x1C
else:
return False
b = data[10] & 0xF0
if (b == 0x30):
lon_dir = 1
else:
if (b == 0x50):
lon_dir = -1
else:
return False
latitude = lat_deg + ((lat_min + (lat_min_frac * 0.01)) * (1.0 / 60.0))
latitude *= lat_dir
longitude = lon_deg + ((lon_min + (lon_min_frac * 0.01)) * (1.0 / 60.0));
longitude *= lon_dir;
radio_code = data[4]
return True
if __name__ == '__main__':
d70 = [7, 34, 97, 95, 43, 3, 23, 0, 0, 0]
d300 = [69, 34, 98, 95, 49, 84, 51, 85, 89, 50, 48, 38, 58, 83, 108, 32, 28, 32, 3, 110]
GPS_dec(d300, 7)
print(latitude)
print(longitude)