/******************************************************************************/ /* Summary : Edgetech FSDW Driver Class */ /* Filename : EdgetechFSDWDriver.cc */ /* Author : Eric Martin / emartin@mbari.org */ /* Project : Edgetech FSDW Driver */ /* Version : $Revision: 1.7 $ */ /* Created : 2014.03.11 */ /* Modified : 2014.03.11 */ /* Archived : */ /******************************************************************************/ /* Modification History: Based on Edgetech FSDW Class from Reed C. */ /******************************************************************************/ #include "DeviceDriver.h" #include "EdgetechFSDWDriver.h" //#include "EdgetechFSDWCommands.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 EdgetechFSDWDriver::EdgetechFSDWDriver(void) : Task(EdgetechFSDWDriverName), //_navIF(0), _insIF(0), _fastcatIF(0),_depthSensorIF(0), _recalcInterval(120) { int status = 0; int returnVal = 0; debug = 1; Time::gettime(&_timeStamp); Syslog::write("Starting EdgeTech Handler..\n"); //TODO REMOVE //dwrite(5)("Entering driver constructor..."); Syslog::write("--> Starting Edgetech FSDW Driver...."); // Create output shared memory //dwrite(5)("Creating output (shared memory)..."); _output = new EdgetechFSDWOutput(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..."); // _edgetechPayCmds = NULL; // _edgetechPayCmds = new EdgetechFSDWCommands(); // Create command message queue dwrite(5)("Creating command message queue..."); _command = NULL; try { _command = new EdgetechFSDWCommand( 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"); } #if 0 // Find the configuration file dwrite(5)("Contacting vehicle configuration server..."); VehicleConfigurationIF::DeviceInfo devInfo; if( VehicleConfigurator::getConfig(EdgetechFSDWDriverName, &devInfo ) ) { Syslog::write("EdgetechFSDWDriver() - Can not find %s in config files.", EdgetechFSDWDriverName); exit(-1); } else { dwrite(2)("EdgetechFSDWDriver() - 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 // Create log writer DISABLED Since we do not track data from the client any longer. // _log = new EdgetechFSDWLog(_output, True); //Create EdgeTechHandler // Socket Connection is internal to the class. try { _edgetechCmds = new EdgeTechHandler(_output->data.edgetechIpAddressString, _output->data.edgetechCommandPort , _output->data.edgetechDataPort , stdout); if (_edgetechCmds->isConnected()) { Syslog::write("Initializing Edgetech Configuration...\n"); _edgetechCmds->Initialize(); _edgetechCmds->printStatus(stdout); _output->data.status = DeviceIF::Ok; } else { Syslog::write("Cannot connect to edgetech command port."); } } catch( Exception e ) { Syslog::write("Caught exception while edgetech command queue: %s", e.msg ); } catch(...) { Syslog::write("Caught exception while opening edgetech queue"); } // 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)EdgetechFSDWDriver::periodicCallback ); // Save all new values to shared memory. _output->write(); //dwrite(5)("Set to verbose mode."); //Command_SetVerboseMode(1); dwrite(5)("Leaving driver constructor."); } /// Edgetech FSDW Driver Destructor. EdgetechFSDWDriver::~EdgetechFSDWDriver(void) { dwrite(5)("Entering Edgetech FSDW Driver destructor..."); // Clean up edgetech commands clean(_edgetechCmds); // 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(); dwrite(5)("Leaving driver destructor."); } /// Periodic Callback //void EdgetechFSDWDriver::periodicCallback(void) //{ // int returnVal; //// unsigned char receivedMessage[640]; //// U32 sizeNetworkFrameAndData; // // // // dwrite(6)("Entering driver periodicCallback...\n"); // // Read status from shared memory. // // // Check and process commands // _output->read(); // processCommand(); // _output->write(); // // // Check on Edgetech connections // if (!_edgetechCmds->isConnected()) { // fprintf(stdout, "Attempting to reconnect in 5 seconds...\n"); // sleep(5); // if (_edgetechCmds->Connect()) { // // We reconnected, re-initialize // _edgetechCmds->Initialize(); // } // } else { // Nothing to send and we are connected, poll ports for information. // if (_edgetechCmds->isConnected()) { // _edgetechCmds->ReceiveMessage(); // } // } // // // If a socket doesn't exists, make one. // if(!_plcConnSocket->exists()) // { // dwrite(6)("socket doesn't exist...\n"); // returnVal = _plcConnSocket->createConnectedStreamClient( _output->data.edgetechFSDWCommandPort, // _output->data.edgetechFSDWIpAddressString); // 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;iread(); // 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 EdgetechFSDWCommand::RecalcParams: //recalcParams(True); break; case EdgetechFSDWCommand::RecalcInterval: setRecalcInterval(cmdIn.iVal); break; // case EdgetechFSDWCommand::MBStartPing: // returnVal = MB_RemoteCommand(cmdIn.request); // break; // // case EdgetechFSDWCommand::MBStopPing: // returnVal = MB_RemoteCommand(EdgetechFSDWCommand::SetMBTxPower, 0.0); // break; // // case EdgetechFSDWCommand::SetMBBottomDetectionFilters: // returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal, cmdIn.fVal1, cmdIn.fVal2, cmdIn.fVal3); // break; // // case EdgetechFSDWCommand::SetMBRange: // case EdgetechFSDWCommand::SetMBPingRate: // case EdgetechFSDWCommand::SetMBTxPower: // case EdgetechFSDWCommand::SetMBRxGain: // case EdgetechFSDWCommand::SetMBPulseWidth: // dwrite(2)("Multibeam remote command %d - %f...", cmdIn.iVal, cmdIn.fVal); // returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal); // break; // // case EdgetechFSDWCommand::UseDepthRangeMBBottomDetect: // returnVal = MB_RemoteCommand(cmdIn.request, 0., 0., 0., 0., cmdIn.iVal); // // break; // // case EdgetechFSDWCommand::SetMBSnippetControl: // returnVal = MB_RemoteCommand(cmdIn.request, cmdIn.fVal, 0., 0., 0., cmdIn.iVal); // // break; case EdgetechFSDWCommand::None: // If not sending a command to sonar, keep same status. returnVal = 0; break; case EdgetechFSDWCommand::DataLoggingOn: dwrite(2)("Received start logging data command."); // Not functioning in FSDW lib returnVal = 0; break; case EdgetechFSDWCommand::DataLoggingOff: dwrite(2)("Received stop logging data command."); // Not functioning in FSDW lib returnVal = 0; break; case EdgetechFSDWCommand::StartSubsystem: // PLC Command ID 0 dwrite(2)("Starting subsystem %d...", cmdIn.iVal); returnVal = Command_StartSubsystem(cmdIn.iVal,cmdIn.bVal); break; case EdgetechFSDWCommand::StopSubsystem: // PLC Command ID 0 dwrite(2)("Starting subsystem %d...", cmdIn.iVal); returnVal = Command_StopSubsystem(cmdIn.iVal,cmdIn.bVal); break; case EdgetechFSDWCommand::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,cmdIn.bVal); break; case EdgetechFSDWCommand::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,cmdIn.bVal); break; case EdgetechFSDWCommand::SetPingRate: // PLC Command ID 0 dwrite(2)("Setting subsystem %d's ping rate to %6.2f...", cmdIn.iVal, cmdIn.fVal); //TODO IS THIS FLOAT PPS???? returnVal = Command_SetPingRate(cmdIn.iVal, cmdIn.fVal,cmdIn.bVal); break; case EdgetechFSDWCommand::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,cmdIn.bVal); break; case EdgetechFSDWCommand::StartLogging: // PLC Command ID 1 dwrite(2)("Start sonar logging..."); returnVal = Command_StartLogging(cmdIn.bVal); break; // case EdgetechFSDWCommand::Start7KCenterLogging: // dwrite(2)("Start 7K Center logging..."); // returnVal = MB_RemoteCommand(cmdIn.request); // break; // case EdgetechFSDWCommand::Stop7KCenterLogging: // dwrite(2)("Stop 7K Center logging..."); // returnVal = MB_RemoteCommand(cmdIn.request); // break; case EdgetechFSDWCommand::StopLogging: // PLC Command ID 1 dwrite(2)("Stop sonar logging..."); returnVal = Command_StopLogging(cmdIn.bVal); break; case EdgetechFSDWCommand::GetVersion: // PLC Command ID 4 dwrite(2)("Getting sonar version..."); returnVal = Command_GetVersion(); break; case EdgetechFSDWCommand::GetSonarStatus: // PLC Command ID 10 dwrite(2)("Getting sonar status for subsystem %d...", cmdIn.iVal); returnVal = Command_GetSonarStatus(cmdIn.iVal); break; // case EdgetechFSDWCommand::SetVerboseMode: // PLC Command ID 11 // dwrite(2)("Setting sonar verbose mode to %d...", cmdIn.iVal); // returnVal = Command_SetVerboseMode(cmdIn.iVal); // break; // case EdgetechFSDWCommand::GetModules: // PLC Command ID 14 // dwrite(2)("Requesting sonar modules..."); // returnVal = Command_GetModules(); // break; // case EdgetechFSDWCommand::GetRunList: // PLC Command ID 15 // dwrite(2)("Requesting sonar run list..."); // returnVal = Command_GetRunList(); // break; case EdgetechFSDWCommand::SetTime: // PLC Command ID 20 dwrite(2)("Setting sonar time..."); returnVal = Command_SetTime(cmdIn.bVal); break; case EdgetechFSDWCommand::SetSBPDuration: dwrite(2)("Setting SBP duration..."); returnVal = Command_SetSBPDuration(cmdIn.fVal, cmdIn.bVal); break; case EdgetechFSDWCommand::SetSBPGain: dwrite(2)("Setting SBP gain..."); returnVal = Command_SetSBPGain(cmdIn.fVal, cmdIn.bVal); break; // case EdgetechFSDWCommand::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; cmdInCnt++; } } } // dwrite(6)("Leaving driver processCommand."); return returnStatus; } /// Create Command DeviceIF::Status EdgetechFSDWDriver::createCommand(DriverCommand **cmd) { dwrite(5)("Entering driver createCommand..."); *cmd = new EdgetechFSDWCommand(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 EdgetechFSDWDriver::loadConfiguration(char *configFile) { dwrite(5)("Entering driver loadConfiguration..."); Boolean displayConfiguration = False; // Get latest copy of memory to update. _output->read(); Attributes attributes(EdgetechFSDWDriverName); #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)); // 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, 1700)); attributes.add(new IntegerAttribute("EdgeTechDataPort", "EdgeTech Data Port", (long *)&_output->data.edgetechDataPort, 1701)); // 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("-------EdgetechFSDW 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("Edgetech FSDW IP Address = %s.", _output->data.edgetechFSDWIpAddressString); // Syslog::write("Edgetech FSDW Command Port = %d.", _output->data.edgetechFSDWCommandPort); // Syslog::write("Edgetech FSDW Shutdown Port = %d.", _output->data.edgetechFSDWShutdownPort); 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."); } int EdgetechFSDWDriver::Command_StartSubsystem(int subSystem, Boolean bUpdate) { return( Command_StartStopSubsystem(subSystem, 1, bUpdate)); } int EdgetechFSDWDriver::Command_StopSubsystem(int subSystem, Boolean bUpdate) { return( Command_StartStopSubsystem(subSystem, 0, bUpdate)); } int EdgetechFSDWDriver::Command_StartStopSubsystem(int subSystem, int action, Boolean bUpdate) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; 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; } switch (subSystem) { case EdgetechFSDWIF::SubBottom: dwrite(5)("Command_StartStopSubsystem(sub = %d, action = %d)", subSystem, action); returnVal1 = _edgetechCmds->EnablePing(SUBSYSID_SB, action, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanLow: dwrite(5)("Command_StartStopSubsystem(sub = %d, action = %d)", subSystem, action); returnVal1 = _edgetechCmds->EnablePing(SUBSYSID_SSLF, action, bUpdate); if (returnVal1 < 0) errorCount++; break; break; case EdgetechFSDWIF::SideScanHigh: dwrite(5)("Command_StartStopSubsystem(sub = %d, action = %d)", subSystem, action); returnVal1 = _edgetechCmds->EnablePing(SUBSYSID_SSHF, action, bUpdate); if (returnVal1 < 0) errorCount++; break; break; case EdgetechFSDWIF::SideScanBoth: returnVal2 = Command_StartStopSubsystem(EdgetechFSDWIF::SideScanLow, action, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_StartStopSubsystem(EdgetechFSDWIF::SideScanHigh, action, bUpdate); if (returnVal3 < 0) errorCount++; break; case EdgetechFSDWIF::EdgetechAll: returnVal1 = Command_StartStopSubsystem(EdgetechFSDWIF::SubBottom, action, bUpdate); if (returnVal1 < 0) errorCount++; returnVal2 = Command_StartStopSubsystem(EdgetechFSDWIF::SideScanLow, action, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_StartStopSubsystem(EdgetechFSDWIF::SideScanHigh, action, bUpdate); 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); } /* * MODIFIED FOR FSDW LIB 201305 EJM */ int EdgetechFSDWDriver::Command_SetPulseFilename(int subSystem, char *pulseFilename, Boolean bUpdate) { // return( sendCommand(EdgetechFSDWCommands::PayloadControllerCommand, // U32(subSystem), // EdgetechFSDWCommands::PLC_Command_Generic, // EdgetechFSDWCommands::PLC_Action_Set, // "Pulse = %s", // pulseFilename) int retVal = 0; subsysId_t subsys = SUBSYSID_NONE; switch (subSystem) { case EdgetechFSDWIF::SubBottom: subsys = SUBSYSID_SB; break; case EdgetechFSDWIF::SideScanLow: subsys = SUBSYSID_SSLF; break; case EdgetechFSDWIF::SideScanHigh: subsys = SUBSYSID_SSHF; break; default: Syslog::write("Unable to set pulsefile for unknown subsystem %d!", subSystem); subsys = SUBSYSID_NONE; break; } if (subsys==SUBSYSID_NONE) return (-1); retVal = _edgetechCmds->SetPulseFile(subsys, pulseFilename, strlen(pulseFilename),bUpdate); return (retVal); } int EdgetechFSDWDriver::Command_SetSBPDuration(float milliseconds, Boolean bUpdate) { int returnVal1 = 0; Syslog::write("Setting SBP sweep to %f\n", milliseconds); // returnVal1 = sendCommand(EdgetechFSDWCommands::PayloadControllerCommand, // EdgetechFSDWIF::SubBottom, // EdgetechFSDWCommands::PLC_Command_Generic, // EdgetechFSDWCommands::PLC_Action_Set, // "Duration = %.2f", // milliseconds); returnVal1 = _edgetechCmds->SetPulseDuration(SUBSYSID_SB,milliseconds,bUpdate); return returnVal1; } int EdgetechFSDWDriver::Command_SetSBPGain(float gain, Boolean bUpdate) { int returnVal1 = 0; Syslog::write("Setting SBP gain to %f\n", gain); // returnVal1 = sendCommand(EdgetechFSDWCommands::PayloadControllerCommand, // EdgetechFSDWIF::SubBottom, // EdgetechFSDWCommands::PLC_Command_Generic, // EdgetechFSDWCommands::PLC_Action_Set, // "Gain = %.2f", // gain); returnVal1 = _edgetechCmds->SetRxGain(gain,bUpdate); return returnVal1; } int EdgetechFSDWDriver::Command_SetPingRange(int subSystem, float range, Boolean bUpdate) { int returnVal1 = 0, returnVal2 = 0, errorCount = 0; Syslog::write("ETDRIVER:: Setting subsys %d range to %f\n", subSystem, range); switch (subSystem) { case EdgetechFSDWIF::SideScanLow: returnVal1 = _edgetechCmds->SetRange(SUBSYSID_SSLF, range, bUpdate); break; case EdgetechFSDWIF::SideScanHigh: returnVal1 = _edgetechCmds->SetRange(SUBSYSID_SSHF, range, bUpdate); break; case EdgetechFSDWIF::SideScanBoth: returnVal1 = Command_SetPingRange(EdgetechFSDWIF::SideScanLow, range, bUpdate); if (returnVal1 < 0) errorCount++; returnVal2 = Command_SetPingRange(EdgetechFSDWIF::SideScanHigh, range, bUpdate); if (returnVal2 < 0) errorCount++; break; default: Syslog::write("Unable to set ping range for invalid subsystem %d!", subSystem); errorCount++; break; } // Sum up all errors. if(errorCount < 1) return (returnVal1 + returnVal2); else return(-1); } int EdgetechFSDWDriver::Command_SetPingRate(int subSystem, float rate, Boolean bUpdate) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; switch (subSystem) { case EdgetechFSDWIF::SubBottom: returnVal1 = _edgetechCmds->SetPingRate(SUBSYSID_SB, rate, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanLow: returnVal1 = _edgetechCmds->SetPingRate(SUBSYSID_SSLF, rate, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanHigh: returnVal1 = _edgetechCmds->SetPingRate(SUBSYSID_SSHF, rate, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanBoth: returnVal2 = Command_SetPingRate(EdgetechFSDWIF::SideScanLow, rate, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_SetPingRate(EdgetechFSDWIF::SideScanHigh, rate, bUpdate); if (returnVal3 < 0) errorCount++; break; case EdgetechFSDWIF::EdgetechAll: returnVal1 = Command_SetPingRate(EdgetechFSDWIF::SubBottom, rate, bUpdate); if (returnVal1 < 0) errorCount++; returnVal2 = Command_SetPingRate(EdgetechFSDWIF::SideScanLow, rate, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_SetPingRate(EdgetechFSDWIF::SideScanHigh, rate, bUpdate); if (returnVal3 < 0) errorCount++; break; 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 EdgetechFSDWDriver::Command_SetPulsePower(int subSystem, float percentage, Boolean bUpdate) { int returnVal1 = 0, returnVal2 = 0, returnVal3 = 0, errorCount = 0; switch (subSystem) { case EdgetechFSDWIF::SubBottom: returnVal1 = _edgetechCmds->SetTxPower(SUBSYSID_SB, percentage, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanLow: returnVal1 = _edgetechCmds->SetTxPower(SUBSYSID_SSLF, percentage, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanHigh: returnVal1 = _edgetechCmds->SetTxPower(SUBSYSID_SSHF, percentage, bUpdate); if (returnVal1 < 0) errorCount++; break; case EdgetechFSDWIF::SideScanBoth: returnVal2 = Command_SetPulsePower(EdgetechFSDWIF::SideScanLow, percentage, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_SetPulsePower(EdgetechFSDWIF::SideScanHigh, percentage, bUpdate); if (returnVal3 < 0) errorCount++; break; case EdgetechFSDWIF::EdgetechAll: returnVal1 = Command_SetPulsePower(EdgetechFSDWIF::SubBottom, percentage, bUpdate); if (returnVal1 < 0) errorCount++; returnVal2 = Command_SetPulsePower(EdgetechFSDWIF::SideScanLow, percentage, bUpdate); if (returnVal2 < 0) errorCount++; returnVal3 = Command_SetPulsePower(EdgetechFSDWIF::SideScanHigh, percentage, bUpdate); 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); } int EdgetechFSDWDriver::Command_StartEdgetechStorage(Boolean bUpdate) { return _edgetechCmds->EnableStorage(true, bUpdate); } int EdgetechFSDWDriver::Command_StopEdgetechStorage(Boolean bUpdate) { return _edgetechCmds->EnableStorage(false, bUpdate); } // PLC COMMAND ID 4 int EdgetechFSDWDriver::Command_GetVersion() { return _edgetechCmds->QueryCommonSettings(); } // PLC Command ID 10 int EdgetechFSDWDriver::Command_GetSonarStatus(int subSystem) { return (_edgetechCmds->IsHealthy() ? 1: 0); } int EdgetechFSDWDriver::Command_SetTime(Boolean bUpdate) { return _edgetechCmds->SetTimeFromSystem(bUpdate); } int EdgetechFSDWDriver::Command_RelayCompositeSituation(long flags, unsigned char vDirections, timespec timestamp, double latitudeD, double longitudeD, float depthM, float altitudeM, float heaveM, float velocity1MS, float velocity2MS, float velocityDownMS, float pitchD, float rollD, float headingD, float soundSpeedMS, float waterTempC) { return _edgetechCmds->SendCompositeSituation(flags,vDirections,timestamp, latitudeD,longitudeD,depthM,altitudeM,heaveM,velocity1MS,velocity2MS, velocityDownMS,pitchD,rollD,headingD,soundSpeedMS,waterTempC); } int EdgetechFSDWDriver::Command_RelaySituation(long flags, timespec timestamp, double latitudeD, double longitudeD, double depthM, double headingD, double pitchD, double rollD, double xRelativePosM, double yRelativePosM, double zRelativePosM, double xVelocityMS, double yVelocityMS, double zVelocityMS, double northVelocityMS, double eastVelocityMS, double downVelocityMS, double xAngularRateDS, double yAngularRateDS, double zAngularRateDS, double xAccelerationMS, double yAccelerationMS, double zAccelerationMS, double latitudeStandardDeviationM, double longitudeStandardDeviationM, double depthStandardDeviationM, double headingStandardDeviationD, double pitchStandardDeviationD, double rollStandardDeviationD) { return _edgetechCmds->SendSituation(flags, timestamp, latitudeD, longitudeD, depthM, headingD, pitchD, rollD, xRelativePosM, yRelativePosM, zRelativePosM, xVelocityMS, yVelocityMS, zVelocityMS, northVelocityMS, eastVelocityMS, downVelocityMS, xAngularRateDS, yAngularRateDS, zAngularRateDS, xAccelerationMS, yAccelerationMS, zAccelerationMS, latitudeStandardDeviationM, longitudeStandardDeviationM, depthStandardDeviationM, headingStandardDeviationD, pitchStandardDeviationD, rollStandardDeviationD); } int EdgetechFSDWDriver::Command_RelayPressureRecord(float pressurePSI, float tempC, float salPSU, float condSM, float sosMS, float depthM, long flags) { return _edgetechCmds->SendPressureData(pressurePSI, tempC, salPSU, condSM, sosMS, depthM, flags); } int EdgetechFSDWDriver::Command_RelayAltitude(long flags, float altitudeM, float forwardVelocityMS, float crossTrackVelocityMS) { return _edgetechCmds->SendAltitudeData(flags, altitudeM, forwardVelocityMS, crossTrackVelocityMS); } // ####### Error Handling ####### void EdgetechFSDWDriver::resetInitError() { _output->data.initErrorCount = 0; _output->data.status = DeviceIF::Ok; _output->write(); } void EdgetechFSDWDriver::onInitError() { //just leave status Initializing ++_output->data.initErrorCount; _output->write(); if(++_output->data.initErrorCount >= _output->data.initErrorCountLimit) { onInitFailure(); } } void EdgetechFSDWDriver::onInitFailure() { _output->data.status = DeviceIF::FailedInit; _output->write(); exit(1); } void EdgetechFSDWDriver::resetError() { _output->data.errorCount = 0; _output->data.status = DeviceIF::Ok; _output->write(); } void EdgetechFSDWDriver::onError() { _output->data.status = DeviceIF::Error; _output->data.errorCount++; _output->write(); if( _output->data.errorCount >= _output->data.errorCountLimit) { onFailure(); } } int EdgetechFSDWDriver::Command_StartDataLogging() { return 0; } int EdgetechFSDWDriver::Command_StopDataLogging() { return 0; } void EdgetechFSDWDriver::onFailure() { _output->data.status = DeviceIF::FailedRead; _output->write(); exit(1); }