//========================================================================= // Summary : Main file for Reson 6046 // Filename : Reson6046Driver.cc // Author : Reed Christenson // Project : Reson 6046 Driver // Revision : 0.0 // Created : 2002.02.26 // Modified : 2002.02.26 // $Id: Reson6046Driver.cc,v 1.73.2.3 2016/11/11 00:40:00 hthomas Exp $ //========================================================================= // Description : Reson 6046 Driver //========================================================================= #include "DeviceDriver.h" #include "Reson6046Driver.h" #include "Reson6046Commands.h" #include #define POSITION_MSG 1003 #define ATTITUDE_MSG 1004 // #define ALTITUDE_MSG 1006 // #define MOTION_MSG 1007 // #define DEPTH_MSG 1008 // #define CTD_MSG 1010 Reson6046Driver::Reson6046Driver(void) : PeriodicTask(Reson6046DriverName), _navIF(0), _insIF(0), _fastcatIF(0),_depthSensorIF(0), _recalcInterval(120) { int status = 0; int returnVal = 0; debug = 1; Time::gettime(&_timeStamp); //dwrite(5)("Entering driver constructor..."); Syslog::write("--> Starting Reson 6046 Driver...."); // Create output shared memory //dwrite(5)("Creating output (shared memory)..."); _output = new Reson6046Output(False, SharedData::ReadWrite, False); // Initialize some values. _output->data.deviceReady = True; _output->data.status = DeviceIF::Initializing; _output->data.initErrorCount = 0; _output->data.errorCount = 0; _output->write(); // Read attributes from cfg file and save in shared memory. //dwrite(5)("Loading configuration..."); //loadConfiguration(ConfigurationFile); // Renew local copy of configured shared memory. _output->read(); dwrite(5)("Creating connection to commands..."); _resonPayCmds = NULL; _resonPayCmds = new Reson6046Commands(); // Create command message queue dwrite(5)("Creating command message queue..."); _command = NULL; try { _command = new Reson6046Command( MessageQueue::ReadWrite ); _command->flush(); } catch( Exception e ) { Syslog::write("Caught exception while opening command queue: %s", e.msg ); } catch(...) { Syslog::write("Caught exception while opening message queue"); } // Create log writer //_log = new Reson6046Log(_data); #if 0 // Find the configuration file dwrite(5)("Contacting vehicle configuration server..."); VehicleConfigurationIF::DeviceInfo devInfo; if( VehicleConfigurator::getConfig(Reson6046DriverName, &devInfo ) ) { Syslog::write("Reson6046Driver() - Can not find %s in config files.", Reson6046DriverName); exit(-1); } else { dwrite(2)("Reson6046Driver() - Contacted vehicle configuration server."); } // Set task priority dwrite(5)("Setting task priority to %d...",devInfo.priority); if (System::setTaskPriority(devInfo.priority) == -1) exit (-1); #endif _log = new Reson6046Log(_output, True); // Test remote command //MB_RemoteCommand(Reson6046Command::SetMBRange, 45.); // Test PLC command //Command_SetPingRate(Reson6046IF::SubBottom, 2); // Create an instance of the simple socket driver. // _plcConnSocket = NULL; // _plcConnSocket = new SimpleSocket(); // Create another instance of a simple socket for sending bluefin data frames // _bfDataSocket = NULL; // _bfDataSocket = new SimpleSocket(); // Create another instance of a simple socket for sending bluefin data frames _mbDataSocket = NULL; _mbDataSocket = new SimpleSocket(); // Make socket connection /* while(!_plcConnSocket->ready()) { returnVal = _plcConnSocket->createConnectedStreamClient( 6046, "134.89.32.107"); dwrite(5)("Reson driver - attempting to connect socket...(%d)\n", returnVal); printf("Reson driver - attempting to connect socket...(%d)\n", returnVal); if(!_plcConnSocket->ready()) { dwrite(5)("Reson PLC socket - init error, socket not ready"); printf("Reson PLC socket - init error, socket not ready"); _output->data.deviceReady = False; _output->write(); onInitError(); sleep(1); } else { dwrite(5)("Reson PLC socket - socket ready"); printf("Reson PLC socket - socket ready"); _output->data.deviceReady = True; _output->write(); resetInitError(); } } */ // Make a data socket connection for Multibeam remote control while(!_mbDataSocket->ready()) { returnVal = // port 7000 - according to Reson tester _mbDataSocket->createBroadcastDatagramSend(7000, "134.89.32.110"); dwrite(5)("Reson driver - attempting to connect mb...(%d)\n", returnVal); printf("Reson driver - attempting to connect mb...(%d)\n", returnVal); if(_mbDataSocket->ready()) { dwrite(5)("Multibeam Datagram Socket - socket ready"); printf("Multibeam Datagram Socket - socket ready"); _output->data.deviceReady = True; _output->write(); resetInitError(); } else { dwrite(5)("Multibeam Datagram Socket - init error, socket not ready"); printf("Multibeam Datagram Socket - init error, socket not ready"); _output->data.deviceReady = False; _output->write(); onInitError(); sleep(1); } } /* // Make a data socket connection for bluefin data records while(!_bfDataSocket->ready()) { returnVal = _bfDataSocket->createBroadcastDatagramSend(3100,"134.89.33.255"); dwrite(5)("Reson driver - attempting to connect bf...(%d)\n", returnVal); printf("Reson driver - attempting to connect bf...(%d)\n", returnVal); if(_bfDataSocket->ready()) { dwrite(5)("Bluefin Datagram Socket - socket ready"); printf("Bluefin Datagram Socket - socket ready"); _output->data.deviceReady = True; _output->write(); resetInitError(); } else { dwrite(5)("Bluefin Datagram Socket - init error, socket not ready"); printf("Bluefin Datagram Socket - init error, socket not ready"); _output->data.deviceReady = False; _output->write(); onInitError(); sleep(1); } } */ //set up sonar for external triggering (void)MB_RemoteCommand(Reson6046Command::MBExternalTrigger); #if 0 returnVal = Command_StopLogging(); printf("StopLogging returnVal = %d\n", returnVal); returnVal = Command_StopSubsystem(Reson6046IF::EdgetechAll); printf("StopSubsystem returnVal = %d\n", returnVal); // Setup sonar from configuration file. printf("PulseFilename returnVal = %d\n", returnVal); returnVal = Command_SetPulsePower(Reson6046IF::SubBottom, 27.); printf("PulsePower returnVal = %d\n", returnVal); returnVal = Command_SetPingRate(Reson6046IF::SubBottom, 1.); printf("PingRate returnVal = %d\n", returnVal); printf("calling GetHealth\n"); returnVal = Command_GetHealth(Reson6046IF::SubBottom); printf("GetHealth returnVal = %d\n", returnVal); //sleep(2); returnVal = Command_SetPulsePower(Reson6046IF::SideScanLow, 27.); returnVal = Command_SetPulsePower(Reson6046IF::SideScanHigh, 27); //sleep(2); #endif //create taskIF to CTD and Navigation and Ins try { _fastcatIF = new FastCatIF("fastcat", "FastCat"); Syslog::write("%s: fastcatIF is 0x%x\n", name(), _fastcatIF); } catch (Exception e) { Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg ); _fastcatIF = NULL; } try { _navIF = new NavigationIF("navigation"); Syslog::write("%s: navIF is 0x%x\n", name(), _navIF); } catch (Exception e) { Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg ); _navIF = NULL; } try { _insIF = new InsIF("kearfott"); Syslog::write("%s: insIF is 0x%x\n", name(), _insIF); } catch (Exception e) { Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg ); _insIF = NULL; } try { _depthSensorIF = new DepthSensorIF("depthSensor"); Syslog::write("%s: DepthSensorIF is 0x%x\n",name(), _depthSensorIF); } catch (Exception e) { Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg); _depthSensorIF = NULL; } try { _atsIF = new AuvTimeSyncIF("Ats"); Syslog::write("%s: atsIF is 0x%x\n", name(), _atsIF); } catch (Exception e) { Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg ); _atsIF = NULL; } // try { // _adamIF = new AdamIF("AdamIFServer"); // Syslog::write("%s: adamIF is 0x%x\n", name(), _adamIF); // } // catch (Exception e) { // Syslog::write("%s: Caught exception while opening interfaces: %s", name(), e.msg); // _adamIF = NULL; // } // Set up the periodic callback dwrite(5)("Setting up the periodic callback to %d ms...",_output->data.callbackPeriod); // Run at 1 Hz addPeriodicCallback( 200, (CallbackMethod)Reson6046Driver::periodicCallback ); // Save all new values to shared memory. _output->write(); //dwrite(5)("Set to verbose mode."); //Command_SetVerboseMode(1); dwrite(5)("Leaving driver constructor."); } /// Reson 6046 Driver Destructor. Reson6046Driver::~Reson6046Driver(void) { dwrite(5)("Entering Reson 6046 Driver destructor..."); // Clean up 6046 commands. clean(_resonPayCmds); // Close sockets. // clean(_plcConnSocket); // clean(_bfDataSocket); clean(_mbDataSocket); // Clean up command queue. clean(_command); // Clean up the log. clean(_log); // Inform shared memory of offline status. _output->data.deviceReady = False; //healthy states if(_output->data.status == DeviceIF::Ok || _output->data.status == DeviceIF::Initializing || _output->data.status == DeviceIF::Error) { _output->data.status = DeviceIF::Offline; } _output->write(); delete _output; delete _resonPayCmds; dwrite(5)("Leaving driver destructor."); } // Decode received data into record fragments U32 Reson6046Driver::decodeRecord(unsigned char* data, int length) { Reson6046NetworkFrame nf; Reson6046DataRecordFrame drf; // Perform some obvious sanity checks before proceeding // if (!data || length <= 0) return 0L; if (length < (sizeof(nf) + sizeof(drf))) return 0L; // Now look into the record and find out the record type // memset(&nf, 0, sizeof(nf)); memset(&drf, 0, sizeof(drf)); memcpy(&nf, data, sizeof(nf)); memcpy(&drf, data+sizeof(nf), sizeof(drf)-sizeof(U32)); //drf minus checksum dwrite(5)("decodeRecord: received record of type %ul", drf.recordTypeIdentifier); printf("decodeRecord: received record of type %u\n", drf.recordTypeIdentifier); return drf.recordTypeIdentifier; } int CheckSumCRC32( const char *pcBuffer, const int riBufferLength ) { // Routine supplied by BlueFin w/ only slight modifications made. const char * pcNext = pcBuffer; const char * pcEnd = pcBuffer + riBufferLength; const int c1 = 52845; const int c2 = 22719; int r = 55665; int iChecksum = 0; for ( ; pcNext < pcEnd; ++pcNext ) { unsigned char cipher = (unsigned char)( ( (*pcNext) ^ (r >> 8)) ); r = (cipher + r) * c1 + c2; iChecksum += cipher; } return iChecksum; } void Reson6046Driver::initBFRecords() { Reson6046Commands dummy; memset(&_bfNav, 0x00, sizeof(TBfNavRecord)); _bfNav.header.packetSize = sizeof(TBfNavRecord); _bfNav.header.version = 2; _bfNav.header.offset = sizeof(TBfDataFrame); _bfNav.header.dataType = 1; _bfNav.header.dataSize = _bfNav.header.packetSize - _bfNav.header.offset; _bfNav.header.checksum = 0; _bfNav.quality = 0xFFFF; // set lower 16 bits dummy.set7kTime(&(_bfNav.header.timestamp)); memset(&_bfEnv, 0x00, sizeof(TBfEnvRecord)); _bfEnv.header.packetSize = sizeof(TBfEnvRecord); _bfEnv.header.version = 2; _bfEnv.header.offset = sizeof(TBfDataFrame); _bfEnv.header.dataType = 2; _bfEnv.header.dataSize = _bfEnv.header.packetSize - _bfEnv.header.offset; _bfEnv.header.checksum = 0; _bfEnv.data.quality = 1; // set lowest bit dummy.set7kTime(&(_bfEnv.header.timestamp)); } void Reson6046Driver::displayBFRecords(int type) { if (1 == type) { printf("Nav record:\n"); printf("\tPktSize: %d\n\tversion: %d\n\toffset: %d\n\tdataType: %d\n\tdataSize: %d\n\tcheck: %d\n\tquality: \n", _bfNav.header.packetSize, _bfNav.header.version, _bfNav.header.offset, _bfNav.header.dataType, _bfNav.header.dataSize, _bfNav.header.checksum, _bfNav.quality); } else if (2 == type) { printf("Env record:\n"); printf("\tPktSize: %d\n\tversion: %d\n\toffset: %d\n\tdataType: %d\n\tdataSize: %d\n\tcheck: %d\n\tquality: \n", _bfEnv.header.packetSize, _bfEnv.header.version, _bfEnv.header.offset, _bfEnv.header.dataType, _bfEnv.header.dataSize, _bfEnv.header.checksum, _bfEnv.data.quality); } } void Reson6046Driver::processExternalSensors() { // Initialize the data records initBFRecords(); // Setup the Bluefin-style Navigation record if (_insIF && _navIF && _depthSensorIF) { Message_SetNav(); // int bytesSent = _bfDataSocket->broadcast(&_bfNav, sizeof(TBfNavRecord)); //Syslog::write("sent %d bytes in nav record through bf socket\n", bytesSent); } // Setup the Bluefin-style environmental record (CTD data) if (_fastcatIF && _depthSensorIF) { Message_SetCTD(); // int bytesSent = _bfDataSocket->broadcast(&_bfEnv, sizeof(TBfEnvRecord)); //Syslog::write("sent %d bytes\n", bytesSent); } } /// Periodic Callback void Reson6046Driver::periodicCallback(void) { int returnVal; unsigned char receivedMessage[640]; U32 sizeNetworkFrameAndData; // dwrite(6)("Entering driver periodicCallback...\n"); // Read status from shared memory. _output->read(); // If a socket doesn't exists, make one. /* if(!_plcConnSocket->exists()) { dwrite(6)("socket doesn't exist...\n"); returnVal = _plcConnSocket->createConnectedStreamClient( _output->data.reson6046CommandPort, _output->data.reson6046IpAddressString); if(_plcConnSocket->ready()) { _output->data.deviceReady = True; resetError(); } else { _output->data.deviceReady = False; onError(); } } // If connection is ready, receive data. if(_plcConnSocket->ready()) { // dwrite(6)("Receiving message..."); receivedMessage[0]='\0'; // dwrite(6)("Calling _plcConnSocket->receiving\n"); returnVal = _plcConnSocket->receiving(&receivedMessage[0], sizeof(receivedMessage)); // dwrite(6)("Returned from _plcConnSocket->receiving\n"); if(returnVal < 0) onError(); else { if(returnVal > 0) // Decode the record and determine the type of message { printf("recieved a return message\n"); dwrite(5)("DEBUG: Received:"); for(int i=0;iwrite(); // dwrite(6)("Leaving driver periodicCallback."); } /// Process Command DeviceIF::Status Reson6046Driver::processCommand(void) { // dwrite(6)("Entering driver processCommand..."); DeviceIF::Status returnStatus = DeviceIF::Ok; Reson6046Command::Command cmdIn; int returnVal; char buffer[32]; // Make sure _command exists. if (!(_command)) { Syslog::write("processCommand() - _command doesn't exist!\n"); return DeviceIF::Error; } else { // Read in shared memory to get latest status. _output->read(); // Process all commands in queue. while( (_command->read( &cmdIn )) > 0 ) { dwrite(5)("processCommand() --- Processing command: %d\n", cmdIn.request); if ((DeviceDriver::processCommand_Skel(cmdIn.request))) { switch( cmdIn.request ) { case Reson6046Command::RecalcParams : //recalcParams(True); break; case Reson6046Command::RecalcInterval : setRecalcInterval(cmdIn.iVal); break; case Reson6046Command::MBStartPing : //returnVal = MB_RemoteCommand(cmdIn.request); break; case Reson6046Command::MBStopPing : returnVal = MB_RemoteCommand(Reson6046Command::SetMBTxPower, 0.0); break; case Reson6046Command::MBExternalTrigger: returnVal = MB_RemoteCommand(cmdIn.request); break; case Reson6046Command::SetMBBottomDetectionFilters : returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal, cmdIn.fVal1, cmdIn.fVal2, cmdIn.fVal3); break; case Reson6046Command::SetMBRange : case Reson6046Command::SetMBPingRate : case Reson6046Command::SetMBTxPower : case Reson6046Command::SetMBRxGain : case Reson6046Command::SetMBPulseWidth : dwrite(2)("Multibeam remote command %d - %f...",cmdIn.iVal, cmdIn.fVal); returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal); break; case Reson6046Command::UseDepthRangeMBBottomDetect : returnVal = MB_RemoteCommand(cmdIn.request, 0., 0., 0., 0., cmdIn.iVal); break; case Reson6046Command::SetMBSnippetControl : returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal, 0., 0., 0., cmdIn.iVal); break; case Reson6046Command::None : // If not sending a command to sonar, keep same status. returnVal = 0; break; case Reson6046Command::DataLoggingOn : dwrite(2)("Received start logging data command."); returnVal = 0; break; case Reson6046Command::DataLoggingOff : dwrite(2)("Received stop logging data command."); returnVal = 0; break; case Reson6046Command::StartSubsystem: // PLC Command ID 0 dwrite(2)("Starting subsystem %d...",cmdIn.iVal); returnVal = Command_StartSubsystem(cmdIn.iVal); break; case Reson6046Command::StopSubsystem: // PLC Command ID 0 dwrite(2)("Starting subsystem %d...",cmdIn.iVal); returnVal = Command_StopSubsystem(cmdIn.iVal); break; case Reson6046Command::SetPulseFilename: // PLC Command ID 0 dwrite(2)("Setting subsytem %d's pulse file to %s...",cmdIn.iVal,cmdIn.sVal); returnVal = Command_SetPulseFilename(cmdIn.iVal, cmdIn.sVal); break; case Reson6046Command::SetPingRange: // PLC Command ID 0 dwrite(2)("Setting subsystem %d's ping range to %6.2f...",cmdIn.iVal,cmdIn.fVal); returnVal = Command_SetPingRange(cmdIn.iVal, cmdIn.fVal); break; case Reson6046Command::SetPingRate: // PLC Command ID 0 dwrite(2)("Setting subsystem %d's ping rate to %6.2f...",cmdIn.iVal,cmdIn.fVal); returnVal = Command_SetPingRate(cmdIn.iVal, cmdIn.fVal); break; case Reson6046Command::SetPingPower: // PLC Command ID 0 dwrite(2)("Setting subsystem %d's ping transmit power to %6.2f...", cmdIn.iVal, cmdIn.fVal); returnVal = Command_SetPulsePower(cmdIn.iVal, cmdIn.fVal); break; case Reson6046Command::StartLogging : // PLC Command ID 1 dwrite(2)("Start sonar logging..."); returnVal = Command_StartLogging(); break; case Reson6046Command::Start7KCenterLogging : dwrite(2)("Start 7K Center logging..."); returnVal = MB_RemoteCommand(cmdIn.request); break; break; case Reson6046Command::Stop7KCenterLogging : dwrite(2)("Stop 7K Center logging..."); returnVal = MB_RemoteCommand(cmdIn.request); break; case Reson6046Command::StopLogging : // PLC Command ID 1 dwrite(2)("Stop sonar logging..."); returnVal = Command_StopLogging(); break; case Reson6046Command::GetVersion: // PLC Command ID 4 dwrite(2)("Getting sonar version..."); returnVal = Command_GetVersion(); break; case Reson6046Command::GetSonarStatus: // PLC Command ID 10 dwrite(2)("Getting sonar status for subsystem %d...", cmdIn.iVal); returnVal = Command_GetSonarStatus(cmdIn.iVal); break; case Reson6046Command::SetVerboseMode: // PLC Command ID 11 dwrite(2)("Setting sonar verbose mode to %d...", cmdIn.iVal); returnVal = Command_SetVerboseMode(cmdIn.iVal); break; case Reson6046Command::GetModules: // PLC Command ID 14 dwrite(2)("Requesting sonar modules..."); returnVal = Command_GetModules(); break; case Reson6046Command::GetRunList: // PLC Command ID 15 dwrite(2)("Requesting sonar run list..."); returnVal = Command_GetRunList(); break; case Reson6046Command::SetTime: // PLC Command ID 20 dwrite(2)("Setting sonar time..."); returnVal = Command_SetTime(); break; case Reson6046Command::SetSBPDuration: dwrite(2)("Setting SBP duration..."); returnVal = Command_SetSBPDuration(cmdIn.fVal); break; case Reson6046Command::SetSBPGain: dwrite(2)("Setting SBP gain..."); returnVal = Command_SetSBPGain(cmdIn.fVal); break; case Reson6046Command::SetMBSoundVelocity: returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal); break; default: Syslog::write("Ignoring unknown command %d.", cmdIn.request); returnVal = -1; } if(returnVal < 0) onError(); else resetError(); returnStatus = _output->data.status; } } } // dwrite(6)("Leaving driver processCommand."); return returnStatus; } /// Create Command DeviceIF::Status Reson6046Driver::createCommand(DriverCommand **cmd) { dwrite(5)("Entering driver createCommand..."); *cmd = new Reson6046Command(MessageQueue::ReadWrite); if (cmd) { dwrite(5)("Leaving driver createCommand."); } else { Syslog::write("createCommand() --- couldn't create commands\n"); return DeviceIF::Error; } return DeviceIF::Ok; } /// Load Configuration void Reson6046Driver::loadConfiguration(char *configFile) { dwrite(5)("Entering driver loadConfiguration..."); Boolean displayConfiguration = False; // Get latest copy of memory to update. _output->read(); Attributes attributes(Reson6046DriverName); #if 0 attributes.add(new DebugAttribute("debug", "Debug Flag", &_output->data.debug, 1)); #endif attributes.add(new IntegerAttribute("initLimit", "Seconds to keep trying to initialize", (long *)&_output->data.initErrorCountLimit, 10)); attributes.add(new IntegerAttribute("timeOutCount", "Allowed number of bad reads and writes in a row", (long *)&_output->data.errorCountLimit, 10)); attributes.add(new IntegerAttribute("callbackPeriod", "CallbackPeriod (milliseconds)", (long *)&_output->data.callbackPeriod, 100)); attributes.add(new BooleanAttribute("displayConfiguration", "Display Configuration", &displayConfiguration, False)); // Reson 6046 Network Parameters attributes.add(new StringAttribute("Reson6046IPAddressString", "Reson IP Address xxx.xxx.xxx.xxx", &_output->data.reson6046IpAddressString, "127.0.0.1")); attributes.add(new IntegerAttribute("Reson6046CommandPort", "Reson 6046 Command Port", (long *)&_output->data.reson6046CommandPort, 6046)); attributes.add(new IntegerAttribute("Reson6046ShutdownPort", "Reson 6046 Shutdown Port", (long *)&_output->data.reson6046ShutdownPort, 8123)); // EdgeTech FS-AU Network Parameters attributes.add(new BooleanAttribute("EdgeTechVisibleOnNetwork", "EdgeTech Visible On Network", &_output->data.edgetechVisibleOnNetwork, False)); attributes.add(new StringAttribute("EdgeTechIpAddressString", "IP Address xxx.xxx.xxx.xxx", &_output->data.edgetechIpAddressString, "127.0.0.1")); attributes.add(new IntegerAttribute("EdgeTechCommandPort", "EdgeTech Command Port", (long *)&_output->data.edgetechCommandPort, 1800)); attributes.add(new IntegerAttribute("EdgeTechDataPort", "EdgeTech Data Port", (long *)&_output->data.edgetechDataPort, 1801)); attributes.add(new IntegerAttribute("EdgeTechShutdownPort", "EdgeTech Shutdown Port", (long *)&_output->data.edgetechShutdownPort, 8123)); // Sub-Bottom Profiler Settings attributes.add(new StringAttribute("sbpPulseFile", "SBP Pulse Filename", &_output->data.sbpPulseFilename, "216Pulse_9k_14k_20ms.spf")); attributes.add(new FloatAttribute("sbpPingRate", "SBP Ping Rate in Hz", &_output->data.sbpPingRate, 4.0)); attributes.add(new FloatAttribute("sbpPulsePower", "SBP Pulse Power Percentage", &_output->data.sbpPulsePower, 0.0)); // Sidescan Low Settings attributes.add(new StringAttribute("sslPulseFile", "SSL Pulse Filename", &_output->data.sslPulseFilename, "ss75_5k_9k_20ms.spf")); attributes.add(new FloatAttribute("sslPingRange", "SSL Ping Range in Meters", &_output->data.sslPingRange, 100.0)); attributes.add(new FloatAttribute("sslPulsePower", "SSL Pulse Power Percentage", &_output->data.sslPulsePower, 0.0)); // Sidescan High Settings attributes.add(new StringAttribute("sshPulseFile", "SSH Pulse Filename", &_output->data.sshPulseFilename, "ss75_5k_9k_20ms.spf")); attributes.add(new FloatAttribute("sshPingRange", "SSH Ping Range in Meters", &_output->data.sshPingRange, 100.0)); attributes.add(new FloatAttribute("sshPulsePower", "SSH Pulse Power Percentage", &_output->data.sshPulsePower, 0.0)); dwrite(5)("loadConfiguration: parse attributes..."); AttributeParser::parse(System::configurationFile(configFile),&attributes); // Save values in shared memory. _output->write(); if(displayConfiguration) { Syslog::write("-------Reson6046 Configuration-------"); Syslog::write("Debug Level = %d.", debug); Syslog::write("Init Error Count Limit = %d.", _output->data.initErrorCountLimit); Syslog::write("Error Count Limit = %d.", _output->data.errorCountLimit); Syslog::write("Callback Period = %d milliseconds.", _output->data.callbackPeriod); Syslog::write("Reson 6046 IP Address = %s.", _output->data.reson6046IpAddressString); Syslog::write("Reson 6046 Command Port = %d.", _output->data.reson6046CommandPort); Syslog::write("Reson 6046 Shutdown Port = %d.", _output->data.reson6046ShutdownPort); Syslog::write("EdgeTech Visible on Network = %s.", _output->data.edgetechVisibleOnNetwork ? "True": "False"); Syslog::write("EdgeTech IP Address = %s.", _output->data.edgetechIpAddressString); Syslog::write("EdgeTech Command Port = %d.", _output->data.edgetechCommandPort); Syslog::write("EdgeTech Data Port = %d.", _output->data.edgetechDataPort); Syslog::write("EdgeTech Shutdown Port = %d.", _output->data.edgetechShutdownPort); Syslog::write("SBP Pulse Filename = %s.", _output->data.sbpPulseFilename); Syslog::write("SBP Ping Rate = %6.2fHz.", _output->data.sbpPingRate); Syslog::write("SBP Pulse Power = %6.2f%%.", _output->data.sbpPulsePower); Syslog::write("SSL Pulse Filename = %s.", _output->data.sslPulseFilename); Syslog::write("SSL Ping Range = %6.2fm.", _output->data.sslPingRange); Syslog::write("SSL Pulse Power = %6.2f%%.", _output->data.sslPulsePower); Syslog::write("SSH Pulse Filename = %s.", _output->data.sshPulseFilename); Syslog::write("SSH Ping Rate = %6.2fm.", _output->data.sshPingRange); Syslog::write("SSH Pulse Power = %6.2f%%.", _output->data.sshPulsePower); Syslog::write("-------------------------------------------"); } dwrite(5)("Leaving driver loadConfiguration."); } // Lowest-level sendCommand() function // int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command) { int debugThreshold = 5; int returnVal = -1; return 0; //if(1 || _plcConnSocket->ready()) if(_plcConnSocket->ready()) { returnVal = _resonPayCmds->setPlcCommandFrame( sensorIndex, commandID, action, command); if(debug >= debugThreshold) _resonPayCmds->displayPlcCommandFrame(Reson6046Commands::ShowFrameAndData); _resonPayCmds->initDataRecordFrame(); returnVal = _resonPayCmds->setDataRecordFrame(recordTypeIdentifier); if(debug >= debugThreshold) _resonPayCmds->displayDataRecordFrame(Reson6046Commands::ShowFrameAndData); _resonPayCmds->setNetworkFrame(); if(debug >= debugThreshold) _resonPayCmds->displayNetworkFrame(Reson6046Commands::ShowFrameAndData); dwrite(5)("Sending message to plc: rec=%d|sensor=%d|cmdId=%d|act=%d|cmd=\"%s\"", recordTypeIdentifier, sensorIndex, commandID, action, command); // returnVal = _plcConnSocket->sending(_resonPayCmds->getPtrNetworkFrameAndData(), // _resonPayCmds->getSizeNetworkFrameAndData()); dwrite(5)("Send return value = %d.",returnVal); } return(0); } int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command, U32 parameter) { int returnVal = -1; dwrite(5)("sendCommand(U32): command = %s, parameter = %d.", command, parameter); sprintf(&_buffer[0], command, parameter); dwrite(5)("sendCommand(U32): _buffer = %s (%02X).", _buffer, _buffer[0]); returnVal = sendCommand(recordTypeIdentifier, sensorIndex, commandID, action, &_buffer[0]); return(returnVal); } int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command, F32 parameter) { int returnVal = -1; dwrite(5)("sendCommand(F32): command = %s, parameter = %d.", command, parameter); sprintf(&_buffer[0], command, parameter); dwrite(5)("sendCommand(F32): _buffer = %s.", _buffer); returnVal = sendCommand(recordTypeIdentifier, sensorIndex, commandID, action, &_buffer[0]); return(returnVal); } int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command, char *parameter) { int returnVal = -1; dwrite(5)("sendCommand(char): command = %s, parameter = %s.", command, parameter); sprintf(&_buffer[0], command, parameter); dwrite(5)("sendCommand(char): _buffer = %s.", _buffer); returnVal = sendCommand(recordTypeIdentifier, sensorIndex, commandID, action, &_buffer[0]); return(returnVal); } int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command, U32 parameter1, U32 parameter2) { int returnVal = -1; dwrite(5)("sendCommand(U32, U32): command = %s, parameters = %u, %u ", command, parameter1, parameter2); sprintf(&_buffer[0], command, parameter1, parameter2); dwrite(5)("sendCommand(U32, U32): _buffer = %s.", _buffer); returnVal = sendCommand(recordTypeIdentifier, sensorIndex, commandID, action, &_buffer[0]); return(returnVal); } int Reson6046Driver::sendCommand(U32 recordTypeIdentifier, U32 sensorIndex, U32 commandID, U32 action, char *command, U32 parameter1, U32 parameter2, U32 parameter3, U32 parameter4, U32 parameter5, F32 parameter6) { int returnVal = -1; dwrite(5)("sendCommand(U32*5,F32): command = %s, parameters = %u, %u, %u, %u, %u, %f", command, parameter1, parameter2, parameter3, parameter4, parameter5, parameter6); sprintf(&_buffer[0], command, parameter1, parameter2, parameter3, parameter4, parameter5, parameter6); dwrite(5)("sendCommand(U32*5,F32): _buffer = %s.", _buffer); returnVal = sendCommand(recordTypeIdentifier, sensorIndex, commandID, action, &_buffer[0]); return(returnVal); } // ############################## // ####### RESON COMMANDS ####### // ############################## // PLC Command ID 0 int Reson6046Driver::Command_StartSubsystem(int subSystem) { return( Command_StartStopSubsystem(subSystem, 1)); } int Reson6046Driver::Command_StopSubsystem(int subSystem) { return( Command_StartStopSubsystem(subSystem, 0)); } int Reson6046Driver::Command_StartStopSubsystem(int subSystem, int action) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; switch(subSystem) { case Reson6046IF::SubBottom: case Reson6046IF::SideScanLow: case Reson6046IF::SideScanHigh: case Reson6046IF::Multibeam: if(action < 0) { Syslog::write("Unknown subsystem state %d, setting subsystem %d to 0 (off)!", action, subSystem); action = 0; } if(action > 1) { Syslog::write("Unknown subsystem state %d, setting subsystem %d to 1 (on)!", action, subSystem); action = 1; } dwrite(5)("Command_StartStopSubsystem(sub = %d, action = %d)", subSystem, action); returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, U32(subSystem), Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Ping = %d", U32(action)); if(returnVal1 < 0) errorCount++; break; case Reson6046IF::SideScanBoth: returnVal2 = Command_StartStopSubsystem(Reson6046IF::SideScanLow, action); if(returnVal2 < 0) errorCount++; returnVal3 = Command_StartStopSubsystem(Reson6046IF::SideScanHigh, action); if(returnVal3 < 0) errorCount++; break; case Reson6046IF::EdgetechAll: returnVal1 = Command_StartStopSubsystem(Reson6046IF::SubBottom, action); if(returnVal1 < 0) errorCount++; returnVal2 = Command_StartStopSubsystem(Reson6046IF::SideScanLow, action); if(returnVal2 < 0) errorCount++; returnVal3 = Command_StartStopSubsystem(Reson6046IF::SideScanHigh, action); if(returnVal3 < 0) errorCount++; break; default: Syslog::write("Unable to stop unknown subsystem %d!",action); errorCount++; } // Sum up all errors. if(errorCount < 1) return( returnVal1 + returnVal2 + returnVal3 ); else return(-1); } int Reson6046Driver::Command_SetPulseFilename(int subSystem, char *pulseFilename) { return( sendCommand(Reson6046Commands::PayloadControllerCommand, U32(subSystem), Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Pulse = %s", pulseFilename) ); } int Reson6046Driver::Command_SetSBPDuration(float milliseconds) { int returnVal1 = 0; Syslog::write("Setting SBP sweep to %f\n", milliseconds); returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046IF::SubBottom, Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Duration = %.2f", milliseconds); return returnVal1; } int Reson6046Driver::Command_SetSBPGain(float gain) { int returnVal1 = 0; Syslog::write("Setting SBP gain to %f\n", gain); returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046IF::SubBottom, Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Gain = %.2f", gain); return returnVal1; } int Reson6046Driver::Command_SetPingRange(int subSystem, float range) { int returnVal1 = 0, returnVal2 = 0, errorCount = 0; printf("Setting subsys %d range to %f\n", subSystem, range); switch(subSystem) { case Reson6046IF::SideScanLow: case Reson6046IF::SideScanHigh: case Reson6046IF::Multibeam: returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, U32(subSystem), Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Range = %.2f", range); if(returnVal1 < 0) errorCount++; break; case Reson6046IF::SideScanBoth: returnVal1 = Command_SetPingRange(Reson6046IF::SideScanLow, range); if(returnVal1 < 0) errorCount++; returnVal2 = Command_SetPingRange(Reson6046IF::SideScanHigh, range); if(returnVal2 < 0) errorCount++; break; default: Syslog::write("Unable to set ping range for invalid subsystem %d!", subSystem); errorCount++; } // Sum up all errors. if(errorCount < 1) return( returnVal1 + returnVal2 ); else return(-1); } int Reson6046Driver::Command_SetPingRate(int subSystem, float rate) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; switch(subSystem) { case Reson6046IF::SubBottom: case Reson6046IF::SideScanLow: case Reson6046IF::SideScanHigh: case Reson6046IF::Multibeam: // Need to and range check on rate. // Need to verify that rates can be set for sidescan sonars. returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, U32(subSystem), Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "Rate = %.2f", rate); if(returnVal1 < 0) errorCount++; break; case Reson6046IF::SideScanBoth: returnVal2 = Command_SetPingRate(Reson6046IF::SideScanLow, rate); if(returnVal2 < 0) errorCount++; returnVal3 = Command_SetPingRate(Reson6046IF::SideScanHigh, rate); if(returnVal3 < 0) errorCount++; break; case Reson6046IF::EdgetechAll: returnVal1 = Command_SetPingRate(Reson6046IF::SubBottom, rate); if(returnVal1 < 0) errorCount++; returnVal2 = Command_SetPingRate(Reson6046IF::SideScanLow, rate); if(returnVal2 < 0) errorCount++; returnVal3 = Command_SetPingRate(Reson6046IF::SideScanHigh, rate); if(returnVal3 < 0) errorCount++; default: Syslog::write("Unable to set ping rate for invalid subsystem %d!", subSystem); errorCount++; } // Sum up all errors. if(errorCount < 1) return( returnVal1 + returnVal2 + returnVal3 ); else return(-1); } int Reson6046Driver::Command_SetPulsePower(int subSystem, float percentage) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; switch(subSystem) { case Reson6046IF::SubBottom: case Reson6046IF::SideScanLow: case Reson6046IF::SideScanHigh: case Reson6046IF::Multibeam: if(percentage < 0.0) { Syslog::write("Invalid power level, setting to 0.0 percent!"); percentage = 0.0; } if(percentage > 100.0) { Syslog::write("Invalid power level, setting to 100.0 percent!"); percentage = 100.0; } returnVal1 = sendCommand(Reson6046Commands::PayloadControllerCommand, U32(subSystem), Reson6046Commands::PLC_Command_Generic, Reson6046Commands::PLC_Action_Set, "TxPower = %.2f", percentage); if(returnVal1 < 0) errorCount++; break; case Reson6046IF::SideScanBoth: returnVal2 = Command_SetPulsePower(Reson6046IF::SideScanLow, percentage); if(returnVal2 < 0) errorCount++; returnVal3 = Command_SetPulsePower(Reson6046IF::SideScanHigh, percentage); if(returnVal3 < 0) errorCount++; break; case Reson6046IF::EdgetechAll: returnVal1 = Command_SetPulsePower(Reson6046IF::SubBottom, percentage); if(returnVal1 < 0) errorCount++; returnVal2 = Command_SetPulsePower(Reson6046IF::SideScanLow, percentage); if(returnVal2 < 0) errorCount++; returnVal3 = Command_SetPulsePower(Reson6046IF::SideScanHigh, percentage); if(returnVal3 < 0) errorCount++; break; default: Syslog::write("Unable to stop unknown subsystem %d!",subSystem); errorCount++; } // Sum up all errors. if(errorCount < 1) return( returnVal1 + returnVal2 + returnVal3 ); else return(-1); } // PLC Command ID 1 int Reson6046Driver::Command_StartLogging() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Logging, Reson6046Commands::PLC_Action_Set, "%d", (U32)1) ); } int Reson6046Driver::Command_StopLogging() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Logging, Reson6046Commands::PLC_Action_Set, "%d", (U32)0) ); } // PLC COMMAND ID 4 int Reson6046Driver::Command_GetVersion() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Version, Reson6046Commands::PLC_Action_Get, "") ); } // PLC Command ID 9 int Reson6046Driver::Command_SetAlarm(int alarmID, int action) { if(action < 0) { Syslog::write("Invalid action %d, setting to 0 (disabled)!", action); action = 0; } if(action > 1) { Syslog::write("Invalid action %d, setting to 1 (enabled)!", action); action = 1; } return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Status, Reson6046Commands::PLC_Action_Get, "%d, %d", U32(alarmID), U32(action)) ); } // PLC Command ID 10 int Reson6046Driver::Command_GetSonarStatus(int subSystem) { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Status, Reson6046Commands::PLC_Action_Get, "%d", U32(subSystem)) ); } // PLC Command ID 11 int Reson6046Driver::Command_SetVerboseMode(int action) { if(action < 0) { Syslog::write("Invalid action %d, setting to 0 (disabled)!", action); action = 0; } if(action > 1) { Syslog::write("Invalid action %d, setting to 1 (enabled)!", action); action = 1; } return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Verbose, Reson6046Commands::PLC_Action_Set, "%d", U32(action)) ); } // PLC Command ID 14 int Reson6046Driver::Command_GetModules() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Modules, Reson6046Commands::PLC_Action_Get, "") ); } // PLC Command ID 15 int Reson6046Driver::Command_GetRunList() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_RunList, Reson6046Commands::PLC_Action_Get, "") ); } // PLC Command ID 18 int Reson6046Driver::Command_Reserved1() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Reserved1, Reson6046Commands::PLC_Action_Get, "") ); } // PLC Command ID 19 int Reson6046Driver::Command_Reserved2() { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Reserved2, Reson6046Commands::PLC_Action_Get, "") ); } // PLC Command ID 20 int Reson6046Driver::Command_SetTime() { return 0; #if 0 do { Time::gettime(&_timeStamp); dwrite(5)("sec = %12d nano = %12d.",_timeStamp.seconds,_timeStamp.nanoSeconds); }while(_timeStamp.nanoSeconds < 750000000); Time::splitTime(&_fullTime); return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_TimeSync, Reson6046Commands::PLC_Action_Set, "%04u/%02u/%02u,%02u:%02u:%8.5f", _fullTime.year, _fullTime.mon+1, _fullTime.mday, _fullTime.hour, _fullTime.min, (F32)_fullTime.sec) ); #endif } // PLC Command ID 21 int Reson6046Driver::Command_GetHealth(int subSystem) { return( sendCommand(Reson6046Commands::PayloadControllerCommand, Reson6046Commands::SensorIndexPLC, Reson6046Commands::PLC_Command_Health, Reson6046Commands::PLC_Action_Get, "%d", U32(subSystem)) ); } // PLC Record Type 1010 int Reson6046Driver::Message_SetCTD() { float cond, temp, depth, sal, soundvel; TimeIF::TimeSpec condSampleTime, tempSampleTime; TBfEnvData *data; struct _CtdData *ctd; ctd = &(_bfEnv.data.ctd); DeviceIF::Status stat = _fastcatIF->get_CTD_data(&(ctd->cond), &(ctd->temp), &(ctd->pressure), &(ctd->condSampleTime), &(ctd->tempSampleTime), &(ctd->salinity), &(_bfEnv.data.soundVelocity), (long *)&(ctd->tcounts), &(ctd->cfreq), (long *)&(ctd->pcounts), &(ctd->presCompVoltage)); // check return status; set velocity invalid if (stat == DeviceIF::Ok) { _bfEnv.data.quality = 1; // sound velocity valid } else { _bfEnv.data.quality = 0; } //get surface pressure and place in data structure stat = _depthSensorIF->surfacePressure(&ctd->surfacePressure); if (stat == DeviceIF::Ok) { _bfEnv.data.quality = 1; } else { _bfEnv.data.quality = 0; } // stat = _adamIF->get(&ctd->analogSampleTime, // &ctd->analogChannels[0], // &ctd->analogChannels[1], // &ctd->analogChannels[2], // &ctd->analogChannels[3], // &ctd->analogChannels[4], // &ctd->analogChannels[5], // &ctd->analogChannels[6], // &ctd->analogChannels[7]); // _bfEnv.header.checksum = CheckSumCRC32((char *)&(_bfEnv.data), sizeof(TBfEnvData)); return 0; } // Put Nav and INS data into Bluefin Nav record // int Reson6046Driver::Message_SetNav() { NavigationIF::Attitude attitude; NavigationIF::Position position; DeviceIF::Status stat; double lat,lng,depth,alt,pressure; InsIF::InertialState insState; TimeIF::TimeSpec insSampleTime; TimeIF::TimeSpec depthSampleTime; // Get inertial state and position _insIF->getInertialState(&insState, &insSampleTime); _navIF->state(&position, &attitude); _navIF->location(&lat, &lng, &depth, &alt); _depthSensorIF->depth(&depth, &depthSampleTime); _depthSensorIF->pressure(&pressure, &depthSampleTime); // Place data into BF nav record _bfNav.lat = insState.lat; _bfNav.lng = insState.lon; _bfNav.speed = sqrt(insState.vbodyx*insState.vbodyx + insState.vbodyy*insState.vbodyy + insState.vbodyz*insState.vbodyz); // Use ins data now // _bfNav.speed = sqrt(position.xRate*position.xRate + // position.yRate*position.yRate + // position.zRate*position.zRate); _bfNav.depth = depth; _bfNav.altitude = position.altitude; _bfNav.roll = insState.roll; _bfNav.pitch = insState.pitch; _bfNav.yaw = insState.yaw; _bfNav.northingRate = insState.vbodyx; _bfNav.eastingRate = insState.vbodyy; _bfNav.depthRate = insState.vbodyz; _bfNav.altitudeRate = pressure; //added pressure into this slot HJT 08072007 _bfNav.rollRate = insState.rollRate; _bfNav.pitchRate = insState.pitchRate; _bfNav.yawRate = insState.yawRate; _bfNav.positionTime = (double)insSampleTime.seconds + (double)insSampleTime.nanoSeconds/1.e9; _bfNav.altitudeTime = (double)depthSampleTime.seconds + (double)depthSampleTime.nanoSeconds/1.e9; // Adjust time based on drift between system clocks. // Place the difference in the reserved spot in the nav record header. // The difference is calculated by Ats as (resonTime - auvTime). To adjust // the auv timestamp to more closely match the reson time, add the // difference to the auv timestamp. Units are milliseconds. // float atsDelta; _atsIF->getDelta(&atsDelta); _bfNav.header.reserved = (unsigned)(atsDelta * 1000.); _bfNav.quality = insState.status; _bfNav.header.checksum = CheckSumCRC32((char *)&(_bfNav.quality), //sizeof(TBfNavRecord)-sizeof(TBfDataFrame)); sizeof(TBfEnvData)); return 0; } // ####### Error Handling ####### void Reson6046Driver::resetInitError() { _output->data.initErrorCount = 0; _output->data.status = DeviceIF::Ok; _output->write(); } void Reson6046Driver::onInitError() { //just leave status Initializing ++_output->data.initErrorCount; _output->write(); if(++_output->data.initErrorCount >= _output->data.initErrorCountLimit) { onInitFailure(); } } void Reson6046Driver::onInitFailure() { _output->data.status = DeviceIF::FailedInit; _output->write(); exit(1); } void Reson6046Driver::resetError() { _output->data.errorCount = 0; _output->data.status = DeviceIF::Ok; _output->write(); } void Reson6046Driver::onError() { _output->data.status = DeviceIF::Error; _output->data.errorCount++; _output->write(); if( _output->data.errorCount >= _output->data.errorCountLimit) { onFailure(); } } void Reson6046Driver::onFailure() { _output->data.status = DeviceIF::FailedRead; _output->write(); exit(1); } // Build and send a 7K Remote Command to the 7K multibeam // Parameters have default value of 0.0 // int Reson6046Driver::MB_RemoteCommand(int cmdIn, float cmdParam, float cmdParam1, float cmdParam2, float cmdParam3, int iParam) { F32 param = cmdParam; F32 param1 = cmdParam1; F32 param2 = cmdParam2; F32 param3 = cmdParam3; U32 iparam = iParam; // Determine which command and build the Remote Control message section char recordData[512]; char *ptrToData = recordData; char *ptrs = ptrToData; int data_size = 0; int rc_id, rth; switch (cmdIn) { case Reson6046Command::MBStopPing : // No params rc_id = 1101; rth = 7500; break; case Reson6046Command::MBStartPing : rc_id = 1100; rth = 7500; break; case Reson6046Command::MBExternalTrigger : rc_id = 1114; rth = 7500; break; case Reson6046Command::SetMBRange : // Single params rc_id = 1003; rth = 7500; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBPingRate : rc_id = 1004; rth = 7500; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBTxPower : rc_id = 1005; rth = 7500; data_size += sizeof(F32); Syslog::write("setting multibeam transmit power\n"); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBRxGain : rc_id = 1008; rth = 7500; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBPulseWidth : rc_id = 1006; rth = 7500; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBSoundVelocity : if (param > -0.01 && param < 0.01) // param is effectively zero { // Use fastcat to set the sound velocity // if (_fastcatIF) { float cond, temp, press, sal, sos,cfreq, presCompVolts; long tcnts, pcnts; TimeIF::TimeSpec condTime, tempTime; DeviceIF::Status stat = _fastcatIF->get_CTD_data(&cond, &temp, &press, &condTime, &tempTime, &sal, &sos, &tcnts, &cfreq, &pcnts, &presCompVolts); Syslog::write("setting speed of sound to %f\n", sos); if (stat == DeviceIF::Ok) param = sos; // set param to sos, then continue else { Syslog::write("Reson6046Driver: Can't set sound velocity - ", "bad status from fastcat IF"); return 0; } } else { Syslog::write("Reson6046Driver: Can't set sound velocity - no fastcat"); return 0; } } // Use whatever value is in param // rc_id = -1; //no rc_id for this command rth = 7610; data_size += sizeof(F32); if (param < 100.) param = 1484.0; memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBAbsorptionLoss : rc_id = -1; rth = 7611; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBSpreadingLoss : rc_id = -1; rth = 7612; data_size += sizeof(F32); memcpy(ptrToData, ¶m, sizeof(F32)); break; case Reson6046Command::SetMBBottomDetectionFilters : // Multiple params rc_id = 1010; rth = 7500; data_size += 4*sizeof(F32); memcpy((void*)ptrs, ¶m, sizeof(F32)); // minRange ptrs += sizeof(F32); memcpy((void*)ptrs, ¶m1, sizeof(F32)); // maxRange ptrs += sizeof(F32); memcpy((void*)ptrs, ¶m2, sizeof(F32)); // minDepth ptrs += sizeof(F32); memcpy((void*)ptrs, ¶m3, sizeof(F32)); // maxDepth ptrs += sizeof(F32); break; case Reson6046Command::UseDepthRangeMBBottomDetect : rc_id = 1009; rth = 7500; data_size += sizeof(U32); iparam = 0x0; if (iParam == 1) iparam = 0x200; if (iParam == 2) iparam = 0x100; if (iParam == 3) iparam = 0x200 | 0x100; memcpy(ptrToData, &iparam, sizeof(U32)); break; case Reson6046Command::SetMBSnippetControl : rc_id = 1103; rth = 7500; data_size += 2*sizeof(U32); memcpy(ptrToData, &iparam, sizeof(U32)); iparam = (U32)param; // Window size memcpy(ptrToData+sizeof(U32), &iparam, sizeof(U32)); break; case Reson6046Command::Start7KCenterLogging : char fileName[256]; time_t rawtime; struct tm *timeinfo; int length; rc_id = 1200; rth = 7500; data_size += (sizeof(U32) + sizeof(U8)*256); memset(ptrToData, 0x00, data_size); iparam = 0; //always append to existing file, never overwrite memcpy(ptrs, &iparam, sizeof(U32)); ptrs += sizeof(U32); time(&rawtime); timeinfo = gmtime(&rawtime); length = strftime(fileName, 256, "%Y%m%d_%H%M%S\0",timeinfo); // memcpy(ptrs,fileName,length); break; case Reson6046Command::Stop7KCenterLogging : rc_id = 1201; rth = 7500; data_size += 0; //no parameters break; default: return 0; break; } // Build message network frame // unsigned drf_size; if (rth == 7500) { drf_size = sizeof(Reson6046DataRecordFrame) + sizeof(MBRemoteControl) + data_size; } else { drf_size = sizeof(Reson6046DataRecordFrame) + data_size; } unsigned totalSize = sizeof(struct Reson6046NetworkFrame) + drf_size; char *buf = new char[totalSize]; struct Reson6046NetworkFrame *nf = (struct Reson6046NetworkFrame*)buf; nf->version = MB7K_PROTOCOL_VERSION; nf->offset = sizeof(struct Reson6046NetworkFrame); nf->totalPackets = 1; nf->totalRecords = 1; nf->transmissionIdentifier = 0; nf->packetSize = totalSize; nf->totalSize = drf_size; nf->sequenceNumber = 0; nf->destDeviceIdentifier = 0; nf->destEnumerator = 0; nf->srcEnumerator = 0; nf->srcDeviceIdentifier = 0; // Build Data Record Frame // struct Reson6046DataRecordFrame *drf = (struct Reson6046DataRecordFrame*) (buf + sizeof(struct Reson6046NetworkFrame)); drf->offset = sizeof(Reson6046DataRecordFrame) - 2*sizeof(U16) - sizeof(U32); drf->syncPattern = 0x0000FFFF; drf->size = drf_size; drf->optionalDataOffset = 0; drf->optionalDataIdentifier = 0; _resonPayCmds->set7kTime(&(drf->time7k)); drf->recordVersion = MB7K_RECORD_VERSION; drf->recordTypeIdentifier = rth; drf->deviceIdentifier = 7125; drf->reserved1 = 0; drf->systemEnumerator = 0; drf->sequentialRecordCounter = 0; drf->flags = 0; drf->reserved2 = 0; drf->reserved3 = rc_id; drf->totalRecords = 0; drf->fragmentNumber = 0; drf->version = MB7K_PROTOCOL_VERSION; // Place remote control info in meesage buffer // if (rth == 7500) { MBRemoteControl *pRC = (MBRemoteControl*) (buf + sizeof(struct Reson6046NetworkFrame) + sizeof(struct Reson6046DataRecordFrame) - sizeof(U32)); pRC->rc_id = rc_id; pRC->ticket = 0; memcpy((char*)(pRC) + sizeof(MBRemoteControl), recordData, data_size); // For debug purposes use #if 1, otherwise use #if 0 #if 0 printf("Dumping MB record of size %u, drf size of %u...\n", totalSize, drf_size); for (int ii = 0; ii < totalSize;) { printf("%02X:", buf[ii++]); if ((ii % 8) == 0) printf("\n"); } printf("\n"); #endif } else { memcpy((char*)(buf + sizeof(struct Reson6046NetworkFrame) + sizeof(struct Reson6046DataRecordFrame)-sizeof(U32)), recordData, data_size); Syslog::write("sending speed of sound of %f\n", *((float *)recordData)); } int bytesSent = _mbDataSocket->broadcast(buf, totalSize); delete buf; return 0; } void Reson6046Driver::recalcParams(Boolean commanded) { long now; if (!commanded) { // Interval of zero means to never automatically recalculate parameters // if (_recalcInterval <= 0) return; time(&now); if ((now - _lastRecalcTime) < _recalcInterval ) return; } // Recalc params based on current altitude // double altitude = 0., lat, lng, depth; if (_navIF) // Get the current altitude from Nav { _navIF->location(&lat, &lng, &depth, &altitude); } // Calculate min/max params F32 minR, maxR, minD, maxD; minR = altitude * 0.4; maxR = altitude * 4.0; minD = 0.; maxD = altitude * 3.0; // Send params to Reson PLC Syslog::write("Reson6046Driver - recalculating altitude parameters"); MB_RemoteCommand(Reson6046Command::SetMBBottomDetectionFilters, minR, maxR, minD, maxD, 0); // Remember the time for determining the next update _lastRecalcTime = now; }