# -*- coding: utf-8 -*- """ Created on Wed Mar 02 14:54:25 2016 @author: Ivan Masmitja """ #!/usr/bin/env python """ Revision History: 2014-07-08 - Initial creation B. Kieft NOTE: THIS SCRIPT MUST BE EXECUTED FROM SMC-SCRIPT DIRECTORY. This script is designed to take VMC GPS data and Benthos modem data as inputs and output a position of a corresponding acoustic modem. To run on an SMC, is is recommened that pip is installed: https://pip.pypa.io/en/latest/installing.html to enable the installation of the following dependent packages: pynmea Note that VMC GPS sensor output is expected in the following form: ... GPS,41827.86832176,36.8023927,-121.7876722 NMEA,GPVTG,,T,,M,0.034,N,0.063,K,D*24 NMEA,GPGGA,205024.00,3648.14354,N,12147.26031,W,2,08,1.27,8.7,M,-30.7,M,,0000*6F NMEA,GPGLL,3648.14354,N,12147.26031,W,205024.00,A,D*76 NMEA,GPGST,205024.00,15,,,,1.8,2.3,3.3*5A NMEA,GPZDA,205024.00,07,07,2014,00,00*60 NMEA,GPRMC,205025.00,A,3648.14350,N,12147.26027,W,0.128,,070714,,,D*63 GPS,41827.86833333,36.8023923,-121.7876718 ... Additionally, Benthos output is expected to be in the following form, configured as shown: 16:20:33.8419 , LVL= 21216, 14545, 20466, 24051, AGC= 72, IDX=258, 0.00, 2.820, PHS= 2.918, 0.027,-3.054, |v|= 2.987, RAW= 59.0, 0.4, CAL= 33.1, 8.3, ROT= 305.8, 8.3 Range 1 to 0 : 4.2 m @DatVerbose parameter should be set to 27505 (0x6B71 per spec). response to "ATRCH" or "dat" command should look something like this: Output lines prefaced with 'RANGE:' are in the following format target,channel,epochSec,range,bearing,lon,lat,timeString """ from pynmea import nmea import time import subprocess import serial import calendar import os import sys import trackerStalker import utm #(Ivan) import numpy as np #(Ivan) #import matplotlib.pyplot as plt # In general, variables named with all UPPER_CAPS are held constant during the program; # You may want to edit these values before running # Set IVANSIM=True when the program is runing out of Wave Glider IVANSIM = False # Set SIMULATED=False when we have actual targets to range to SIMULATED = False # Set ENABLE_ODSS_MAIL=True to enable sending mail to ODSS ENABLE_ODSS_MAIL = True # Print additional messages if DEBUG is true DEBUG = True # GPS data file GPS_FILE = '/mnt/XDATA/GPS/gpsdata.txt' #Define targets, their acoustic channels, type, latitude, and longitude (Ivan) TARGET_CHANNELS = dict([('Near_Shore', (11, 'Manualmodem',36.82,-121.84)), \ ('CCE_BIN', (2, 'BIN',36.70178,-122.09388)), \ ('benthic_Rover', (3, 'Rover',35.13705552463214,-122.99263522552522))]) # Read WayPoints to follow (Ivan) WAYPOINTS = open("circle.txt","r") #WAYPOINTS = open("square.txt","r") #WAYPOINTS = open("triangle.txt","r") #WAYPOINTS = open("l.txt","r") # Target select STALK_CHANNEL = 11 # Radius of the zone to be considered the waypoint reached RADIUSREACHED = 25 TARGET_CHANNELS.keys() # Radius of stalker watch circle (meters) #STALK_CIRCLE_RADIUS = 100 # Safe operational area, defined as KML polygon SAFE_POLYGON_FILE = '/opt/hotspot/config/CANON-ops.kml' # String output by Benthos RESPONSE_NOT_RECEIVED = 'Response Not Received' RANGE_RESP_PREFIX = "Range" RANGE_RESP_SUFFIX = "m" # Acoustic address of the Benthos #WG_ACOUSTIC_ADDR = 10 # Warn if GPS time differs from system clock time by this amount ALLOWED_CLOCK_ERR_SEC = 60 # Interval at which to send locations to ODSS via email MAIL_INTERVAL_SEC = 30 # Interval at which to check sys clock against GPS time CLOCK_CHECK_INTERVAL_SEC = 300 # Time to pause between each loop LOOP_PAUSE_SEC = 10 # Create some pynmea objects _gpgga = nmea.GPGGA() _gpzda = nmea.GPZDA() #initial lat and lon prevLat = 0 prevLon = 0 # Print message if in debug mode def dprint(str): if DEBUG: print str # Using a decimal degree converter function from Jordan Stanway def degrees_minutes_to_decimal_degrees(dm): """Convert degrees, minutes, and hemisphere to decimal degrees. """ dm = dm.strip() dprint('dm =' + dm) boundary = dm.find('.') - 2 return float(dm[:boundary]) + float(dm[boundary:]) / 60. # Get our current latitude/longitude from GPS file, latest GPS time # Grab the latest GGA string from the file and clean it up # pynmea expects a GGA string in the following (and typical) format: # '$GPGGA,205024.00,3648.14354,N,12147.26031,W,2,08,1.27,8.7,M,-30.7,M,,0000*6F' # pynmea expects a ZDA string in the following (and typical) format: # '$GPZDA,205056.00,07,07,2014,00,00*65' # Return 0 if GPGGA and GPZDA sentences found, else return -1 def getGPS(): p = subprocess.Popen('tail -30 ' + GPS_FILE, \ shell=True, stdout=subprocess.PIPE, \ stderr=subprocess.STDOUT) gotGPGGA = False gotGPZDA = False for line in p.stdout.readlines(): # dprint('getGPS(): line = ' + line) if line.find('GPGGA') > -1: # Get the latest GGA string # dprint('found GPGGA: ' + line) line = '$' + line.lstrip('NMEA,') # Get rid of the SMC specific formatting # Parse the GPGGA data # dprint('parse line: ' + line) _gpgga.parse(line) # dprint('parsed') gotGPGGA = True if line.find('GPZDA') > -1: # Get the latest GGA string line = '$' + line.lstrip('NMEA,') # Get rid of the SMC specific formatting # Parse the GPZDA data _gpzda.parse(line) gotGPZDA = True if not gotGPGGA: print 'No GPGGA sentences found in ' + GPS_FILE if not gotGPZDA: print 'No GPZDA sentences found in ' + GPS_FILE # Need to close subprocess stdout? p.stdout.close() if gotGPGGA and gotGPZDA: return 0 else: return -1 # Read serial port until user prompt is found. Assume that '>' means user prompt for now def consumePrompt(serialPort): n = 0 limit = 100 while True: # Read a single character. If character is '>', assume we've got a prompt char = serialPort.read(size=1) if char == '>': dprint('found prompt') return True n += 1 if n >= limit: print 'ERROR: Could not find modem prompt' return False if IVANSIM == False: # Start GPS stream print 'Set GPS output file' os.system('ServerExec GPSListener \'GPS DEFAULTOUTPUT ' + GPS_FILE + '\'') print '\n' os.system('ServerExec GPSListener \'GPS OUTPUT ' + GPS_FILE + '\'') print '\nStart GPS stream' os.system('ServerExec GPSListener \'GPS START\'') print '\n' dprint('get modem\'s serial and power port') modemsConfigFile = '/opt/hotspot/config/modems.cfg' powerSerial = subprocess.check_output(['python', 'modemConfig.py', modemsConfigFile, 'dat']) dprint('powerSerial=' + powerSerial) arr=powerSerial.split(' ') powerPort = str(arr[0]).strip() serialPortName = arr[1].strip() print 'Benthos on serial port ' + serialPortName + ', power port ' + powerPort print '(from ' + modemsConfigFile + ')' # Power cycle the Benthos to put device in known state print 'Power cycle the Benthos:' delay = 12 print 'Turn off the Benthos, wait ' + str(delay) + ' seconds for power-down' subprocess.call(['SetPowerSwitch', powerPort, '0']) time.sleep(delay) print 'Turn on Benthos:' subprocess.call(['SetPowerSwitch', powerPort, '1']) time.sleep(5) print 'Turn off the Benthos, wait ' + str(delay) + ' seconds for power-down' subprocess.call(['SetPowerSwitch', powerPort, '0']) time.sleep(delay) print 'Turn on Benthos:' subprocess.call(['SetPowerSwitch', powerPort, '1']) # Open Benthos serial port serialPort = serial.Serial(port=serialPortName, baudrate=9600, timeout=20) print 'Wait for Benthos CONNECT message' tries = 0 while True: try: resp = serialPort.readline() resp = resp.rstrip('\n') dprint('Benthos msg len: ' + str(len(resp)) + ', msg: ' + resp) print 'from Benthos: ' + resp if resp.find('CONNECT ') != -1: dprint('Got connect msg: ' + resp) break if tries > 10: print 'ERROR: No connect message from Benthos' sys.exit(1) except OSError as e: print 'Caught OSError' + str(e.errno) + \ ' While rading acoustic port; trying again...' time.sleep(0.5) tries += 1 time.sleep(3) serialPort.flush() dprint('Get Benthos user prompt') serialPort.write('+++') consumePrompt(serialPort) lastEmailTime = 0 lastClockCheckTime = 0 # Alarm if we lose acoustic track #trackAlarmArmed = True #nMissedAcousticResp = 0 # Consecutive acoustic misses that trigger an alarm #MISSED_RESP_THRESHOLD = 15 # Alarm if target coords are outside of safe polygon stalkAlarmArmed = True #Variables to calculate the target position eastingpoints = [] northingpoints = [] rangepoints = [] allerror = [] allerror.append([]) allerror.append([]) allerror.append([]) # Target select (Ivan) found = False targets = TARGET_CHANNELS.keys() for target in targets: channel, targetType, targetlat, targetlon = TARGET_CHANNELS[target] if channel == STALK_CHANNEL: print ('\nGet range to ' + targetType + ' \'' + target + '\' on channel ' + \ str(channel) + ' at lat= ' +str(targetlat)+ ' and lon= ' +str(targetlon)+ ':') found = True break if found == False: print ('ERROR: STALK_CHANNEL ('+str(STALK_CHANNEL)+') not found in TARGET_CHANNELS') sys.exit(1) # log file initialization #UTC time if IVANSIM == True: logname = 'ranges_'+time.strftime('%Y%m%d%H%M',time.gmtime())+'.log' else: logname = '/opt/hotspot/logs/ranges_'+time.strftime('%Y%m%d%H%M',time.gmtime())+'.log' if os.path.isdir(logname): print 'ERROR: ' + logname + ' is a directory' sys.exit(1) else: if os.path.exists(logname): print 'append to log file ' + logname else: print 'create new log file ' + logname # File doesn't exist yet - write header dprint ('Write header to ' + logname) logfile = open(logname, 'w') logfile.write('# epochSec target easting northing range utmZone\n') logfile.close() nLogEntries = 0 if IVANSIM == True: # Initial lat0 and lon0 lat0 = targetlat lon0 = targetlon #Init Variables for simulation eastingwgutmposition = [] northingwgutmposition = [] eastingwputmposition = [] northingwputmposition = [] #Time interval between adquisitions LOOP_PAUSE_SEC = 0.01 # UTM position of target tuple = utm.from_latlon(targetlat, targetlon) easting, northing, zonenumber, zoneletter = tuple t_position=np.matrix([easting, northing]).T #init firstwp = True ########## Main ############################################################### for wp in WAYPOINTS: #Flush buffer if IVANSIM == False: time.sleep(3) serialPort.flushInput() ########## Read WayPoint ###################################################### #Get information of the next WP from lines (Ivan) dprint ('Get information of the next WP from lines') wp = wp.split(',') numberwp = int(wp[0]) eastingwp = float(wp[1]) northingwp = float(wp[2]) radiuswp = float(wp[3]) if radiuswp == 0: runcircle = False radiusstalker = 25 else: radiusstalker = radiuswp #Convert latlon to utm tuple = utm.from_latlon(targetlat, targetlon) easting, northing, zonenumber, zoneletter = tuple wp_utm_position=np.matrix([easting+eastingwp, northing+northingwp]).T #Convert utm to latlon tuple = utm.to_latlon(wp_utm_position[0], wp_utm_position[1],zonenumber,zoneletter) lat, lon = tuple wp_latlon_position=np.matrix([lat, lon]).T print('\nWG goes to NumberWP=' +str(numberwp)+ \ ' latitude=' +str(round(wp_latlon_position[0],5))+ \ ' longitude=' +str(round(wp_latlon_position[1],5))+ ' radius=' +str(radiuswp)) #Init point for WG if firstwp == True and IVANSIM == True: wg_utm_position=np.matrix([easting+eastingwp+1, northing+northingwp]).T firstwp = False ########## Stop Read WayPoint ################################################# ########## GPS read ########################################################### # Get our current location and GPS time dprint ('Get our current location and GPS time') if IVANSIM == False: if getGPS() == -1: print 'ERROR: missing GPGGA and/or GPZDA data in ' + GPS_FILE continue try: lat0 = degrees_minutes_to_decimal_degrees(_gpgga.latitude) north_latitude = (_gpgga.lat_direction == 'N')*2 - 1 lat0 *= north_latitude lon0 = degrees_minutes_to_decimal_degrees(_gpgga.longitude) east_longitude = (_gpgga.lon_direction == 'E')*2 - 1 lon0 *= east_longitude if lat0 == prevLat and lon0 == prevLon: print 'WARNING: Location not changed; GPS not updating?' prevLat = lat0 prevLon = lon0 except ValueError, e: print 'ERROR: Invalid lat and/or lon: ' + str(_gpgga) continue else: if radiuswp != 0: #circle path norm = radiuswp incangle = 2*np.pi/6 angle = 0 eastingcenter = wp_utm_position[0] northingcenter = wp_utm_position[1] else: #Calculate the steps for simulation in non cirle path norm = wp_utm_position-wg_utm_position norm = np.sqrt(norm.T*norm).item(0) norm_aux = np.round(norm/25,0) + 1. norm = norm/norm_aux angle = np.arctan((wp_utm_position-wg_utm_position).item(1)/(wp_utm_position-wg_utm_position).item(0)) if (wp_utm_position-wg_utm_position)[0] < 0: angle = angle + np.pi incnorthing = round(norm * np.sin(angle),2) inceasting = round(norm * np.cos(angle),2) dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0)) ########## Stop GPS read ###################################################### ########## Set Stalker position ############################################### if IVANSIM == False: ret = trackerStalker.stalk(lat0, lon0, \ wp_latlon_position.item(0), \ wp_latlon_position.item(1), \ radiusstalker, \ SAFE_POLYGON_FILE, \ stalkAlarmArmed) if ret != 0: # trackerStalker.stalk() got error - now in alarm state stalkAlarmArmed = False else: # trackerStalker.stalk() ran ok - send alarm on next error stalkAlarmArmed = True else: print ('Set the coordinates to the Wave Glider') ########## Stop Set Stalker position ########################################## firsttime = True ######### Start going to WP until it reach #################################### #Keep the WP until it reach (Ivan) while (True): ########## GPS read ########################################################### if IVANSIM == False: # Get our current location and GPS time if getGPS() == -1: print 'ERROR: missing GPGGA and/or GPZDA data in ' + GPS_FILE continue try: lat0 = degrees_minutes_to_decimal_degrees(_gpgga.latitude) north_latitude = (_gpgga.lat_direction == 'N')*2 - 1 lat0 *= north_latitude lon0 = degrees_minutes_to_decimal_degrees(_gpgga.longitude) east_longitude = (_gpgga.lon_direction == 'E')*2 - 1 lon0 *= east_longitude if lat0 == prevLat and lon0 == prevLon: print 'WARNING: Location not changed; GPS not updating?' prevLat = lat0 prevLon = lon0 except ValueError, e: print 'ERROR: Invalid lat and/or lon: ' + str(_gpgga) continue dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0)) else: if radiuswp != 0 or runcircle == True: #circle wg_utm_position[1] = northingcenter + norm*np.sin(angle) wg_utm_position[0] = eastingcenter + norm*np.cos(angle) angle = angle + incangle tuple=utm.to_latlon(wg_utm_position[0], wg_utm_position[1],zonenumber,zoneletter) lat0, lon0 = tuple print(norm*np.sin(angle)) runcircle = True else: #path wg_utm_position[1] = wg_utm_position[1] + incnorthing wg_utm_position[0] = wg_utm_position[0] + inceasting tuple=utm.to_latlon(wg_utm_position[0], wg_utm_position[1],zonenumber,zoneletter) lat0, lon0 = tuple ########## Stop GPS read ###################################################### ######### Range read ########################################################## if IVANSIM == False: time.sleep(2) serialPort.flushInput() if SIMULATED: print 'USING SIMULATED COMMAND' serialPort.write('dat\r\n') else: # Issue range command serialPort.write('atr' + str(channel) + '\r\n') #serialPort.write('usbl ' + str(channel) + ' 1\r\n') resp = serialPort.readline() # Eat echo'ed newline """ Try to get range response. Possible responses other than range include: 1. Target may not respond at all 2. May get Bearing response, if host modem has USBL 3. May get two Bearing responses if target modem also has USBL """ gotRange = False for n in range(1, 4): resp = serialPort.readline().rstrip() dprint('resp: ' + resp) # Debug - Print hex representation of string print ':'.join('{:02x}'.format(ord(c)) for c in resp) if resp == RESPONSE_NOT_RECEIVED: print 'WARNING: response not received from ' + target break if resp.find(RANGE_RESP_PREFIX) != -1: dprint('Got range response') gotRange = True timestamp = time.time() break if gotRange == False: print 'WARNING: did not get range response from ' + target continue # Try next target dprint('resp = ' + resp) # Parse target's range from 'Range to : m' start = resp.find(':') end = resp.find(RANGE_RESP_SUFFIX, start) if start == -1 or end == -1: print 'ERROR: couldn\'t parse range from \"' + resp + '\"' continue slantRange = float(resp[start+1:end]) consumePrompt(serialPort) else: #Range plus +/-1m of random noise plus +/-1m random noise*slantRange simdepth = 4000. slantRange = wg_utm_position - t_position slantRange = np.sqrt(slantRange.T * slantRange).item(0) slantRange = np.sqrt(np.power(slantRange,2) + np.power(simdepth,2)) + np.random.normal(0,0.5) + 0*np.sqrt(np.power(slantRange,2) + np.power(simdepth,2))/4000 timestamp = time.time() print (np.sqrt(np.power(slantRange,2) + np.power(simdepth,2))) dprint('slantRange: ' + str(slantRange)) ########## Stop Range read #################################################### ########## See if WP is reached ############################################### #WG current position tuple = utm.from_latlon(lat0, lon0) easting, northing, zonenumber, zoneletter = tuple wg_utm_position=np.matrix([easting, northing]).T #Distance between WG and WP d = wg_utm_position - wp_utm_position d = round(np.sqrt(d.T*d).item(0),2) print('Distance between WG and WP is ' +str(d)+ 'm') if firsttime == False and d > RADIUSREACHED+10: #Wave Glaider is now out of the reached radius radiuswp = 0 if d < radiuswp+RADIUSREACHED and d>radiuswp-RADIUSREACHED and radiuswp != 0 and firsttime == True: #Wave Glaider is on the circle for first time firsttime = False wp_utm_position = wg_utm_position #Define current position as a new waypoint elif d <= RADIUSREACHED and radiuswp == 0: #Waypoint is reached. Only for noncircle path print ('Waypoint ' +str(numberwp)+ ' is reached') break ########## Compute target position ############################################ #Points selected as anchor nodes eastingpoints = np.append(eastingpoints, easting) northingpoints = np.append(northingpoints, northing) rangepoints = np.append(rangepoints, slantRange) numpoints=eastingpoints.size dprint ('Num points for LS = '+str(numpoints)) #substract offset eastingpoints = eastingpoints - t_position.item(0) northingpoints = northingpoints - t_position.item(1) #Unconstrained Least Squares (LS-U) algorithm 2D #/P_LS-U = N0* = N(A^T A)^-1 A^T b #where: P=np.matrix([eastingpoints,northingpoints]) # N is: N = np.concatenate((np.identity(2),np.matrix([np.zeros(2)]).T),axis=1) # A is: A = np.concatenate((2*P.T,np.matrix([np.zeros(numpoints)]).T-1),axis=1) # b is: b = np.matrix([np.diag(P.T*P)-rangepoints*rangepoints]).T # Then using the formula "/P_LS-U" the position of the target is: try: Plsu = N*(A.T*A).I*A.T*b except np.linalg.LinAlgError: #not invertible. skip this one. pass else: # Finally we calculate the depth as follows r=np.matrix(np.power(rangepoints,2)).T a=np.matrix(np.power(Plsu[0]-eastingpoints,2)).T b=np.matrix(np.power(Plsu[1]-northingpoints,2)).T depth = np.sqrt(np.abs(r-a-b)) depth = np.mean(depth) Plsu = np.concatenate((Plsu.T,np.matrix(depth)),axis=1).T #add offset Plsu[0] = Plsu[0] + t_position.item(0) Plsu[1] = Plsu[1] + t_position.item(1) eastingpoints = eastingpoints + t_position.item(0) northingpoints = northingpoints + t_position.item(1) if IVANSIM == True: #Error in 'm' error = np.concatenate((t_position.T,np.matrix(simdepth)),axis=1).T - Plsu allerror = np.append(allerror,error,axis=1) ########## Stop Compute target position ####################################### ########## Send position by Email ############################################# #Check for send email if time.time() - lastEmailTime > MAIL_INTERVAL_SEC and ENABLE_ODSS_MAIL and IVANSIM == False: tstr = time.gmtime(timestamp) print 'ODSS format: targ,chan,time,lon0,lat0,range,timestring' print 'RESULT: ' + target + ',' + str(channel) + ',' + \ str(int(timestamp)) + ',' + \ str(round(lon0, 5)) + ',' + \ str(round(lat0, 5)) + ',' + \ str(round(slantRange, 2)) + ',' + \ ',' + time.strftime('%Y-%m-%dT%H:%M:%S',tstr) dprint('email locations to ODSS on this cycle') subject = target + ',' + str(int(timestamp)) + ',' + \ str(round(lon0, 5)) + ',' + \ str(round(lat0, 5)) recips = targetType + 'track@mbari.org' print 'Sending ' + target + ' position to ODSS' cmd = 'python pymail.py hotspot-noreply@myzw.com ' + \ subject + ' ' + \ subject + ',' + time.strftime('%Y-%m-%dT%H:%M:%S',tstr) + \ ' ' + \ recips; # Send mail in background process ('&') os.system(cmd + "&") lastEmailTime = time.time() elif not ENABLE_ODSS_MAIL: print 'ODSS mail not enabled (ENABLE_ODSS_MAIL = False)' else: dprint('do not email locations to ODSS on this cycle') ########## Stop Send position by Email ######################################## ########## Save data to log file ############################################## print 'logname: ' + logname try: print 'log entry #' + str(nLogEntries) logfile = open(logname, 'a') logfile.write(str(int(timestamp)) + ',' + target + ',' + str(easting) + ',' + str(northing) + ',' + str(slantRange) + ',' + str(zonenumber) + "\n") nLogEntries += 1 except KeyboardInterrupt: print('Got keyboard interrupt during log write - keep going') finally: logfile.close() # targ,chan,time,lon0,lat0,range,bearing,elev,lonTarg,latTarg,timestring print 'RESULT: ' + target + ',' + str(channel) + ',' + \ str(int(timestamp)) + ',' + \ str(round(lon0, 5)) + ',' + \ str(round(lat0, 5)) + ',' + \ str(round(slantRange, 2)) ########## Stop Save data to log file ######################################### ########## Check clock drift ################################################## if IVANSIM == False: # Check system clock against GPS time, at specified interval if timestamp - lastClockCheckTime > CLOCK_CHECK_INTERVAL_SEC: lastClockCheckTime = timestamp tmp = str(_gpzda.year) + '-' + \ str(_gpzda.month) + '-' + \ str(_gpzda.day) + 'T' + \ str(_gpzda.timestamp) # Parse the GPS timestamp gpsTime = time.strptime(tmp, '%Y-%m-%dT%H%M%S.%f') print 'GPS time: ' + time.strftime('%Y-%m-%dT%H:%M:%S', gpsTime) gpsTimestamp = calendar.timegm(gpsTime) dprint('GPS timestamp: ' + tmp + ' struct: ' + str(gpsTime)) dprint('gpsTimestamp=' + str(gpsTimestamp)) if abs(timestamp - gpsTimestamp) > ALLOWED_CLOCK_ERR_SEC: print 'WARNING: GPS time minus system clock time = ' + \ str(gpsTimestamp - timestamp) + ' sec' else: print 'sys clock offset from GPS: ' str(timestamp - gpsTimestamp) + ' sec' ########## Stop Check clock drift ############################################# ########## Sleep ############################################################## # Sleep before next loop print 'Wait ' + str(LOOP_PAUSE_SEC) + ' sec before next loop' time.sleep(LOOP_PAUSE_SEC) #Plot the simulation # if IVANSIM == True: # eastingwgutmposition=np.append(eastingwgutmposition,wg_utm_position[0]) # northingwgutmposition=np.append(northingwgutmposition,wg_utm_position[1]) # eastingwputmposition=np.append(eastingwputmposition,wp_utm_position[0]) # northingwputmposition=np.append(northingwputmposition,wp_utm_position[1]) # plt.figure(figsize=(5,5)) # plt.plot(eastingwgutmposition,northingwgutmposition, 'bo', eastingwputmposition, northingwputmposition, 'yo', # wp_utm_position[0],wp_utm_position[1], 'ro', t_position[0],t_position[1], 'r^') # if runcircle == True: # plt.plot(eastingcenter,northingcenter, 'yo') # plt.plot(Plsu.item(0), Plsu.item(1), 'g.') # plt.show() ########## Stop Sleep ######################################################### # Close the Benthos serial port if IVANSIM == False: serialPort.close() WAYPOINTS.close() #Ivan dprint('Serial port closed')