-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathGSOF.py
173 lines (164 loc) · 8.51 KB
/
GSOF.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
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
from struct import unpack
import socket
import math
import argparse
import json
import functools
import logging
logger = logging.getLogger(__name__)
## GSOF CLASS
__author__ = "Henry T. Berglund"
class Gsof(object):
""" Class to connect to tcp port and parse GSOF messages """
def __init__(self, sock=None):
if sock is None:
self.sock = socket.socket(
socket.AF_INET, socket.SOCK_STREAM)
else:
self.sock = sock
self.msg_dict = {}
self.msg_bytes = None
self.checksum = None
self.rec_dict = {}
def connect(self, host, port):
self.sock.connect((host, port))
logger.info("Connected to %s on port %i", host, port)
def disconnect(self):
self.sock.close()
logger.info("Disconnected from host")
def get_message_header(self):
data = self.sock.recv(7)
msg_field_names = ('STX', 'STATUS', 'TYPE', 'LENGTH',
'T_NUM', 'PAGE_INDEX', 'MAX_PAGE_INDEX')
self.msg_dict = dict(zip(msg_field_names, unpack('>7B', data)))
try:
self.msg_bytes = self.sock.recv(self.msg_dict['LENGTH'] - 3)
(checksum, etx) = unpack('>2B', self.sock.recv(2))
except:
logger.debug("There has been an incomplete header but I will raise an error")
raise RuntimeError
pass
def checksum256(st):
"""Calculate checksum"""
return functools.reduce(lambda x, y: x+y, st) % 256
if checksum-checksum256(self.msg_bytes+data[1:]) == 0:
self.checksum = True
else:
self.checksum = False
def get_records(self):
while len(self.msg_bytes) > 0:
try:
# READ THE FIRST TWO BYTES FROM RECORD HEADER
record_type, record_length = unpack('>2B', self.msg_bytes[0:2])
self.msg_bytes = self.msg_bytes[2:]
self.select_record(record_type, record_length)
except:
logger.debug("There has been an incomplete record but I will restart")
break
def select_record(self, record_type, record_length):
if record_type == 1:
rec_field_names = ('GPS_TIME', 'GPS_WEEK', 'SVN_NUM',
'FLAG_1', 'FLAG_2', 'INIT_NUM')
rec_values = unpack('>LH4B', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 2:
rec_field_names = ('LATITUDE', 'LONGITUDE', 'HEIGHT')
rec_values = unpack('>3d', self.msg_bytes[0:record_length])
rec_values = list(rec_values)
rec_values = (math.degrees(rec_values[0]), math.degrees(rec_values[1]), rec_values[2])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 3:
rec_field_names = ('X_POS', 'Y_POS', 'Z_POS')
rec_values = unpack('>3d', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 4:
rec_field_names = ('LOCAL_DATUM_ID', 'LOCAL_DATUM_LAT',
'LOCAL_DATUM_LON', 'LOCAL_DATUM_HEIGHT', 'OPRT')
rec_values = unpack('>8s3dB', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 5:
rec_field_names = ('LOCAL_DATUM_ID', 'LOCAL_ZONE_ID',
'LOCAL_ZONE_NORTH', 'LOCAL_ZONE_EAST', 'LOCAL_DATUM_HEIGHT')
rec_values = unpack('>2s3d', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 6:
rec_field_names = ('DELTA_X', 'DELTA_Y', 'DELTA_Z')
rec_values = unpack('>3d', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 7:
rec_field_names = ('DELTA_EAST', 'DELTA_NORTH', 'DELTA_UP')
rec_values = unpack('>3d', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 8:
rec_field_names = ('VEL_FLAG', 'VELOCITY', 'HEADING', 'VERT_VELOCITY')
rec_values = unpack('>B3f', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 9:
rec_field_names = ('PDOP', 'HDOP', 'VDOP', 'TDOP')
rec_values = unpack('>4f', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 10:
rec_field_names = ('CLOCK_FLAG', 'CLOCK_OFFSET', 'FREQ_OFFSET')
rec_values = unpack('>B2d', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 11:
rec_field_names = ('POSITION_RMS_VCV', 'VCV_XX', 'VCV_XY', 'VCV_XZ',
'VCV_YY', 'VCV_YZ', 'VCV_ZZ', 'UNIT_VAR_VCV', 'NUM_EPOCHS_VCV')
rec_values = unpack('>8fh', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 12:
rec_field_names = ('POSITION_RMS_SIG', 'SIG_EAST', 'SIG_NORT', 'COVAR_EN', 'SIG_UP',
'SEMI_MAJOR', 'SEMI_MINOR', 'ORIENTATION', 'UNIT_VAR_SIG',
'NUM_EPOCHS_SIG')
rec_values = unpack('>9fh', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 15:
rec_field_names = 'SERIAL_NUM'
rec_values = unpack('>l', self.msg_bytes[0:record_length])
self.rec_dict.update({rec_field_names: rec_values[0]})
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 16:
rec_field_names = ('GPS_MS_OF_WEEK', 'CT_GPS_WEEK', 'UTC_OFFSET', 'CT_FLAGS')
rec_values = unpack('>l2hB', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 26:
rec_field_names = ('UTC_MS_OF_WEEK', 'UTC_GPS_WEEK', 'UTC_SVS_NUM', 'UTC_FLAG_1', 'UTC_FLAG_2',
'UTC_INIT_NUM')
rec_values = unpack('>lh4B', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
elif record_type == 34:
NUM_OF_SVS = unpack('>B', self.msg_bytes[0])
self.msg_bytes = self.msg_bytes[1:]
rec_field_names = ('PRN', 'SV_SYSTEM', 'SV_FLAG1', 'SV_FLAG2', 'ELEVATION', 'AZIMUTH',
'SNR_L1', 'SNR_L2', 'SNR_L5')
for field in xrange(len(rec_field_names)):
self.rec_dict[rec_field_names[field]] = []
for sat in xrange(NUM_OF_SVS[0]):
rec_values = unpack('>5Bh3B', self.msg_bytes[0:10])
self.msg_bytes = self.msg_bytes[10:]
for num in xrange(len(rec_field_names)):
self.rec_dict[rec_field_names[num]].append(rec_values[num])
elif record_type == 37:
rec_field_names = ('BATT_CAPACITY', 'REMAINING_MEM')
rec_values = unpack('>hd', self.msg_bytes[0:record_length])
self.rec_dict.update(dict(zip(rec_field_names, rec_values)))
self.msg_bytes = self.msg_bytes[record_length:]
else:
"""Unknown record type? Skip it for now!"""
#print record_type
self.msg_bytes = self.msg_bytes[record_length:]
## END GSOF CLASS