/****************************************************************************/
/* Summary  :                                                               */
/* Filename : EchoSounderDriver.h                                           */
/* Author   : Rob McEwen, Rich Henthorn                                     */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 11/15/2005                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include "EchoSounderDriver.h"
#include "EchoSounderLog.h"
#include "EchoSounderOutput.h"
#include "Syslog.h"
#include <FloatAttribute.h>
#include <StringAttribute.h>
#include <IntegerAttribute.h>
#include <AngleAttribute.h>
#include <BooleanAttribute.h>
#include <AttributeParser.h>
#include <Exception.h>
#include <i86.h>   //For the delay()

#define RECORD_TERMINATOR "\xa"
//#define RECORD_TERMINATOR 0xFD

Boolean debug_flag = False;

EchoSounderDriver::EchoSounderDriver(SerialDeviceExt *device, char* config)
   : Task("echoSounderDriver"), _config(config), _attributes(config),
     _device(device)
{
   Syslog::write("EchoSounderDriver:: Constructor beginning.");
  //
  _running = True;
  _ansTime = _askTime = _lastAskTime = 0;
  _crashCtr = 0;
  _error = False;
  _maxStepSize = 8;
  _nsleepTime.tv_sec  = 0;
  _nsleepTime.tv_nsec = 0;
  _tindx = 0;
  _tindxLast = 0;
  _tindxLastLast = 0;
  _toffset = 0;
  _firstRun = True;
  //
  // Initialize:
  //
  // init() sets _profile, which tells the log object how many columns there
  // are.  Thus init() must be called before _log is constructed.  init()
  // also reads in configuration constants like _xzAngle.
  init();
  //
  _output  = new EchoSounderOutput();
  _command = new EchoSounderCommand(MessageQueue::ReadWrite);
  _log     = new EchoSounderLog(this, DataLog::BinaryFormat, _profile);
  //
  // Write out constants only once.

  for(int i=0; i<EchoSounderIF::ScanArraySize; i++) 
  {
     _output->data.ranges[i]=-2;
  }
  _output->data.maxRange = (unsigned char) _maxRange;
  _output->data.xzAngle  = _xzAngle;
  _output->data.tindxMax = 0;
  _output->data.tindxMin = EchoSounderIF::ScanArraySize;
  _output->write();


   Syslog::write("EchoSounderDriver:: Constructor finishing.");
}


EchoSounderDriver::~EchoSounderDriver()
{
   Syslog::write("EchoSounderDriver:: Destructor executing.");
   delete _log;
   delete _output;
   delete _command;
}

void EchoSounderDriver::init()
{
   
  Boolean debug = debug_flag;
  char configFile[100];
  strcpy(configFile, System::configurationFile(_config));

  configFile[99]='\0';
  dprintf("Configuration file = %s\n", configFile);
  createCfgAttributes();
  loadConfigFile(configFile);
  System::copyToLogDir(configFile);
  printCfgAttributes();
  //
  // Build the interrogation "switch data" string:
  //
  _interrogation[0] = 0xFE;             //Header
  _interrogation[1] = 0x44;             //Header
  _interrogation[2] = 0x10;             //Head ID
  _interrogation[3] = (unsigned char) _maxRange;  //Maximum range setting
  _interrogation[4] = 0;                //Reserved
  _interrogation[5] = 0;                //Pause=1/Resume=0
  _interrogation[6] = 0;                //Reserved
  _interrogation[7] = 0;                //Reserved
  _interrogation[8] = (unsigned char) _startGain;//Start gain, 0-40, 1 dB incr
  _interrogation[9] = 1;                //LogF - fixed at 1 => 20 dB
  _interrogation[10]= (unsigned char) _absorption;//Two-way absorption,dB/m*100
  _interrogation[11]= (unsigned char) (_trainRef*180/PI/3 + 60); //Train angle
  //_interrogation[12]= (unsigned char) (_sector*180/PI/3); //Sector angle
  _interrogation[12]= _sector;          //Sector angle
  _interrogation[13]= (unsigned char) _stepSize;          //.3 Degrees/step
  _interrogation[14]= (unsigned char) _pulseLen;//Pulse length,10 usec incr
  _interrogation[15]= (unsigned char) _minRange;//Min range, 0-25, .1m incr
  // Trigger control; 0 or 1=> disabled, 2=> falling edge, 3=> rising
  _interrogation[16]= (unsigned char) _triggerCtrl;
  _interrogation[17]= 0;                //Reserved
  _interrogation[18]= 0;                //Reserved
  _interrogation[19]= 50;               //50 => 500 data points;no other choice
  _interrogation[20]= 8;                //8 => 8 bit resolution;no other choice
  _interrogation[21]= 0;                //Reserved
  _interrogation[22]= (unsigned char) _profile;// 0=>off, 1=>on, no data bytes
  _interrogation[23]= 0;                //Reserved
  _interrogation[24]= (unsigned char) _delay;//Return delay,0-250ms,in 2 ms incr
  _interrogation[25]= (unsigned char) _acousticFreq; //Frequency, scaled&shifted
  _interrogation[26]= 0xFD;             //Termination byte

  dprintf("The train angle = %.2f degrees; byte value = %d\n",
	  _trainRef*180/PI, 
	  (unsigned char) (_trainRef*180/PI/3 + 60) );

  dprintf("strlen(_interrogation) = %d\n", strlen(_interrogation));

  for( int i=0; i<27; i++) printf("%2X",_interrogation[i]);  printf("\n");
  Syslog::write("Scaled and shifted frequency = %d", _acousticFreq);


  _device->openSD();
#if 0
  char serverString[64];
  serverString[63] = '\0';
  strcpy( serverString, _device->name() );
  printf("EchoSounderDriver:: _device->name() = %s\n", serverString);
#endif     

  _nBytesRead = 0;
}
//
// Request device to ping, and then parse range values.
// Place values in output object.
//
void EchoSounderDriver::acquireData()
{
  Boolean debug = debug_flag;

  int tbytes;
  int shortMsgLen = 13;
  int longMsgLen  = 513;
  int timeOut = 2500;          //2.5 seconds
  char responseHdr[8];
  responseHdr[3] = '\0';

//  _device->clearPort();
  //
  // Don't use strlen because the string is not ASCII and contains zeros
  // before the end.  
  // _device->write(_interrogation, strlen(_interrogation));
//  for( int i=0; i<27; i++) printf("%2X",_interrogation[i]);  printf("\n");
//  printf("Got to A.\n");
//  _device->commsDebugMode(SerialDeviceExt::DebugOn);
//  printf("sd = %d\n", _device->getFd());

  _device->write(_interrogation, 27);
  _device->commsDebugMode(SerialDeviceExt::DebugOff);
  _error = False;
  _lastAskTime = _askTime;
  _askTime = Time::milliseconds();
  try
  {
     if( _profile )
     {
	_nBytesRead = _device->readNChars(_response, shortMsgLen, timeOut);
	_ansTime = Time::milliseconds();
	Time::gettime( &_updateTime );
	_compPeriod = _ansTime-_askTime;
	tbytes = _device->nRecvdBytes();
	strncpy(responseHdr, _response, 3);
	dprintf("Short Msg; responseHdr = %s, _nBytesRead = %d, tbytes = %d" 
		" deltaT = %d msec\n",
		responseHdr, _nBytesRead, tbytes, _compPeriod);
     }
     else
     {
	_nBytesRead = _device->readNChars(_response, longMsgLen, timeOut);
	_ansTime = Time::milliseconds();
	Time::gettime( &_updateTime );
	_compPeriod = _ansTime-_askTime;
	tbytes = _device->nRecvdBytes();
	strncpy(responseHdr, _response, 3);
	dprintf("Long Msg; responseHdr = %s, _nBytesRead = %d, tbytes = %d" 
		" deltaT = %d msec\n",
		responseHdr, _nBytesRead, tbytes, _compPeriod);
     }
     if( (_nBytesRead != shortMsgLen) && (_nBytesRead != longMsgLen) )
     {
	_error = True;
	Syslog::write("EchoSounderDriver::Error - received %d bytes, which "
		      "is incorrect.", _nBytesRead);
	Time::gettime( &_updateTime );
     }
  } // try
  catch (Exception e) 
  {
     Syslog::write("EchoSounderDriver - caught exception: %s ", e.msg);
     _error = True;
     Time::gettime( &_updateTime );
  }
  catch (...) 
  {
     Syslog::write("EchoSounderDriver - caught exception.");
     _error = True;
     Time::gettime( &_updateTime );
  }


  if( _error ) resetSD();

  _period = _askTime - _lastAskTime;

  if( _period > 10 ) _freq = 1000./_period;
  else               _freq = 1000.;

  //printf("_period = %d milliseconds\n", _period);


  _response[519] = '\0';

}

void EchoSounderDriver::resetSD(void)
{
   int nTries = 0;
   int tooManyTries = 2;
   int fd;
   _crashCtr++;
   Syslog::write("EchoSounderDriver:: Reopening socket\n");
   printf("EchoSounderDriver:: Reopening socket\n");

   while( _error )
   {
      if( nTries >= tooManyTries ) 
      {
	 char *errorStr = "EchoSounderDriver::resetSD - Error "
	    "Cannot restablish connection to the OA sonar after %d tries.\n"
	    "ABORTING MISSION\n";
	 Syslog::write(errorStr, nTries);
	 printf(errorStr, nTries);
	 //
	 //Issue abort here
	 exit(-1);
      }
      try
      {
	 _device->closeSD();
	 fd = _device->openSD();
      }
      catch (Exception e) 
      {
	 nTries++;
	 Syslog::write("EchoSounderDriver::resetSD - "
		       "caught exception: %s \n nTries = %d", 
		       e.msg, nTries);
	 _error = True;
	 Time::gettime( &_updateTime );
	 continue;
      }
      catch (...) 
      {
	 nTries++;
	 Syslog::write("EchoSounderDriver::resetSD - "
		       "caught unknown exception. nTries = %d", nTries);
	 _error = True;
	 Time::gettime( &_updateTime );
	 continue;
      }
      //
      // If there were no exceptions, and _fd>0, the connection is
      // re-estabilished.  Reset _error to false and exit this routine.
      if( fd > 0 ) 
      {
	 _error = False;
	 char *str = "EchoSounderDriver::resetSD - connection re-estabilished "
	    "after %d tries.\n";
	 Syslog::write(str, nTries);
	 printf(str, nTries);
      }
   }
}


void EchoSounderDriver::parseData()
{
  Boolean debug = debug_flag;

  strncpy(_header, _response, 3);
  _header[3] ='\0';

  int indx = 3;
  unsigned char hiByte, loByte;
  _headID       = _response[indx];indx++;
  //_serialStatus = _response[indx];indx+=3;
  _serialStatus = _response[indx];indx++;
  //
  // Parse out the train angle value from bytes 5 and 6:
  hiByte = (_response[indx+1] & 0x3E) >> 1;
  loByte =((_response[indx+1] & 0x01) << 7) | (_response[indx] & 0x7F);
  _train        = (short) ( (hiByte << 8) | loByte );
  indx+=2;
  _maxRangeCfrm = (unsigned char) _response[indx];indx++;
  //
  // Parse out the range value from bytes 8 and 9:
  hiByte = (_response[indx+1] & 0x7E) >> 1;
  loByte =((_response[indx+1] & 0x01) << 7) | (_response[indx] & 0x7F);
  _range        = (short) ( (hiByte << 8) | loByte );
  indx+=2;
  //
  // Parse out the number of data bytes from bytes 10 and 11:
  hiByte = (_response[indx+1] & 0x7E) >> 1;
  loByte =((_response[indx+1] & 0x01) << 7) | (_response[indx] & 0x7F);
  _nBytes  = (short) ( (hiByte << 8) | loByte );
  indx+=2;

  dprintf("indx = %d\n", indx);

  char errorStr[100];
  if( !strcmp(_header, "IPX") )
  {
     //
     // There is no data, and byte 13 should be the terminating character.
     if( (short)_response[indx] != 0xFC ) 
     {
	sprintf(errorStr, "EchoSounderDriver::parseData() - Error \n"
		"     Incorrect terminating character %2X for %s header.\n"
		, _response[indx], _header);
	throw Exception(errorStr);
     }
  }
  else if( !strcmp(_header, "IGX") )
  {
     for( ; indx<512; indx++ )
     {
	_echoData[indx-12] = _response[indx];
     }	
     //
     // After 500 bytes of data, byte 512 should be the terminating character.
     if( _response[indx] != 0xFC ) 
     {
	sprintf(errorStr, "EchoSounderDriver::parseData() - Error \n"
		"     Incorrect terminating character %2X for %s header.\n"
		, ((int)_response[indx]), _header);
	throw Exception(errorStr);
     }
  }
  else
  {
     sprintf(errorStr, "EchoSounderDriver::parseData() - Error \n"
	     "     Incorrect header %s.\n"
	     , _header);
     throw Exception(errorStr);
  }
}


//void EchoSounderDriver::printData( char *header, char headID, 
void printData( char *header, char headID, 
char serialStatus, double dtrain, double sectorEng, unsigned char maxRangeCfrm,
		double drange, short nBytes,
		short nBytesRead, double freq, 
		long crashCtr, char *echoData, Boolean WantToSeeIt )
{
  Boolean debug = debug_flag;

  Syslog::write("Header              = %s\n"
		"  Head ID             = %d\n"
		"  Serial Status       = %d (65=Command Accepted, 1=>Rejected)\n"
		"  Train               = %-6.1f degrees\n"
		"  Sector              = %-6.1f degrees\n"
		"  Max Range           = %d meters\n"
		"  Range               = %-6.2f (meters if maximum "
		                         "range >= 5 meters)\n"
		"  Header - Data Bytes = %d Number of bytes\n"
		"  Total bytes read    = %d\n"
		"  Ping frequency      = %.3f Hz\n"
		"  Disconnects         = %d\n"
		,header, headID, serialStatus, dtrain*180./PI,
                sectorEng*180./PI, maxRangeCfrm, drange , nBytes,
		nBytesRead, freq, crashCtr);

//  if( _nBytes == EchoSounderIF::EchoArraySize )
  if( nBytes == EchoSounderIF::EchoArraySize && WantToSeeIt )
  {
     int max = 20;
     for( int j=0; j<25; j++ )
     {
	for( int i=0; i<max; i++ )
	{
	   printf("%3d ", echoData[i+j*max] );
	}
	printf("\n");
     }
  }
}


void EchoSounderDriver::run()
{
  Boolean debug = debug_flag;
  short cntr = 0;
  //
  // Check for any commands.  Execute them if requested.
  //
  EchoSounderCommand::Command cmdObj;
  //
  // Check for commands each time through the loop.
  // while ((_command->read(&cmdObj)) > 0)
  while(1)
  {
     while ((_command->read(&cmdObj)) > 0)
     {
	switch (cmdObj._cmd) {
	   case EchoSounderCommand::Start:
	      _running = True;
	      _device->openSD();
	      Syslog::write("EchoSounderDriver::Start\n");
 	      break;

	   case EchoSounderCommand::Stop:
	      _running = False;
	      _device->closeSD();
	      Syslog::write("EchoSounderDriver::Stop\n");
	      break;

	   case EchoSounderCommand::MoveHead:
	      Syslog::write("EchoSounderDriver::MoveHead to %.2f\n", 
			    cmdObj._cmdArg*180/PI);
	      _interrogation[11]= 
		 (unsigned char) (cmdObj._cmdArg*180./PI/3 + 60.);
	      //
	      // Re-determine the max and min scan array indices:
	      _output->data.tindxMax = 0;
	      _output->data.tindxMin = EchoSounderIF::ScanArraySize;
	      break;

	   case EchoSounderCommand::ScanSector:
	      _sectorEng = cmdObj._cmdArg;
	      Syslog::write("EchoSounderDriver::Set scan sector to %.2f\n", 
			    _sectorEng*180/PI);
	      _sector = (unsigned char) (_sectorEng*180./PI/3 );
	      _interrogation[12]= _sector;
	      //
	      // Re-determine the max and min scan array indices:
	      _output->data.tindxMax = 0;
	      _output->data.tindxMin = EchoSounderIF::ScanArraySize;
	      break;

	   default:
	      break;
	}
	printf("%d messages pending.\n", _command->msgsPending());
     }

     if( !_running )
     {
	sleep(2);    //Wait for two seconds, then poll for a command.
	continue;
     }

     if( (_nsleepTime.tv_sec != 0) || (_nsleepTime.tv_nsec != 0) )  
     {
	nanosleep(&_nsleepTime, 0);
     }

     acquireData();   // Block until the data comes back:

     Boolean localError = False;
     if( _nBytesRead == -1 ) 
     {
	localError = True;
	Syslog::write("EchoSounderDriver - error from readNChars.");
     }
     else parseData();
#if 0
     //
     // The instrument returns a zero if it didn't detect a return because
     // the bottom is either too far or too close.  We can't let this through
     // because the altitude-following or OA control will believe it and
     // likely abort the mission.
     //
     if( _range <= _minRange*10 ) _range = (short) _maxRange*100;
#endif
     double drange = ((double)_range)/100.;
     double dtrain = ((double)(_train-trainShift))*trainDegPerStep*PI/180.;
     if( !localError )
     {
	
	if(debug) printData( _header, _headID, _serialStatus, dtrain, _sectorEng,
			     _maxRangeCfrm, drange, _nBytes, _nBytesRead, _freq, 
			     _crashCtr, _echoData);
	_log->write();
     }
     //
     // If the instrument is a fixed beam, or if the scanner had the head at
     // a fixed angle, copy _range to the appropriate slot in the scan array.
     //

     _lastToffset = _toffset;
     _toffset = _train % 8;
     if( _firstRun )
     {
	Syslog::write("EchoSounderDriver::run() _toffset = %d",_toffset);
	_lastToffset = _toffset;
	_firstRun = False;
     }
	
     if( _lastToffset != _toffset )
     {
	Syslog::write("EchoSounderDriver::run() Error, toffset changed to %d",
	   _toffset);
	_lastToffset = _toffset;
     }

     _tindxLastLast = _tindxLast;
     _tindxLast = _tindx;
     _tindx  = _train / 8;

     if( _tindx < EchoSounderIF::ScanArraySize && _tindx >= 0 )
     {
	if( drange > _noReturnRange )
	{
	   _output->data.ranges[_tindx] = 0;
	   _output->data.echoReceived = False;
	}
	else
	{
	   _output->data.ranges[_tindx] = _range;
	   _output->data.echoReceived = True;
	}

	_output->data.times[_tindx]  = Time::milliseconds();
	//_output->data.times[_tindx].seconds = _updateTime.seconds;
	//_output->data.times[_tindx].nanoSeconds = _updateTime.nanoSeconds;

	if( _sector>0 )
	{
	   if( _tindx && _tindxLast && _tindxLastLast )
	   {
	      //
	      // If it switched to descending latch the maximum index:
	      if( _tindxLastLast < _tindxLast && _tindx < _tindxLast ) 
		 _output->data.tindxMax = _tindx;
	      //
	      // If it switched to ascending latch the minimum index:
	      if( _tindxLastLast > _tindxLast && _tindx > _tindxLast ) 
		 _output->data.tindxMin = _tindx;
	   }
	   else
	   {
	      if( _tindx > _output->data.tindxMax ) _output->data.tindxMax = _tindx;
	      if( _tindx < _output->data.tindxMin ) _output->data.tindxMin = _tindx;
	   }
	}
	else
	{
	   _output->data.tindxMax = _tindx;
	   _output->data.tindxMin = _tindx;
	}
     }
     else
	Syslog::write("EchoSounderDriver::run() Error; tindx = %d "
	              "is out of bounds.", _tindx);

     //
     // Navigation/Echosounder.cc checks the "switches accepted" bit of the
     // serial status byte and declares an error if it's not set.

     _output->data.error   = _error;
     _output->data.updateTime.seconds      = _updateTime.seconds;
     _output->data.updateTime.nanoSeconds  = _updateTime.nanoSeconds;
     _output->data.nBytesRead  = _nBytesRead;
     strcpy( _output->data.header, _header );
     _output->data.headID  = _headID;
     _output->data.serialStatus = _serialStatus;
     _output->data.train = _train;
     _output->data.sector = _sector;
     _output->data.maxRangeCfrm = _maxRangeCfrm;
     _output->data.nBytes  = _nBytes;
     _output->data.range   = drange;
     _output->data.freq    = _freq;
     _output->data.crashCtr= _crashCtr;
     _output->data.toffset = _toffset;
     _output->data.tindx   = _tindx;
     memcpy( _output->data.echoData, _echoData, sizeof(_echoData) );
     _output->write();
  }  
}

// ########### Configuration file attributes section ###########

// Create attributes in our attricutes member
//
void EchoSounderDriver::createCfgAttributes()
{
  _attributes.add( new IntegerAttribute("range",
                   "Maximum Range, meters", &_maxRange, 200 ) );
  _attributes.add( new FloatAttribute("noReturnRange",
                   "No Return Range, meters", &_noReturnRange, 60. ) );
  _attributes.add( new IntegerAttribute("startGain",
                   "Start Gain, dB", &_startGain, 10 ) );
  _attributes.add( new FloatAttribute("absorption",
                   "Two-Way Absorption, dB/m", &_absorpEng, .1 ) );
//                 "Two-Way Absorption, dB/m*100", &_absorption, 10 ) );
  _attributes.add( new AngleAttribute("trainAngle", "pointing angle",
				      &_trainRef, 0));
  _attributes.add( new AngleAttribute("sector", "scan sector size",
				      &_sectorEng, 0));
  _attributes.add( new AngleAttribute("stepSize", "step size",
				      &_stepSizeEng, trainDegPerStep));
  _attributes.add( new IntegerAttribute("pulseLen",
                   "Pulse Length, usec", &_pulseLenEng, 60 ) );
//                 "Pulse Length, usec/10", &_pulseLen, 10 ) );
  _attributes.add( new FloatAttribute("minRange",
                   "Minimum range, m", &_minRangeEng, 0 ) );
//                 "Minimum range, m*10", &_minRange, 20 ) );
  _attributes.add( new IntegerAttribute("triggerCtrl",
                   "Enable, Rising/Falling", &_triggerCtrl, 0 ) );
  _attributes.add( new BooleanAttribute("profile",
                   "Turn off Echo Data", &_profile, 1 ) );
  _attributes.add( new IntegerAttribute("delay",
                   "Return Delay, ms", &_delay, 0 ) );
  _attributes.add( new IntegerAttribute("acousticFreq",
                   "acoustic frequency kHz", &_acousticFreqEng, 330 ) );
  _attributes.add( new FloatAttribute("pingPeriod",
                   "ping period in Hz", &_pingPeriod, 2 ) );
  _attributes.add( new AngleAttribute("xzAngle", "x-z plane mounting angle",
				      &_xzAngle, 90));
//
// Simulation parameters.  These are not used during flight.
  _attributes.add( new AngleAttribute("minGrazingSim", "simulated min grazing angle",
				      &_minGrazing, 30));
}

// Load the config attributes and do some sanity checking
//
void EchoSounderDriver::loadConfigFile(const char* configFile)
{
  AttributeParser::parse(configFile, &_attributes); 

// Validate the input data, that is, check ranges here.

  _absorption   = (long) (_absorpEng*100.);
  if( (_absorption < 0) || (_absorption > 255) ) 
  {
     //
     // dB/m = _absorption/100.  Absorption in db/m*100, range is 0-255 =>
     // 0-2.55 db/m in .01 db/m increments.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for absorption. Exiting", _absorption);
     exit(-1);
  }

  _pulseLen     = (long) (_pulseLenEng/10.);
  if( (_pulseLen < 1) || (_pulseLen > 100) ) 
  {
     //
     // Microsecs = _pulseLen*10. Pulse length, range 1-100=> 10-1000
     // microsecs in 10 usec increments.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for pulse length. Exiting", _pulseLen);
     exit(-1);
  }

  _minRange     = (long) (_minRangeEng*10.);
  if( (_minRange < 0) || (_minRange > 250) ) 
  {
     //
     // Must be [0, 250].  Meters = _minRange/10.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for minRange. Exiting", _minRange);
     exit(-1);
  }

  if( (_maxRange < 0) || (_maxRange > 250) ) 
  {
     //
     // Must be [0, 250].  
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for maxRange. Exiting", _maxRange);
     exit(-1);
  }

  _acousticFreq = (long) ((_acousticFreqEng - 675)/5 + 100);
  if( (_acousticFreq < 0) || (_acousticFreq > 200) ) 
  {
     //
     // Must be [0, 200].  kHz = 5*(_acousticFreq - 100) + 675.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for acousticFreq. Exiting", _acousticFreq);
     exit(-1);
  }


  _stepSize = (long) ((_stepSizeEng)*180/PI/trainDegPerStep);
  if( (_stepSize < 0) || (_stepSize > 8) ) 
  {
     //
     // Must be [0, 8].  .3 degrees/step.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for step size. Exiting", _stepSize);
     exit(-1);
  }


  _sector = (unsigned char) ((_sectorEng)*180/PI/3);
  if( (_sector < 0) || (_sector > 120) ) 
  {
     //
     // Must be [0, 120].  3 degrees/step.
     Syslog::write("EchoSounderDriver - Error: Incorrect value %d "
		   "for sector. Exiting", _sector);
     exit(-1);
  }

  
  _nsleepTime.tv_sec  = (long) _pingPeriod;
  _nsleepTime.tv_nsec = (long) ((_pingPeriod - _nsleepTime.tv_sec) * 1e9);

  printf("sleepTime = %.4f seconds\n", 
	 _nsleepTime.tv_sec + _nsleepTime.tv_nsec*1.e-9);


  printf("tv_sec = %d, tv_nsec = %d\n", 
	 _nsleepTime.tv_sec,
	 _nsleepTime.tv_nsec );


}

// Record the attributes in Syslog
//
void EchoSounderDriver::printCfgAttributes()
{
   //
   // Convert the integer byte values to engineering units in the argument
   // list below.
   //
   Syslog::write("EchoSounder -- configuration:\n"
		 "\tMaximum Range  = %ld m\n"
		 "\tNo Return Range= %.2f m\n"
		 "\tStartGain      = %ld dB\n"
		 "\tAbsorption     = %.2f dB/m\n"
		 "\tPulse Length   = %ld microseconds\n"
		 "\tMinimum Range  = %.2f m\n"
		 "\tTrigger Control= %ld \n"
		 "\tProfile        = %d\n"
		 "\tDelay          = %ld milliseconds\n"
		 "\txz mounting an.= %.2f degrees\n"
		 "\tHead angle     = %.2f degrees\n"
		 "\tSector angle   = %.2f degrees\n"
		 "\tStep size      = %.2f degrees\n"
		 "\tAcoustic Freq  = %ld kHz\n"
		 "\tPing period    = %.2f seconds\n"
		 ,
		 _maxRange, 
                 _noReturnRange,
		 _startGain, 
		 (float)(_absorption/100.), 
		 _pulseLen*10, 
		 (float)(_minRange/10.), 
		 _triggerCtrl, 
		 _profile, 
		 _delay*2,
		 _xzAngle*180./PI,
		 _trainRef*180./PI,
		 _sectorEng*180./PI,
		 _stepSize*trainDegPerStep,
		 (_acousticFreq-100)*5 + 675,
		 _pingPeriod);
}