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 !
