# -*- 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 import geopy from geopy.distance import VincentyDistance # 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 = False # 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([('Nearshore', (11, 'Manualmodem',36.81359,-121.82074)), \ ('CCE_BIN', (9, 'BIN',36.70178,-122.09388)), \ ('StationM_SES', (2, 'SES', 35.1585,-122.98383)), \ ('benthic_Rover_MF', (3, 'Rover',35.1368,-122.9823)), \ ('benthic_Rover', (0, 'Rover',35.1368,-122.9823))]) # Radius of the zone to be considered the waypoint reached RADIUSREACHED = 25 # Angle in degrees between ranges in a circle path ANGLERANGE = 10 # Distance in meters between ranges in a noncircle path DISTANCERANGE = 20 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' #BEARING_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 if IVANSIM == False: # Read parameters from user (Ivan) arg = sys.argv[1:] try: print ('Target to follw is '+str(int(arg[0]))) # Target select STALK_CHANNEL = int(arg[0]) #WayPoints to follow print ('Path to follow is '+arg[1]) WAYPOINTS = open(arg[1],"r") except IndexError: print ('Error: too few arguments\nIt expected: sudo trackingall.py ChanelNumber Path') exit() else: # WAYPOINTS = open("circle.txt","r") # WAYPOINTS = open("circle_d100.txt","r") # WAYPOINTS = open("circle_d200.txt","r") # WAYPOINTS = open("circle_d300.txt","r") # WAYPOINTS = open("circle_d400.txt","r") # WAYPOINTS = open("circle_d500.txt","r") # WAYPOINTS = open("circle_r50.txt","r") # WAYPOINTS = open("circle_r100.txt","r") # WAYPOINTS = open("circle_r200.txt","r") # WAYPOINTS = open("circle_r400.txt","r") WAYPOINTS = open("calpath3.txt","r") # WAYPOINTS = open("square.txt","r") # WAYPOINTS = open("triangle.txt","r") #WAYPOINTS = open("l.txt","r") # WAYPOINTS = open("cst.txt","r") STALK_CHANNEL = 11 # 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 # Parse bearing and elevation from instrumnent response def parseBearing(bearingResp): print 'bearingResp: ' + bearingResp #find RAW start = bearingResp.find('RAW=') if start == -1: raise Exception('Cannot find \'RAW\' in \'' + bearingResp + '\'') comma = bearingResp.find(',', start+4) b_raw = float(bearingResp[start+4:comma]) comma2 = bearingResp.find(',', comma+1) e_raw = float(bearingResp[comma+1:comma2]) print 'bearing_RAW: ' + str(b_raw) + ', elev_RAW: ' + str(e_raw) #find CAL start = bearingResp.find('CAL=') if start == -1: raise Exception('Cannot find \'CAL\' in \'' + bearingResp + '\'') comma = bearingResp.find(',', start+4) b_cal = float(bearingResp[start+4:comma]) comma2 = bearingResp.find(',', comma+1) e_cal = float(bearingResp[comma+1:comma2]) print 'bearing_CAL: ' + str(b_cal) + ', elev_CAL: ' + str(e_cal) #find ROT start = bearingResp.find('ROT=') if start == -1: raise Exception('Cannot find \'ROT\' in \'' + bearingResp + '\'') comma = bearingResp.find(',', start+4) b_rot = float(bearingResp[start+4:comma]) comma2 = bearingResp.find(',', comma+1) e_rot = float(bearingResp[comma+1:comma2]) print 'bearing_ROT: ' + str(b_rot) + ', elev_ROT: ' + str(e_rot) #find CMP start = bearingResp.find('CMP=') if start == -1: raise Exception('Cannot find \'CMP\' in \'' + bearingResp + '\'') comma = bearingResp.find(',', start+4) h_cmp = float(bearingResp[start+4:comma]) comma2 = bearingResp.find(',', comma+1) p_cmp = float(bearingResp[comma+1:comma2]) comma3 = bearingResp.find(',', comma2+1) r_cmp = float(bearingResp[comma2+1:comma3]) print 'heading_CMP: ' + str(h_cmp) + ', pitch_CMP: ' + str(p_cmp) + ', roll_CMP: ' + str(r_cmp) #find BRG start = bearingResp.find('BRG=') if start == -1: raise Exception('Cannot find \'BRG\' in \'' + bearingResp + '\'') comma = bearingResp.find(',', start+4) b_brg = float(bearingResp[start+4:comma]) comma2 = bearingResp.find(',', comma+1) e_brg = float(bearingResp[comma+1:comma2]) print 'bearing_RBRG: ' + str(b_brg) + ', elev_BRG: ' + str(e_brg) #find SPD start = bearingResp.find('SPD=') if start == -1: raise Exception('Cannot find \'SPD\' in \'' + bearingResp + '\'') d_spd = float(bearingResp[start+4:]) print 'doppler_spd: ' + str(d_spd) return (b_raw,e_raw,b_cal,e_cal,b_rot,e_rot,h_cmp,p_cmp,r_cmp,b_brg,e_brg,d_spd) def DATread(channel,target): if IVANSIM == False: time.sleep(2) serialPort.flushInput() if SIMULATED: print 'USING SIMULATED COMMAND' serialPort.write('dat\r\n') slantRange = float(11.11) timestamp = time.time() else: # Issue range command # serialPort.write('atr' + str(channel) + '\r\n') #next code is to configure the @DatVerbose to adquiere all information dprint('Start modem configuration 2') dprint('Get DAT user prompt') serialPort.write('+++\r\n') consumePrompt(serialPort) serialPort.flushInput() serialPort.write('dat off\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat off\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +raw\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +raw\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +cal\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +cal\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +rot\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +rot\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +cmp\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +cmp\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +brg\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +brg\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +spd\r\n') serialPort.readline() resp = serialPort.readline() serialPort.readline() dprint('Write: dat +spd\nResponse: ' + resp) time.sleep(1) dprint('Modem configurated') dprint('usbl command') serialPort.write('usbl ' + str(channel) + ' 1\r\n') bearingResp = 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 """ gotBearing = False for n in range(1, 4): bearingResp = serialPort.readline().rstrip() dprint('bearingResp: ' + bearingResp) # Debug - Print hex representation of string ### dprint (':'.join('{:02x}'.format(ord(c)) for c in bearingResp)) if bearingResp == RESPONSE_NOT_RECEIVED: print 'WARNING: response not received from ' + target # continue else: dprint('Got bearing response') gotBearing = True timestamp = time.time() break # if gotBearing == False: print 'WARNING: did not get bearing response from ' + target return ('ERROR') else: # Range string is on a line of its own rangeResp = serialPort.readline() dprint('rangeResp = ' + rangeResp) dprint('Start modem configuration') time.sleep(1) serialPort.flushInput() serialPort.write('dat off\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat off\nResponse: ' + resp) dprint('Modem configurated') dprint('atr command') time.sleep(1) serialPort.flushInput() serialPort.write('atr' + str(channel) + '\r\n') bearingResp2 = serialPort.readline() # Eat echo'ed newline dprint('eaten newline? ' + bearingResp2) gotBearing = False for n in range(1, 4): bearingResp2 = serialPort.readline().rstrip() dprint('bearingResp2: ' + bearingResp2) # Debug - Print hex representation of string ## dprint (':'.join('{:02x}'.format(ord(c)) for c in bearingResp2)) if bearingResp2 == RESPONSE_NOT_RECEIVED: print 'WARNING: response not received from ' + target # continue else: dprint('Got bearing response') gotBearing = True timestamp2 = time.time() break # if gotBearing == False: print 'WARNING: did not get bearing response from ' + target return ('ERROR') else: # Range string is on a line of its own rangeResp2 = serialPort.readline() dprint('rangeResp2 = ' + rangeResp2) if IVANSIM == True: #Range plus +/-1m of random noise plus +/-1m random noise*slantRange timestamp = time.time() timestamp2 = time.time()+2 bearingResp = 'simulated bearingresp from target' rangeResp = 'simulated rangeresp from target' bearingResp2 = 'second simulation of bearing' rangeResp2 = 'second simulation of range' return (timestamp, bearingResp, rangeResp, timestamp2, bearingResp2, rangeResp2) def GPSread (initgpspoint): global runcircle global radiuswp global e global IVANSIM global wp_utm_position global northingcenter global norm global angle global eastingcenter global incangle global incnorthing global inceasting global prevLat global prevLon # Get our current location and GPS time print 'initgpspoint: ' + str(initgpspoint) 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 dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0)) return (lat0,lon0) else: if initgpspoint == True: if radiuswp != 0: #circle path norm = radiuswp incangle = 2*np.pi/60 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) return(targetlat,targetlon) 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 # dprint(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 dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0)) return (lat0,lon0) 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 DAT\'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 'DAT 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 DAT, wait ' + str(delay) + ' seconds for power-down' subprocess.call(['SetPowerSwitch', powerPort, '0']) time.sleep(delay) print 'Turn on DAT:' subprocess.call(['SetPowerSwitch', powerPort, '1']) # Open DAT serial port serialPort = serial.Serial(port=serialPortName, baudrate=9600, timeout=20) print 'Wait for DAT CONNECT message' tries = 0 while True: try: resp = serialPort.readline() resp = resp.rstrip('\n') dprint('DAT msg len: ' + str(len(resp)) + ', msg: ' + resp) print 'from DAT: ' + resp if resp.find('CONNECT ') != -1: dprint('Got connect msg: ' + resp) break if tries > 10: print 'ERROR: No connect message from DAT' 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 DAT user prompt') serialPort.write('+++') consumePrompt(serialPort) #next code is to configure the @DatVerbose to adquiere all information dprint('Start modem configuration 1') serialPort.flushInput() serialPort.write('dat off\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat off\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +raw\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +raw\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +cal\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +cal\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +rot\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +rot\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +cmp\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +cmp\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +brg\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +brg\nResponse: ' + resp) time.sleep(1) serialPort.flushInput() serialPort.write('dat +spd\r\n') serialPort.readline() resp = serialPort.readline() dprint('Write: dat +spd\nResponse: ' + resp) time.sleep(1) dprint('Modem configurated') 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 = 'datinfo_'+time.strftime('%Y%m%d%H%M',time.gmtime())+'.log' else: logname = '/opt/hotspot/logs/datinfo_'+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 RAW(b,e) CAL(b,e) ROT(b,e) CMP(h,p,r) BRG(b,e) SPD(d) Easting Northing Range ZoneNumber\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.1 radiuswp = 0 wp_utm_position = np.matrix([[0],[0],[0]]) wg_utm_position = np.matrix([[1],[1],[1]]) t_position = np.matrix([[1],[1],[1]]) ############## Set the initial target position ############################### #lat0,lon0 = GPSread(True) #try: # timestamp, bearElev, slantRange = DATread(channel,target) #except ValueError: # sys.exit('Error from DAT') # ## compute the target position ## Use Vincenty's algorithm to compute lat/lon of target based on our ## lat/lon, range and bearing to target. #bearing = bearElev[9] #elevDeg = bearElev[10] #elevRad = elevDeg*np.pi/180. #origin = geopy.Point(lat0, lon0) #print 'NOTE - using slantRange instead of surfaceRange' #targetPos = \ # VincentyDistance(meters=slantRange).destination(origin, bearing) #dprint('target: lat ' + str(targetPos.latitude) + \ # ', lon ' + str(targetPos.longitude)) #targetlat=targetPos.latitude #targetlon=targetPos.longitude # 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 startranges = False pointb = t_position pointa = t_position sc = 1 sb = 1 angle2_before = 0 incnumturns = False numturns = 0 nextanglerange = ANGLERANGE sdbefore = 0 firstwpreach = False angledone=0 slantRange = -2 ########## 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 runcircle = True #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 pointa = wp_utm_position #used to compute the angle completed ########## GPS read ########################################################### lat0,lon0 = GPSread(True) ########## 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 ########################################################### lat0,lon0 = GPSread(False) ########## 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 #Calcul the distance between WG and WP in a non circle path sd = wg_utm_position - wp_utm_position sd = round(np.sqrt(sd.T*sd).item(0),2) #Calcul the angle done by WG in a circle path if runcircle == True: if firsttime == False: sb = wg_utm_position - pointa sb = round(np.sqrt(sb.T*sb).item(0),2) sa = wg_utm_position - pointb sa = round(np.sqrt(sa.T*sa).item(0),2) angle2 = np.arccos((sa**2-sb**2-sc**2)/(-2*sb*sc)) angle2 = round(angle2*180/np.pi,2) if angle2_before > angle2: angledone = 180 + (180-angle2) + 360*numturns incnumturns = True else: if incnumturns == True: numturns = numturns + 1 incnumturns = False angledone = angle2 + 360*numturns angle2_before = angle2 print ('Angle completed: ' +str(angledone)+ ' degrees' ) #chose a new range point if angledone >= nextanglerange: nextanglerange = nextanglerange + ANGLERANGE startranges = True else: print('Distance between WG and Circle path is ' +str(sd-radiuswp)+ ' m') else: if firstwpreach == True: #chose a new range point if (sdbefore - sd) >= DISTANCERANGE or sdbefore - sd < 0 : startranges = True sdbefore = sd print('Distance between WG and WP'+str(numberwp)+' is ' +str(sd)+ ' m') if firsttime == False and sd > RADIUSREACHED+20: #Wave Glaider is now out of the reached radius radiuswp = 0 if sd < radiuswp+RADIUSREACHED and sd>radiuswp-RADIUSREACHED and radiuswp != 0 and firsttime == True: #Wave Glaider is on the circle for first time print('WG is on the circle for first time') firsttime = False pointb = wg_utm_position sc = wg_utm_position - wp_utm_position sc = round(np.sqrt(sc.T*sc).item(0),2) wp_utm_position = wg_utm_position #Define current position as a new waypoint startranges = True elif (sd <= RADIUSREACHED and radiuswp == 0) or (angledone >360 and radiuswp == 0): #Waypoint is reached firstwpreach = True print ('Waypoint ' +str(numberwp)+ ' is reached') startranges = True break ######### End if WP is reached ################################################ if startranges == True: ######### Range read ########################################################## try: timestamp1, bearingResp1, rangeResp1, timestamp22, bearingResp22, rangeResp22 = DATread(channel,target) except ValueError: print 'Error from DAT' continue ########## Compute target position #################################################### # # compute the target position # # Use Vincenty's algorithm to compute lat/lon of target based on our # # lat/lon, range and bearing to target. # bearing = bearElev[9] # elevDeg = bearElev[10] # elevRad = elevDeg*np.pi/180. # origin = geopy.Point(lat0, lon0) # print 'NOTE - using slantRange instead of surfaceRange' # targetPos = \ # VincentyDistance(meters=slantRange).destination(origin, bearing) # # dprint('target: lat ' + str(targetPos.latitude) + \ # ', lon ' + str(targetPos.longitude)) ########## Save data to log file ############################################## dprint ('logname: ' + logname) try: print 'log entry #' + str(nLogEntries) logfile = open(logname, 'a') #b_raw,e_raw,b_cal,e_cal,b_rot,e_rot,h_cmp,p_cmp,r_cmp,b_brg,e_brg,d_spd logfile.write(str(int(timestamp1)) + ',' + target + '\n' + bearingResp1 + '\n' + rangeResp1 + '\n' + str(int(timestamp22)) + ',' + target + '\n' + bearingResp22 + '\n' + rangeResp22 + '\n' + str(easting) + ',' + str(northing) + ',' + str(zonenumber) + "\n") nLogEntries += 1 logfile.close() 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 ########## Stop Save data to log file ######################################### ########## Check clock drift ################################################## if IVANSIM == False: # Check system clock against GPS time, at specified interval if timestamp1 - lastClockCheckTime > CLOCK_CHECK_INTERVAL_SEC: lastClockCheckTime = timestamp1 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(timestamp1 - gpsTimestamp) > ALLOWED_CLOCK_ERR_SEC: print 'WARNING: GPS time minus system clock time = ' + \ str(gpsTimestamp - timestamp1) + ' sec' else: print 'sys clock offset from GPS: ' str(timestamp1 - gpsTimestamp) + ' sec' ########## Stop Check clock drift ############################################# ########## Sleep ############################################################## # Sleep before next loop print 'Wait ' + str(LOOP_PAUSE_SEC) + ' sec before next loop\n' time.sleep(LOOP_PAUSE_SEC) # #Plot the simulation # if IVANSIM == True and startranges == 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.show() startranges=False ########## Stop Sleep ######################################################### # Close the Benthos serial port if IVANSIM == False: serialPort.close() WAYPOINTS.close() #Ivan dprint('Serial port closed')