#!/usr/bin/python """pni-tcm-sink.py """ import os import struct import binascii import serial import crcmod import time import datetime import netCDF4 import numpy as np # As a reminder, struct supports endianness: < little, > big, ! network. structB = struct.Struct('>B') structH = struct.Struct('>H') structHBH = struct.Struct('>HBH') crcx = crcmod.predefined.mkPredefinedCrcFun('xmodem') # 1 kGetModInfo = structB.pack(1) # 2 kModInfoResp = struct.Struct('>HBIIH') # 3 # use frame ID 3 to define which data values are returned after a query # 12 values # 5 heading Float32 degrees # 24 pitch Float32 degrees # 25 roll Float32 degrees # 27 mag_x Float32 microTesla # 28 mag_y Float32 microTesla # 29 mag_z Float32 microTesla # 21 acc_x Float32 G # 22 acc_y Float32 G # 23 acc_z Float32 G # 7 temperature Float32 degrees Celsius # 8 distortion Bool # 9 calStatus Bool kSetDataComponents = struct.pack('>14B',3,12,5,24,25,27,28,29,21,22,23,7,8,9) # 4 kGetData = structB.pack(4) # 5 # binary struct signature of the full data response kDataResp = struct.Struct('>HBBBfBfBfBfBfBfBfBfBfBfBBBBH') #kSetConfig = struct.pack('>...', 6,...) kGetConfig = structB.pack(7) # 8 #kGetConfigResp = struct.Struct('>', ...) # 9 kSave = structB.pack(9) # 16 kSaveDone = struct.Struct('>HBHH') # 21 kStartContinuousMode = structB.pack(21) # 22 kStopContinuousMode = structB.pack(22) # 24 # use frame ID 24 to define the parameters for continuous acquisition # True AcquisitionMode bool acquire in continuous mode # False FlushFilter bool do not flush the FIR filter taps (we may want to change this parameter when installed on the LRAUV) # 0.0 AcquireDelay seconds acquire new data immediately after the previous set # 0.05 SampleDelay seconds output at roughly 20 Hz # kSetAcqParams = struct.Struct('>3B2f',24,True,False,0.0,0.05) kSetAcqParams = struct.Struct('>B??ff') # 25 kGetAcqParams = structB.pack(25) # 26 kSetAcqParamsDone = structB.pack(26) # 27 #kGetAcqParamsResp = kSetAcqParams # "This frame is the response to kGetAcqParams frame. The payload has the same structure as kSetAcqParams." kGetAcqParamsResp = struct.Struct('>HB??ffH') # includes space for ByteCount prefix and CRC16 suffix # 46 kSetSyncMode = structB.pack(46) # 47 # kSetSyncModeResp = structB # 49 kSyncRead = structB.pack(49) def unix_epoch_seconds(t): """Simple function to convert datetime to epoch seconds. Providing this to handle cases where the python and datetime versions are old, and as a workaround to get sub-second time resolution in the netCDF4 file. """ delta = t - datetime.datetime.utcfromtimestamp(0) try: return delta.total_seconds() except AttributeError: # probably an older version of python and datetime return delta.days * 86400 + delta.seconds + delta.microseconds * 1e-6 def mkpkt(frame): """ Make a binary datagram packet for the TCM-XB. """ bc = structH.pack(len(frame) + 4) crc = structH.pack(crcx(bc + frame)) return bc + frame + crc def setup_netCDFv4(filename): """Set up groups, metadata, and dimensions for a netCDFv4 log. """ rg = netCDF4.Dataset(filename, 'w', clobber=False, format='NETCDF4') # TODO write nc4 metadata time = rg.createDimension('time', None) # set up time as an unlimited dimension ts = rg.createVariable('time', np.float64, 'time') ts.units = 'seconds since 1970-00-00T00:00:00.0Z' ts.calendar = 'standard' # e.g., ts[0] = netCDF4.date2num(t[0], units=ts.units, calendar=ts.calendar) epoch = rg.createVariable('epoch', np.float64, 'time') epoch.units = 'seconds since 1970-00-00T00:00:00.0Z' byte_count = rg.createVariable('byte_count', np.uint16, 'time') byte_count.units = 'bytes' frame_id = rg.createVariable('frame_id', np.uint8, 'time') frame_id.units = 'count' id_count = rg.createVariable('data_count', np.uint8, 'time') id_count.units = 'count' heading = rg.createVariable('heading', np.float32, 'time') heading.units = 'angular_degree' pitch = rg.createVariable('pitch', np.float32, 'time') pitch.units = 'angular_degree' roll = rg.createVariable('roll', np.float32, 'time') roll.units = 'angular_degree' v3l = [('x', np.float32),('y', np.float32),('z', np.float32)] vector3_t = rg.createCompoundType(np.dtype(v3l),'vector') magnetic_field = rg.createVariable('magnetic_field', vector3_t, 'time') magnetic_field.units = 'microtesla' acceleration = rg.createVariable('acceleration', vector3_t, 'time') acceleration.units = 'g' temperature = rg.createVariable('temperature', np.float32, 'time') temperature.units = 'degrees Celsius' distortion = rg.createVariable('distortion', np.uint8, 'time') calStatus = rg.createVariable('calibration_status', np.uint8, 'time') checksum = rg.createVariable('checksum', np.uint16, 'time') good_checksum = rg.createVariable('good_checksum', np.uint8, 'time') calculated_checksum = rg.createVariable('calculated_checksum', np.uint16, 'time') return rg; def run_poll(sio, period = 10, bin_file = None, nc_file = None, dt = 0.5): """ Log TCM compass data. """ # TODO: handle float args for period, dt count = 0 sio.write(mkpkt(kGetData)) # send the first poll to the compass sio.flush() # do it now, don't wait with the buffer tstart = datetime.datetime.utcnow() while datetime.datetime.utcnow() < tstart + datetime.timedelta( seconds=period ): time.sleep(0.1) bytes_waiting = sio.inWaiting() if bytes_waiting >= kDataResp.size: log(sio, bin_file, nc_file) time.sleep(dt) sio.write(mkpkt(kGetData)) # send the next poll to the compass sio.flush() # do it now, don't wait with the buffer count = 0 elif count > 1: sio.write(mkpkt(kGetData)) # send the next poll to the compass sio.flush() # do it now, don't wait with the buffer count = 0 else: print 'have', bytes_waiting, 'bytes, but waiting for', kDataResp.size # TODO: send this to a logger instead count += 1 time.sleep(dt/2) def run_continuous(sio, period = 10, bin_file = None, nc_file = None): """ Log TCM compass data. """ # TODO: handle float args for period sio.write(mkpkt(kStartContinuousMode)) sio.flush() # do it now, don't wait with the buffer print 'wrote kStartContinuousMode: ', binascii.hexlify(mkpkt(kStartContinuousMode)) tstart = datetime.datetime.utcnow() while datetime.datetime.utcnow() < tstart + datetime.timedelta( seconds=period ): bytes_waiting = sio.inWaiting() if bytes_waiting >= kDataResp.size: log(sio, bin_file, nc_file) else: print 'have', bytes_waiting, 'bytes, but waiting for', kDataResp.size # TODO: send this to a logger instead time.sleep(0.5) sio.write(mkpkt(kStopContinuousMode)) def log(sio, bf, nf, verbosity=1): datagram = sio.read(kDataResp.size) now = datetime.datetime.utcnow() # TODO: ASCII logger bf.write(datagram) data = kDataResp.unpack(datagram) k = nf.variables['time'].shape[0] # get the index of the next timestep nf.variables['time'][k] = netCDF4.date2num(now, units=nf.variables['time'].units, calendar=nf.variables['time'].calendar) nf.variables['epoch'][k] = unix_epoch_seconds(now) # for reference: kDataResp = struct.Struct(' 0: print now, 'checksum match', (crcx(datagram[:-2]) == data[27]), 'heading', data[4], 'pitch', data[6], 'roll', data[8] if verbosity > 1: msg = 'magnetic field = ({0:0.2f}, {1:0.2f}, {2:0.2f}) {3}; acceleration = ({4:0.2f}, {5:0.2f}, {6:0.2f}) {7}' print msg.format(mag[k]['x'], mag[k]['y'], mag[k]['z'], mag.units, acc[k]['x'], acc[k]['y'], acc[k]['z'], acc.units) def convert_bin_to_nc4(bin_filename=None, nc_filename=None, verbosity=0): raise NotImplementedError def main(port = '/dev/ttyUSB0', baudrate = 9600, timeout = 1.0, period = 6, output_path = '/tmp', bin_filename = None, nc_filename = None, log_filename = None, verbosity = 0, mode='poll', dt = 0.5): tstart = datetime.datetime.utcnow() if bin_filename is None: bin_filename = '.'.join((tstart.strftime('%Y%m%dT%H%M%S'), 'pni-tcm', mode, 'bin')) if nc_filename is None: nc_filename = '.'.join((tstart.strftime('%Y%m%dT%H%M%S'), 'pni-tcm', mode, 'nc4')) bf = open(os.path.join(output_path, bin_filename), 'wb') nf = setup_netCDFv4(os.path.join(output_path, nc_filename)) siokw = dict(bytesize=8, parity='N', stopbits=1, xonxoff=0, rtscts=0, writeTimeout=None, dsrdtr=None) sio = serial.Serial(port=port, baudrate=baudrate, timeout=timeout, **siokw) sio.write(mkpkt(kGetModInfo)) print 'kGetModInfo' time.sleep(1e-3) resp = sio.read(kModInfoResp.size) print 'kModInfo response', len(resp), binascii.hexlify(resp), kModInfoResp.unpack(resp) print 'kSetDataComponents' sio.write(mkpkt(kSetDataComponents)) # kSetDataComponents has no response # TODO: use kSetConfig & kGetConfig if mode == 'continuous': sio.write(mkpkt(kSetAcqParams.pack(24,False,False,0.0,0.0))) time.sleep(1e-3) resp = sio.read(structHBH.size) print 'kSetAcqParams response:', binascii.hexlify(resp), structHBH.unpack(resp) if resp == mkpkt(kSetAcqParamsDone): sio.write(mkpkt(kGetAcqParams)) resp = sio.read(kGetAcqParamsResp.size) print 'kGetAcqParams response', binascii.hexlify(resp), kGetAcqParamsResp.unpack(resp) # TODO: log the response in the nc4 sio.write(mkpkt(kSave)) resp = sio.read(kSaveDone.size) print 'kSave response', binascii.hexlify(resp), kSaveDone.unpack(resp) run_continuous(sio, period, bf, nf) else: print 'could not set parameters for continuous mode:', binascii.hexlify(resp), '!=', binascii.hexlify(mkpkt(kSetAcqParamsDone)) elif mode == 'poll': sio.write(mkpkt(kSetAcqParams.pack(24,True,False,0.0,0.0))) resp = sio.read(structHBH.size) if resp == mkpkt(kSetAcqParamsDone): sio.write(mkpkt(kGetAcqParams)) resp = sio.read(kGetAcqParamsResp.size) print 'kGetAcqParams response', binascii.hexlify(resp), kGetAcqParamsResp.unpack(resp) # TODO: log the response in the nc4 sio.write(mkpkt(kSave)) resp = sio.read(kSaveDone.size) print 'kSave response', binascii.hexlify(resp), kSaveDone.unpack(resp) run_poll(sio, period, bf, nf, dt) else: print 'could not set parameters for poll mode:', binascii.hexlify(resp), '!=', binascii.hexlify(mkpkt(kSetAcqParamsDone)) elif mode == 'sync': raise NotImplementedError('sync mode is not implemented') else: raise NotImplementedError(mode + ' mode is not implemented') sio.close() nf.close() bf.close() if __name__=="__main__": import argparse program_description = """read compass data from PNI TCM connected via serial and log to nc4""" parser = argparse.ArgumentParser(description=program_description) parser.add_argument('-V', '--version', action='version', version='%(prog)s 0.0.1', help='display version information and exit') parser.add_argument('-p', '--port', default='/dev/ttyUSB0', help='serial port to communicate on') parser.add_argument('-b', '--baudrate', default=38400, type=int, help='baud rate to communicate with') parser.add_argument('-t', '--timeout', default=1, type=float, help='timeout for serial port') parser.add_argument('-P', '--period', default=10, type=float, help='number of seconds to listen') parser.add_argument('-o', '--output-path', default='/tmp', help='path to output directory') parser.add_argument('-B', '--bin_filename', default=None, help='name of output file') parser.add_argument('-n', '--nc_filename', default=None, help='name of output file') parser.add_argument('-l', '--log_filename', default=None, help='name of output file') parser.add_argument('-v','--verbosity', default=0, type=int, help='verbosity to use with the console (does not affect logs)') # TODO: make v flag work like normal instead of requiring an arg args = parser.parse_args() main(args.port, args.baudrate, args.timeout, args.period, args.output_path, args.bin_filename, args.nc_filename, args.log_filename, args.verbosity, mode="poll", dt=0.1)