#!/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 ONLY APPROPRIATE TO A BENTHOS MODEM, *NOT* A DAT. This script is designed to measure link quality to a specified target. 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 io import calendar import math import geopy import utm from geopy.distance import VincentyDistance import os import sys import argparse import sys # Set _simulated=False when we have actual targets to range to _simulated = False # Print additional messages if _debug is true _debug = True _hasUsbl = True _logName = '' _nLogEntries = 0 _prevLat = 0. _prevLon = 0. # GPS data file _gpsFile = '/mnt/XDATA/GPS/gpsdata.txt' # String output by Benthos RESPONSE_NOT_RECEIVED = 'Response Not Received' RANGE_RESP_PREFIX = "Range=" RANGE_RESP_SUFFIX = "Meters" # Warn if GPS time differs from system clock time by this amount ALLOWED_CLOCK_ERR_SEC = 10 # Interval at which to check sys clock against GPS time CLOCK_CHECK_INTERVAL_SEC = 300 # Time to pause between each loop LOOP_PAUSE_SEC = 60 # Create some pynmea objects _gpgga = nmea.GPGGA() _gpzda = nmea.GPZDA() def main(argv): parser = argparse.ArgumentParser(description='Log acoustic link quality') parser.add_argument('-a', '--append', help='append to log file', action='store_true') parser.add_argument('targetName', help='target name') parser.add_argument('targetAddr', help='target address') parser.add_argument('logName', help='logfile for easting, northing, and range') args = parser.parse_args() global _logName _logName = args.logName global _targetName _targetName = args.targetName global _targetAddr _targetAddr = args.targetAddr print 'targetName=' + _targetName + ', targetAddr=' + str(_targetAddr) if args.append: print 'append to log file ' + _logName else: print 'create new log file ' + _logName if os.path.isdir(_logName): print 'ERROR: ' + _logName + ' is a directory' sys.exit(1) if not args.append or not os.path.isfile(_logName): # File doesn't exist yet - write header print 'Write header to ' + _logName logfile = open(_logName, 'w') logfile.write('# epochSec target easting northing range utmZone\n') logfile.close() # 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 ' + _gpsFile, \ 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 ' + _gpsFile if not gotGPZDA: print 'No GPZDA sentences found in ' + _gpsFile # 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 Benthos prompt' return False if __name__ == "__main__": main(sys.argv[1:]) # Start GPS stream print 'Set GPS output file' os.system('ServerExec GPSListener \'GPS DEFAULTOUTPUT ' + _gpsFile + '\'') print '\n' os.system('ServerExec GPSListener \'GPS OUTPUT ' + _gpsFile + '\'') print '\nStart GPS stream' os.system('ServerExec GPSListener \'GPS START\'') print '\n' dprint('get Benthos\' 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']) # Open Benthos serial port serialPort = serial.Serial(port=serialPortName, baudrate=9600, timeout=20) print 'Wait for CONNECT message' tries = 0 while True: try: resp = serialPort.readline() resp = resp.rstrip('\n') 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 reading acoustic port; trying again...' time.sleep(0.5) tries += 1 time.sleep(3) serialPort.flush() print 'Get Benthos user prompt' serialPort.write('+++') consumePrompt(serialPort) lastClockCheckTime = 0 # Set verbosity, etc serialPort.write('ats13=2\r'); while True: time.sleep(3) serialPort.flushInput() print '\nGet link quality to ' + _targetName + ' on channel ' + \ str(_targetAddr) + ':' time.sleep(2) serialPort.flushInput() # Issue link test command serialPort.write('atx' + str(_targetAddr) + '\r\n') resp = serialPort.readline() # Eat echo'ed newline # Read command response resp = serialPort.readline().rstrip() dprint('resp: ' + resp) if resp == RESPONSE_NOT_RECEIVED: print 'WARNING: response not received from ' + _targetName consumePrompt(serialPort) # Get our current location and GPS time if getGPS() == -1: print 'ERROR: missing GPGGA and/or GPZDA data in ' + _gpsFile 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 dprint('lat0=' + str(lat0) + ', lon0=' + str(lon0)) 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 tstr = time.gmtime(timestamp) print 'TEST: ' + str(easting) + ' ' + str(northing) + ' ' + \ str(slantRange) print '_logName: ' + _logName try: print 'log entry #' + str(_nLogEntries) logfile = open(_logName, 'a') logfile.write(str(int(timestamp)) + ',' + _targetName + ',' + str(lat0) + ',' + str(lon0)) _nLogEntries += 1 except KeyboardInterrupt: print('Got keyboard interrupt during log write - keep going') finally: logfile.close() # Sleep before next loop print 'Wait ' + str(LOOP_PAUSE_SEC) + ' sec before next loop' time.sleep(LOOP_PAUSE_SEC) # Close the Benthos serial port serialPort.close() dprint('Serial port closed')