Skip to content

Commit ad18063

Browse files
jmachuca77jmachuca77
committed
Add TCP Connection to DGPS Module
1 parent e85e717 commit ad18063

File tree

1 file changed

+218
-53
lines changed

1 file changed

+218
-53
lines changed

‎MAVProxy/modules/mavproxy_DGPS.py

Lines changed: 218 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -1,86 +1,251 @@
11
#!/usr/bin/env python
22
'''
33
support for a GCS attached DGPS system
4+
Set the ip, port and conntype settings to your requiered connection
5+
The module supports TCP and UDP connections (conntype)
6+
7+
For using with Emlid Reach use TCP connection, set the IP to the IP address of the Reach base
8+
9+
Configure ReachView like so:
10+
11+
Base mode:
12+
TCP, Role: Server, address: localhost, Port: 9000
13+
Set the RTCM Messages to 10Hz and enable any that are needed.
414
'''
515

6-
import socket, errno
16+
import socket, errno, threading
17+
import time
18+
from MAVProxy.modules.lib import rtcm3
719
from pymavlink import mavutil
820
from MAVProxy.modules.lib import mp_module
21+
from MAVProxy.modules.lib import mp_settings
922

1023
class DGPSModule(mp_module.MPModule):
1124
def __init__(self, mpstate):
12-
super(DGPSModule, self).__init__(mpstate, "DGPS", "DGPS injection support for SBP/RTCP/UBC")
25+
super(DGPSModule, self).__init__(mpstate, "DGPS", "DGPS injection support for SBP/RTCP/UBC over tcp/udp")
26+
print ("Loading DGPS module")
27+
self.dgps_settings = mp_settings.MPSettings(
28+
[('ip', str, "127.0.0.1"),
29+
('port', int, 13320),
30+
('conntype', str, 'UDP'),
31+
('silentFail', bool, True),
32+
('logfile', str, None),
33+
('connectionTimeout', int, 10)
34+
])
35+
self.add_command('dgps', self.cmd_dgps, 'DGPS control',
36+
["<status>",
37+
"<start>",
38+
"<stop>",
39+
"set (DGPSSETTING)"])
40+
self.add_completion_function('(DGPSSETTING)',
41+
self.dgps_settings.completion)
42+
43+
self.port = None
44+
self.waiting = False
45+
self.lastConnAttempt = None
46+
self.last_pkt = time.time()
47+
self.inject_seq_nr = 0
48+
self.pkt_count = 0
49+
self.last_rate = None
50+
self.rate_total = 0
51+
self.rate = 0
52+
# RTCM3 parser
53+
self.rtcm3 = rtcm3.RTCM3()
54+
self.last_id = None
55+
self.id_counts = {}
56+
self.last_by_id = {}
57+
58+
def log_rtcm(self, data):
59+
'''optionally log rtcm data'''
60+
if self.dgps_settings.logfile is None:
61+
return
62+
if self.logfile is None:
63+
self.logfile = open(self.dgps_settings.logfile, 'wb')
64+
if self.logfile is not None:
65+
self.logfile.write(data)
66+
67+
def cmd_dgps(self, args):
68+
'''dgps command handling'''
69+
if len(args) <= 0:
70+
print("Usage: dgps <start|stop|status|set>")
71+
return
72+
if args[0] == "start":
73+
self.cmd_start()
74+
if args[0] == "stop":
75+
self.cmd_stop()
76+
elif args[0] == "status":
77+
self.dgps_status()
78+
elif args[0] == "set":
79+
self.dgps_settings.command(args[1:])
80+
81+
def cmd_stop(self):
82+
self.waiting = False
83+
self.port = None
84+
85+
def cmd_start(self):
86+
'''start dgps link'''
87+
if self.dgps_settings.conntype == "UDP":
88+
print("Connecting to UDP RTCM Stream")
89+
self.connect_udp_rtcm_base(self.dgps_settings.ip, self.dgps_settings.port)
90+
elif self.dgps_settings.conntype == "TCP":
91+
print("Connecting to TCP RTCM Stream")
92+
self.connect_tcp_rtcm_base(self.dgps_settings.ip, self.dgps_settings.port)
93+
else:
94+
print("Undefined connection type!")
95+
return
96+
97+
self.last_rate = time.time()
98+
self.rate_total = 0
99+
100+
def dgps_status(self):
101+
'''show dgps status'''
102+
now = time.time()
103+
print("\nDGPS Configuration:")
104+
print("Connection Type: %s" % self.dgps_settings.conntype)
105+
print("IP Address: %s" % self.dgps_settings.ip)
106+
print("Port: %s" % self.dgps_settings.port)
107+
108+
if self.waiting is True:
109+
print("Connection Error attempting to reconnect")
110+
return
111+
112+
if self.port is None:
113+
print("DGPS: Not started")
114+
return
115+
elif self.last_pkt is None:
116+
print("DGPS: no data")
117+
return
118+
119+
frame_size = 0
120+
for id in sorted(self.id_counts.keys()):
121+
print(" %4u: %u (len %u)" % (id, self.id_counts[id], len(self.last_by_id[id])))
122+
frame_size += len(self.last_by_id[id])
123+
print("dgps: %u packets, %.2f bytes/sec last %.3fs ago framesize %u" % (self.pkt_count, self.rate, now - self.last_pkt, frame_size))
124+
125+
def connect_udp_rtcm_base(self, ip, port):
126+
print ("UDP Connection")
13127
self.portnum = 13320
14128
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
15129
self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
16130
self.port.bind(("127.0.0.1", self.portnum))
17131
mavutil.set_close_on_exec(self.port.fileno())
18132
self.port.setblocking(0)
19-
self.inject_seq_nr = 0
20133
print("DGPS: Listening for RTCM packets on UDP://%s:%s" % ("127.0.0.1", self.portnum))
21-
22-
def send_rtcm_msg(self, data):
23-
msglen = 180;
24-
25-
if (len(data) > msglen * 4):
26-
print("DGPS: Message too large", len(data))
134+
135+
def connect_tcp_rtcm_base(self, ip, port):
136+
if self.waiting is False:
137+
print ("TCP Connection")
138+
self.waiting = True
139+
self.rtcm3.reset()
140+
try:
141+
self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
142+
self.port.settimeout(1)
143+
self.port.connect((ip, port))
144+
except:
145+
if self.dgps_settings.silentFail is False:
146+
print ("ERROR: Failed to connect to RTCM base over TCP, retrying in 2.5s")
147+
self.lastConnAttempt = time.time()
27148
return
28-
29-
# How many messages will we send?
30-
msgs = 0
31-
if (len(data) % msglen == 0):
32-
msgs = len(data) / msglen
33-
else:
34-
msgs = (len(data) / msglen) + 1
35-
36-
for a in range(0, msgs):
37-
38-
flags = 0
39-
40-
# Set the fragment flag if we're sending more than 1 packet.
41-
if (msgs) > 1:
42-
flags = 1
43-
44-
# Set the ID of this fragment
45-
flags |= (a & 0x3) << 1
46-
47-
# Set an overall sequence number
48-
flags |= (self.inject_seq_nr & 0x1f) << 3
49-
50-
51-
amount = min(len(data) - a * msglen, msglen)
52-
datachunk = data[a*msglen : a*msglen + amount]
53-
54-
self.master.mav.gps_rtcm_data_send(
55-
flags,
56-
len(datachunk),
57-
bytearray(datachunk.ljust(180, '\0')))
58-
59-
# Send a terminal 0-length message if we sent 2 or 3 exactly-full messages.
60-
if (msgs < 4) and (len(data) % msglen == 0) and (len(data) > msglen):
61-
flags = 1 | (msgs & 0x3) << 1 | (self.inject_seq_nr & 0x1f) << 3
62-
self.master.mav.gps_rtcm_data_send(
63-
flags,
64-
0,
65-
bytearray("".ljust(180, '\0')))
66-
67-
self.inject_seq_nr += 1
68149

150+
# This prevents the TCP connection from blocking other functions in the system.
151+
# If this is set before attempting the connection (.connect) the connection is
152+
# never established. However if this is not set before that attempt then MAVproxy
153+
# is blocked for the timeout period, in this case 1 sec.
154+
self.port.setblocking(0)
69155

156+
print ("Connected to base using TCP")
157+
self.waiting = False
158+
self.last_pkt = time.time()
70159

71160
def idle_task(self):
72161
'''called in idle time'''
162+
# Dont do anything if no recieve port is set
163+
if self.port is None:
164+
return
165+
166+
# Check to see if we have not recieved packets in a long time and try to reconnect
167+
now = time.time()
168+
if self.waiting is False and now - self.last_pkt > self.dgps_settings.connectionTimeout:
169+
print("Lost Packets attempting reconnection")
170+
self.cmd_stop()
171+
self.cmd_start()
172+
173+
# Try to reconnect in case the connection attempt failed.
174+
if self.waiting is True and self.dgps_settings.conntype == "TCP":
175+
if (time.time() - self.lastConnAttempt) > 2.5:
176+
if self.dgps_settings.silentFail is False:
177+
print("Attempting to connect")
178+
self.connect_tcp_rtcm_base(self.dgps_settings.ip, self.dgps_settings.port)
179+
return
180+
181+
# Read data from socket
73182
try:
74183
data = self.port.recv(1024) # Attempt to read up to 1024 bytes.
75184
except socket.error as e:
76185
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
77186
return
78187
raise
79-
try:
80-
self.send_rtcm_msg(data)
81188

82-
except Exception as e:
83-
print("DGPS: GPS Inject Failed:", e)
189+
# Parse and send RTCM3 data
190+
for element in data: # RTCM3 Library expects to read one byte at a time
191+
if self.rtcm3.read(element): # Data sent to rtcm library, it returns data once a full packet is recieved
192+
self.last_id = self.rtcm3.get_packet_ID()
193+
rtcm3Data = self.rtcm3.get_packet()
194+
if self.rtcm3.get_packet() is None:
195+
print ("No packet")
196+
return
197+
198+
self.log_rtcm(rtcm3Data) # Log the packet
199+
200+
# Store statistics for each RTCM Packet ID
201+
if not self.last_id in self.id_counts:
202+
self.id_counts[self.last_id] = 0
203+
self.last_by_id[self.last_id] = rtcm3Data[:]
204+
self.id_counts[self.last_id] += 1
205+
206+
# Prepare to send the RTCM3 Data through MAVLink
207+
blen = len(rtcm3Data)
208+
if blen > 4*180: # can't send this with GPS_RTCM_DATA (wont fit even on fragmented packets?)
209+
if self.dgps_settings.silentFail is False:
210+
print("Error, cannot send data with GPS_RTCM_DATA")
211+
return
212+
213+
self.rate_total += blen
214+
215+
# Check if data needs to be fragmented
216+
if blen > 180:
217+
flags = 1 # fragmented
218+
else:
219+
flags = 0
220+
221+
# add in the sequence number
222+
flags |= (self.pkt_count & 0x1F) << 3
223+
224+
# Send data through MAVLink
225+
fragment = 0
226+
while blen > 0:
227+
send_data = bytearray(rtcm3Data[:180])
228+
frag_len = len(send_data)
229+
data = rtcm3Data[frag_len:]
230+
if frag_len < 180:
231+
send_data.extend(bytearray([0]*(180-frag_len)))
232+
self.master.mav.gps_rtcm_data_send(flags | (fragment<<1), frag_len, send_data)
233+
fragment += 1
234+
blen -= frag_len
235+
self.pkt_count += 1
236+
237+
# Store time of last packet sent
238+
now = time.time()
239+
self.last_pkt = now
240+
241+
# Calculate the rate at which we are reciving data for stats
242+
now = time.time()
243+
if now - self.last_rate > 1:
244+
dt = now - self.last_rate
245+
rate_now = self.rate_total / float(dt)
246+
self.rate = 0.9 * self.rate + 0.1 * rate_now
247+
self.last_rate = now
248+
self.rate_total = 0
84249

85250
def init(mpstate):
86251
'''initialise module'''

0 commit comments

Comments
 (0)