Home        Store        Docs        Blog

Blueprint subsea SeaTrac USBL integration (Python)


#1

Hello,
I’ve been experimenting with a Blueprint subsea SeaTrac X150 and X110 USBL (underwater GPS). Thought I’d like to share what I did to use it with the BlueROV2.

Set-up:

  • SeaTrac X110 and battery mounted on the BlueROV2
  • SeaTrac X150 mounted on the boat / base station and connected to the PC
  • USB GPS connected to the PC
  • Of course, ROV tether connected to the PC

Using the supplied software, I checked the beacon configuration. Not sure if it is needed as the blinking of the LED tells the beacon ID (3 in my case). You can also adjust for salinity etc.

As the supplied software is a bit cumbersome and not really reliable (and runs on Windows), I wrote a Python script (runs on Linux) that computes the position of the ROV and sends it as a NMEA string to the raspberry pi using UDP on port 27000. It also stores the position history in a .csv file.
As the boat provides GPS data over WiFi, we can choose between a USB GPS and the GPS on WiFi (Yacht Devices WiFi Gateway)

This script works in Switzerland as it is based on the CH1903 coordinate system. I guess it’ll also work in the neighboring countries.
I used this library for converting the coordinates: wgs84_ch1903.py. You should put it in a subfolder called “lib” along with an empty file called “__init__.py”.

import socket # GPS and ROV, TCP/UDP

import serial # SeaTrac
import serial.tools.list_ports
import crcmod # SeaTrac checksum

import select # Non-blocking communication

import pynmea2 # NMEA
import lib.wgs84_ch1903 as wgs84_ch1903 # Swiss coordinates

import datetime # Time of day
import time # Program time

import csv # Write to csv file

######################
# Parameters
######################

# CSV file location
csv_filename = "ROV.csv"

# Set to false to use the boat GPS information (Yacht Devices)
USE_SERIAL_GPS = True

# Yacht devices address
GPS_IP = '192.168.178.9'
GPS_PORT = 1456

BUFFER_SIZE = 4096

# ROV address (UDP)
ROV_IP = '192.168.2.2'
ROV_PORT = 27000

# ROV position update interval
ROV_UPDATE_SECONDS = 0.5

# SeaTrac baudrate
SEATRAC_BPS = 115200

# SeaTrac ping interval
SEATRAC_PING_SECONDS = 3


######################
# Global variables
######################
gps_wgs84 = { 'lat' : 0, 'lon' : 0 }
gps_swiss = {   'x' : 0,   'y' : 0 }
gps_lastfix = ''
rov_depth = 0
rov_swiss = {   'x' : 0,   'y' : 0 }
rov_wgs84 = { 'lat' : 0, 'lon' : 0 }

######################
# SeaTrac serial port setup
######################

# Display all serial ports
ports = list( serial.tools.list_ports.comports() )

print( "Serial ports:" )
port_id = 0
for port in ports :
	print( "{}: {}".format(port_id, port.device) )
	port_id = port_id + 1

# Ask the user which port to use
while True:
	user_selection = input("\r\nSelect SeaTrac serial port: ")
			
	if user_selection.isdigit():
		user_selection = int( user_selection )
		
		if user_selection >= 0 and user_selection < port_id :
			seatrac_port_name = ports[ user_selection ].device
			break

print( "You selected {} as the SeaTrac serial port".format(seatrac_port_name) )

# Open the serial port
seatrac_port = serial.Serial( seatrac_port_name, SEATRAC_BPS, timeout=5)


######################
# GPS serial port setup
######################
if USE_SERIAL_GPS:
	# Ask the user which port to use
	while True:
		user_selection = input("\r\nSelect GPS serial port: ")
				
		if user_selection.isdigit():
			user_selection = int( user_selection )
			
			if user_selection >= 0 and user_selection < port_id :
				gps_port_name = ports[ user_selection ].device
				break

	print( "You selected {} as the GPS serial port".format(seatrac_port_name) )

	# Open the serial port
	gps_port = serial.Serial( gps_port_name, 4800, timeout=5)
	

######################
# SeaTrac response processing functions
######################

# Checksum function setup
crc16 = crcmod.mkCrcFun(poly = 0x18005, initCrc = 0, rev = True, xorOut = 0 )

def process_seatrac(r) :
	global gps_swiss, rov_depth, rov_swiss, rov_wgs84 
	# Extract checksum
	csum_bytes = bytearray.fromhex( response[-4:] )
	csum = csum_bytes[0] + 256 * csum_bytes[1]
	
	# Compute checksum
	csum_computed = crc16( bytearray.fromhex( response[1:-4] ) )
	
	if ( csum == csum_computed ) :
	
		# Extract command ID and command response
		cid = response[1:3]
		cresp = bytearray.fromhex( response[3:-4] )
		
		# Process position response (CID_PING_RESP)
		if ( cid == '42' ) :
			# Position is valid (bit 2 of FLAGS)
			if ( cresp[2] & 0b0100 ) :
				# Extract position in meters
				easting = int.from_bytes( cresp[-6:-4], byteorder='little', signed='True') / 10
				northing = int.from_bytes( cresp[-4:-2], byteorder='little', signed='True') / 10
				depth = int.from_bytes( cresp[-2:], byteorder='little', signed='True') / 10

				# Compute the absolute position
				rov_swiss['x'] = gps_swiss['x'] + easting
				rov_swiss['y'] = gps_swiss['y'] + northing
				rov_depth = depth
				
				# Convert to WGS84 datum
				rov_wgs84['lat'], rov_wgs84['lon'], __ = wgs84_ch1903.GPSConverter().LV03toWGS84(rov_swiss['x'], rov_swiss['y'], 0)


######################
# YachtDevices GPS connection setup
######################

if not USE_SERIAL_GPS:
	# Connect to the Yacht Devices server
	# AF_INET means ipv4, SOCK_STREAM means TCP
	gps_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	gps_socket.connect((GPS_IP, GPS_PORT))


######################
# GPS processing functions
######################

# GPS data interpreter
streamreader = pynmea2.NMEAStreamReader()

def process_gps(data) :
	global gps_wgs84, gps_swiss, gps_lastfix

	for msg in streamreader.next(data):
		# Only consider the RMC NMEA sentence
		if msg.sentence_type == 'RMC' :
			# msg.lat/lon format: 4627.6999 = 46°27.6999'
			lat_deg = int( float(msg.lat) / 100 )
			gps_wgs84['lat'] = ( float(msg.lat) / 100 - lat_deg ) * 100 / 60 + lat_deg
			lon_deg = int( float(msg.lon) / 100 )
			gps_wgs84['lon'] = ( float(msg.lon) / 100 - lon_deg) * 100 / 60 + lon_deg
			
			# Convert the GPS coordinates into CH1903 Swiss coordinates
			gps_swiss['x'], gps_swiss['y'], __ = wgs84_ch1903.GPSConverter().WGS84toLV03(gps_wgs84['lat'], gps_wgs84['lon'], 0)
			
			# Update time of last fix
			gps_lastfix = str(msg.datestamp) + " " + str(msg.timestamp)[:8]


######################
# ROV connection setup
######################
# UDP socket
rov_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)


######################
# Main loop
######################

with open(csv_filename, 'w', newline='') as csvfile:
	csv_writer = csv.writer( csvfile, delimiter=';', quotechar='"', quoting=csv.QUOTE_MINIMAL )
	
	# Column titles
	csv_writer.writerow( [ "PC time", "Base CH1903 X", "Base CH1903 Y", "ROV CH1903 X", "ROV CH1903 Y", "ROV depth", "Last fix GPS time" ] )

	# Store the last ping time to only send commands to SeaTrac every x seconds
	last_ping = 0
	
	# Store the last udp message to ROV time to only update every x seconds
	last_rov_update = 0
	
	while True:
		try:
			# Check for which devices data is available. If no data is available, timeout is 5s.
			if USE_SERIAL_GPS :
				ready_to_read, ready_to_write, __ = select.select([gps_port, seatrac_port], [rov_socket], [], 5)
			else:
				ready_to_read, ready_to_write, __ = select.select([gps_socket, seatrac_port], [rov_socket], [], 5)
			
			# Send a PING every x seconds
			if ( ( time.time() - last_ping ) >= SEATRAC_PING_SECONDS ) :
				# Ping beacon #3
				seatrac_port.write(b'#4003068126\r\n')
				
				# Reset stopwatch
				last_ping = time.time()
			
			# Read data from the SeaTrac and write to CSV
			if seatrac_port in ready_to_read :
				response = seatrac_port.read_until(b'\r\n').decode('ascii')
				start_pos = response.find('$')

				if ( start_pos >= 0 ) :
					# Start at $ and remove trailing \r\n
					response = response[start_pos:-2]
					
					process_seatrac( response )
					
					# Write to CSV
					date_str = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S")
					gps_swiss_str = { 'x' : "%.1f" %gps_swiss['x'], 'y' : "%.1f" %gps_swiss['y'] }
					rov_swiss_str = { 'x' : "%.1f" %rov_swiss['x'], 'y' : "%.1f" %rov_swiss['y'] }
					rov_depth_str = "%.1f" %rov_depth
					
					# Columns:           [ "PC time", "Base CH1903 X",    "Base CH1903 Y",    "ROV CH1903 X",     "ROV CH1903 Y",     "ROV depth",   "Last fix GPS time" ]
					csv_writer.writerow( [  date_str,  gps_swiss_str['x'], gps_swiss_str['y'], rov_swiss_str['x'], rov_swiss_str['y'], rov_depth_str, gps_lastfix ] )

			
			# Read data from the GPS
			if USE_SERIAL_GPS:
				if gps_port in ready_to_read :
					try:
						data = gps_port.read().decode('ascii')
						process_gps(data)
					except:
						print("GPS decoding error")
						# Try to switch it to NMEA mode in case it is in binary mode
						gps_port.write(b'A0A200188102010100010101050101010001000100000001000012C00165B0B3')

			else:
				if gps_socket in ready_to_read :
					data = gps_socket.recv(BUFFER_SIZE).decode('ascii')
					process_gps(data)
				
				
			
			# Send position to ROV every x seconds
			if ( rov_socket in ready_to_write ) and ( ( time.time() - last_rov_update ) >= ROV_UPDATE_SECONDS ) :
				
				# Build GGA NMEA sentence
				lat_deg = int( rov_wgs84['lat'] )
				lat = 100 * lat_deg + ( rov_wgs84['lat'] - lat_deg ) * 60
				lon_deg = int( rov_wgs84['lon'] )
				lon = 100 * lon_deg + ( rov_wgs84['lon'] - lon_deg ) * 60
				#                                             time                                 lat               lon              fix  sats  hdop   alt     height          dgps
				msg = pynmea2.GGA('GP', 'GGA', ( datetime.datetime.utcnow().strftime("%H%M%S"),    str(lat) , 'N',        str(lon), 'E', '1', '08', '1.5', '0', 'M', '0', 'M', '', '0000'))
				
				# Send the sentence to the ROV
				rov_socket.sendto( bytes( str(msg) + '\r\n' , "ascii"), (ROV_IP, ROV_PORT) )
				print(str(msg))
				
				# Reset stopwatch
				last_rov_update = time.time()


		except KeyboardInterrupt:

			# Exit on Ctrl-C
			break

# Close the ports
seatrac_port.close()
gps_socket.close()
gps_port.close()

I’ve only tested it in the office for now, hope I can try it in the lake soon…

And if someone knows how to display the position of the base station (boat) on QGroundControl, I’d be very interested. (for now only the ROV position is displayed)

Hope that bits of this script can help someone !


Compass and position jumping around
(Jacob) #2

So wonderful!

A hint to perhaps simplify the setup (not that there’s anything wrong with it). Most usbl are capable of a NMEA output mode, sending NMEA sentences on the serial port. Then you can just use a program like the Arundale nmea router to bridge the communication serial<->rov udp port.

Re, the usb gps and boat position, look on the General Settings page of QGC, you will see a selection for nmea gps port and baudrate selection. Set that up, and the boat position will be displayed.


#3

Thanks !

I first tried with NMEA Router, but this implies using the USBL Windows software which is quite buggy (parameters not stored between launches, sometimes needs a reboot to work etc.).

Thanks for the boat position setting, I just tried it but QGroundcontrol crashes (segmentation fault) when selecting the serial port (/dev/ttyUSB0). Afterwards, it crashes every time I try to launch it (must remove the AutoconnectNmeaPort line in ~/.config/QGroundControl.org/QGroundControl.ini so it can launch again). I’m not running the Python script when it happens. Do you have a clue what might be wrong? I’m running QGroundControl v3.2.4-BlueRobotics-Rev6.


(Jacob) #4

What a pro!

Can you try in a terminal screen /dev/ttyUSB0 9600 or whatever baudrate the gps uses? Does the gps show any output on the terminal?

It might be a multiple access thing, make sure no other programs are using the gps. If this is not an option, you might be interested in this feature, and you can send it from your python script on UDP (after it is merged): https://github.com/mavlink/qgroundcontrol/pull/7087


#5

NMEA data is displayed correctly using screen, but QGroundcontrol still crashes.

I’m looking forward for the new feature, it’ll be much easier for my setup, and the NMEA on the boat wifi also has heading information which is very welcome !

Tell me if I can do something to help with the segfault.


(Jacob) #6

Can you launch QGroundControl from a terminal and put the output + segfault in a pastebin?

Please go to console->set logging and enable the categories for linkmanager, and positionmanager.


#7

I couldn’t find the category for positionmanager in the list, I tried adding PositionManagerLog=true in QGroundControl.ini, not sure if it helped.
Anyway here is the pastebin, hope it is useful ! https://pastebin.com/ijychsBg

I’m running QGroundControl.AppImage, maybe it behaves differently than when not packaged as AppImage ?

I also tried running it with gdb (never done that, not sure it makes sense). Output is here.

(I won’t be working until mid-January, don’t be surprised if I do not reply until then)