Skip to content

Commit f940a01

Browse files
OldCCOldCC
committed
Added files via upload
1 parent 2aee2a4 commit f940a01

File tree

6 files changed

+553
-71
lines changed

6 files changed

+553
-71
lines changed

‎dst.py‎

Lines changed: 228 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -1,33 +1,64 @@
1-
import sys
21
import serial
32
import math
43
import operator
54
import time
65
import socket
6+
import os.path
7+
import os
78

8-
DST_IP = "127.0.0.4"
9+
DST_IP = "127.0.0.3"
910
DST_PORT = 5005
1011

12+
MON_IP = "127.0.0.6"
13+
MON_PORT = 5005
14+
15+
mon = 0
16+
log = time.time()
17+
18+
# initialize for VDR
19+
five = 0
20+
setx_total = 0
21+
setx_run = [0] * 5
22+
sety_total = 0
23+
sety_run = [0] * 5
24+
drift_total = 0
25+
drift_run = [0] * 5
26+
27+
# initialize for VLW
1128
vlwfirst = 1
1229
vlwinit = 0.0
13-
t_dpt = 0
14-
t_print = time.time()
15-
t_fail = 0.0
16-
17-
sddpt = ''
18-
mtw = ''
19-
yxmtw = ''
20-
vwvhw = ''
21-
vlw = ''
22-
vwvlw = ''
2330

24-
ser = serial.Serial('/dev/ttyUSB0', 4800, timeout=1)
31+
# determine dst port
32+
f = open('gps_port', 'r')
33+
gps_port = f.readline()
34+
f.close()
35+
dst_port = "1"
36+
if gps_port == "1":
37+
dst_port = "0"
38+
ser = serial.Serial('/dev/ttyUSB' + dst_port, 4800, timeout=5)
2539

2640
while True:
41+
42+
# health monitor
2743
hack = time.time()
44+
if hack - mon > .5:
45+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
46+
sock.sendto(str(hack), (MON_IP, MON_PORT))
47+
mon = hack
2848

49+
# log raw input
2950
dst_raw = ser.readline()
30-
51+
f = open('dstraw', 'a')
52+
f.write(dst_raw)
53+
f.close()
54+
if hack - log > 60:
55+
os.remove('dstraw')
56+
f = open('dstraw', 'w')
57+
f.write("Serial port " + dst_port + ": " + time.asctime( time.localtime(time.time()) ) + "\r\n")
58+
f.close()
59+
log = hack
60+
61+
# checking to see if it's a valid NMEA sentence
3162
if "*" in dst_raw:
3263
dst_split = dst_raw.split('*')
3364
dst_sentence = dst_split[0].strip('$')
@@ -36,50 +67,211 @@
3667
if len(cs) == 1:
3768
cs = "0" + cs
3869

70+
# if it is a valid NMEA sentence
3971
if cs0 == cs:
40-
41-
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
4272
dst_vars = dst_sentence.split(',')
4373
title = dst_vars[0]
4474

75+
# depth sentence
4576
if title == "SDDPT":
46-
sddpt = dst_raw[:-2]
77+
78+
# roll and pitch from imu
79+
try:
80+
f = open('imu_bus', 'r')
81+
line = f.readline()
82+
f.close()
83+
imu_split = line.split(',')
84+
imu_hack = float(imu_split[0])
85+
roll = float(imu_split[2])
86+
pitch = float(imu_split[3])
87+
88+
except ValueError:
89+
time.sleep(.03)
90+
f = open('imu_bus', 'r')
91+
line = f.readline()
92+
f.close()
93+
imu_split = line.split(',')
94+
imu_hack = float(imu_split[0])
95+
roll = float(imu_split[2])
96+
pitch = float(imu_split[3])
97+
98+
# correct depth for 23 degree offset from centerline, but if roll/pitch
99+
# are from the last 3 seconds, correct depth for attitude
100+
depth = round(float(dst_vars[1])*math.cos(math.radians(23)),1)
101+
if time.time() - imu_hack < 3.0:
102+
depth = round(float(dst_vars[1])*math.cos(math.radians(23-roll))*math.cos(math.radians(pitch)),1)
103+
104+
# assemble the sentence with .5 meter offset from waterline
105+
dpt = "SDDPT," + str(depth) + ",0.50"
106+
dptcs = format(reduce(operator.xor,map(ord,dpt),0),'X')
107+
if len(dptcs) == 1:
108+
dptcs = "0" + dptcs
109+
sddpt = "$" + dpt + "*" + dptcs + "\r\n"
110+
111+
# to kplex
112+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
47113
sock.sendto(sddpt, (DST_IP, DST_PORT))
48-
t_dpt = hack
49-
t_fail = 0.0
50114

115+
# mean water temp sentence
51116
if title == "YXMTW":
52-
mtw = dst_vars[0] + "," + str(float(dst_vars[1]) * 9 / 5 + 32) + ",F"
117+
118+
# convert to fahrenheit
119+
mtw = dst_vars[0] + "," + str(int(float(dst_vars[1]) * 9 / 5 + 32)) + ",F"
53120
cs = format(reduce(operator.xor,map(ord,mtw),0),'X')
54121
if len(cs) == 1:
55122
cs = "0" + cs
56-
yxmtw = "$" + mtw + "*" + cs
123+
yxmtw = "$" + mtw + "*" + cs + "\r\n"
124+
125+
# to kplex
126+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
57127
sock.sendto(yxmtw, (DST_IP, DST_PORT))
58128

129+
# vessel waterspeed sentence and current set and drift
59130
if title == "VWVHW":
60-
vwvhw = dst_raw[:-2]
131+
132+
# heading and roll from imu
133+
try:
134+
f = open('imu_bus', 'r')
135+
line = f.readline()
136+
f.close()
137+
imu_split = line.split(',')
138+
imu_hack = float(imu_split[0])
139+
heading = float(imu_split[1])
140+
roll = float(imu_split[2])
141+
except ValueError:
142+
f.close()
143+
time.sleep(.03)
144+
f = open('imu_bus', 'r')
145+
line = f.readline()
146+
f.close()
147+
imu_split = line.split(',')
148+
imu_hack = float(imu_split[0])
149+
heading = float(imu_split[1])
150+
roll = float(imu_split[2])
151+
152+
# course and groundspeed from gps
153+
try:
154+
f = open('gps_bus', 'r')
155+
line = f.readline()
156+
gps_split = line.split(',')
157+
f.close()
158+
gps_hack = float(gps_split[0])
159+
valid = gps_split[1]
160+
course = float(gps_split[2])
161+
groundspeed = float(gps_split[3])
162+
except ValueError:
163+
time.sleep(.03)
164+
f = open('gps_bus', 'r')
165+
line = f.readline()
166+
f.close()
167+
gps_split = line.split(',')
168+
gps_hack = float(gps_split[0])
169+
valid = gps_split[1]
170+
course = float(gps_split[2])
171+
groundspeed = float(gps_split[3])
172+
173+
# calculate corrected waterspeed from heel and velocity
174+
waterspeed = float(dst_vars[5])
175+
sensor_angle = 23.0
176+
if time.time() - imu_hack < 3.0:
177+
sensor_angle = math.fabs(23.0-roll)
178+
five_knot_correction = -.02 * sensor_angle
179+
ten_knot_correction = sensor_angle * (.035 - .0065 * sensor_angle) - 2
180+
if sensor_angle > 10.0:
181+
ten_knot_correction = -.03 * sensor_angle - 2
182+
if waterspeed < 5.0:
183+
waterspeed = round(waterspeed + (five_knot_correction * waterspeed / 5), 1)
184+
else:
185+
waterspeed = round((waterspeed + (waterspeed * (ten_knot_correction * (waterspeed - 5) - 2 * five_knot_correction * (waterspeed - 10))) / 50),1)
186+
187+
# assemble the sentence
188+
vhw = "VWVHW,"
189+
if time.time() - imu_hack < 3.0:
190+
vhw = vhw + str(int(heading))
191+
else: vhw = vhw + ''
192+
vhw = vhw + ",T,,M," + str(waterspeed) + ",N,,K"
193+
vhwcs = format(reduce(operator.xor,map(ord,vhw),0),'X')
194+
if len(vhwcs) == 1:
195+
vhwcs = "0" + vhwcs
196+
vwvhw = "$" + vhw + "*" + vhwcs + "\r\n"
197+
198+
# to kplex
199+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
61200
sock.sendto(vwvhw, (DST_IP, DST_PORT))
62201

202+
# VDR set and drift
203+
if time.time() - imu_hack < 3.0 and time.time() - gps_hack < 3.0 and valid == "A":
204+
205+
heading_radians = math.radians(heading)
206+
course_radians = math.radians(course)
207+
set0 = course_radians - heading_radians
208+
if set0 < 0:
209+
set0 = set0 + 2 * math.pi
210+
set_relative = math.pi - math.atan2(groundspeed * math.sin(set0), waterspeed - groundspeed * math.cos(set0))
211+
if waterspeed == 0 and groundspeed == 0:
212+
set_relative = set0
213+
set_radians = heading_radians + set_relative
214+
if set_radians > (2 * math.pi):
215+
set_radians = set_radians - (2 * math.pi)
216+
drift = math.sqrt(pow(waterspeed,2) + pow(groundspeed,2) - 2 * waterspeed * groundspeed * math.cos(set0))
217+
218+
# dampen out set and drift over the last five readings
219+
setx_total = setx_total - setx_run[five]
220+
setx_run[five] = math.cos(set_radians)
221+
setx_total = setx_total + setx_run[five]
222+
setx_ave = setx_total / 5
223+
sety_total = sety_total - sety_run[five]
224+
sety_run[five] = math.sin(set_radians)
225+
sety_total = sety_total + sety_run[five]
226+
sety_ave = sety_total / 5
227+
set_radians = math.atan2(sety_ave, setx_ave)
228+
if set_radians < 0:
229+
set_radians = set_radians + 2 * math.pi
230+
set_true = math.degrees(set_radians)
231+
set_apparent = set_true - heading
232+
if set_apparent < 0:
233+
set_apparent = set_apparent + 360
234+
235+
drift_total = drift_total - drift_run[five]
236+
drift_run[five] = drift
237+
drift_total = drift_total + drift_run[five]
238+
drift = drift_total / 5
239+
five = five + 1
240+
if five > 4:
241+
five = 0
242+
243+
# assemble the sentence
244+
vdr = "IIVDR,A," + str(int(set_true)) + ",T,,,M," + str(round(drift,1)) + ",N"
245+
vdrcs = format(reduce(operator.xor,map(ord,vdr),0),'X')
246+
if len(vdrcs) == 1:
247+
vdrcs = "0" + vdrcs
248+
iivdr = "$" + vdr + "*" + vdrcs + "\r\n"
249+
250+
# ghost MWV sentence to show set/drift instead of wind
251+
mwv = "IIMWV," + str(int(set_apparent)) + ",R," + str(round(drift,1)) + ",N,A"
252+
mwvcs = format(reduce(operator.xor,map(ord,mwv),0),'X')
253+
if len(mwvcs) == 1:
254+
mwvcs = "0" + mwvcs
255+
iimwv = "$" + mwv + "*" + mwvcs + "\r\n"
256+
257+
# to kplex
258+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
259+
sock.sendto(iivdr + iimwv, (DST_IP, DST_PORT))
260+
261+
# voyage log sentence
63262
if title == "VWVLW":
263+
264+
# calculate present trip total vs overall total
64265
if vlwfirst == 1:
65266
vlwinit = dst_vars[1]
66267
vlwfirst = 0
67268
trip = float(dst_vars[1]) - float(vlwinit)
68269
vlw = "VWVLW," + dst_vars[1] + ",N," + str(trip) + ",N"
69-
cs = format(reduce(operator.xor,map(ord,mtw),0),'X')
270+
cs = format(reduce(operator.xor,map(ord,vlw),0),'X')
70271
if len(cs) == 1:
71272
cs = "0" + cs
72-
vwvlw = "$" + vlw + "*" + cs
73-
sock.sendto(vwvlw, (DST_IP, DST_PORT))
273+
vwvlw = "$" + vlw + "*" + cs + "\r\n"
74274

75-
if (hack - t_dpt) > 10.0:
76-
if (hack - t_print > 1.0):
77-
t_fail += 1.0
78-
dst_sentence = "IIXDR,DST_FAIL," + str(round(t_fail / 60, 1))
79-
cs = format(reduce(operator.xor,map(ord,dst_sentence),0),'X')
80-
if len(cs) == 1:
81-
cs = "0" + cs
82-
dst_sentence = "$" + dst_sentence + "*" + cs
83-
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
84-
sock.sendto(dst_sentence, (DST_IP, DST_PORT))
85-
t_print = hack
275+
# to kplex
276+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
277+
sock.sendto(vwvlw, (DST_IP, DST_PORT))

0 commit comments

Comments
 (0)