/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Kearfott.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  :                                                               */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
/*-----------------------------------------------------------------------*
  CLASS: Kearfott

  DESCRIPTION: Device interface for Kearfott SeaDevil INS

  $Id: Kearfott.cc,v 1.15 2001/08/02 02:53:19 hthomas Exp $
 *-----------------------------------------------------------------------*/

/*-----------------------------------------------------------------------*
  A NOTE ON COORDINATE SYSTEMS:

  This driver has been programmed to be consistent with the following 
  right-hand coordinate system:
  
  +X toward the nose of the AUV
  +Y to the starboard
  +Z down

  In this coord. system:

  Positive pitch is nose up
  Positive roll is toward the starboard (starboard down)
  Positive yaw is toward the starboard
 *-----------------------------------------------------------------------*/
#include <i86.h>
#include <sys/dev.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <time.h>
#include <fcntl.h>
#include "SemaphoreP.h"
#include "MathP.h"
#include "BooleanAttribute.h"
#include "IntegerAttribute.h"
#include "AttributeParser.h"
#include "VehicleConfigurationIF.h"
#include "SerialDevice.h"
#include "Syslog.h"
#include "Kearfott.h"
#include "KearfottOutput.h"
#include "KearfottLog.h"
#include "DvlLog.h"
#include "DvlOutput.h"
#include "NavUtils.h"

SerialDevice::lineFormat KearfottLineFormat = { 115200, 8, 1, "ODD" };

Kearfott::Kearfott(char *filename):
  Task("KearfottDriver")
{
     Boolean debug = True;
     short priority = 10;
     char rawLogName[255];

     _device = NULL;
     _displayNavMessages = False;
     _navMsgCnt = 0;
     _externalSensors = False;;
     
     _fd = open(filename, O_RDONLY);
     if (_fd < 0) {
	 perror("open():");
	 Syslog::write("unable to open raw log\n");
     } 
     _rawLog = -1;

     //set up connections to dvl and ins servers
     _kearfottOutput = new KearfottOutput();
     _dvlOutput = new DvlOutput();

     //set up dvl and INS logging
     _kearfottLog = new KearfottLog( this, DataLog::BinaryFormat );
     _dvlLog = new DvlLog(this, DataLog::BinaryFormat);

}

Kearfott::Kearfott(SerialDevice *device, Boolean extSensors):
  Task("KearfottDriver")
{
     Boolean debug = True;
     short priority = 10;
     char rawLogName[255];

     _device = device;
     _displayNavMessages = False;
     _navMsgCnt = 0;
     _externalSensors = extSensors;

     if (getenv("AUV_LOG_DIR")) {
       sprintf(rawLogName,"%s/latest/kearfottRaw.out",
	       getenv("AUV_LOG_DIR"));
     } else {
       sprintf(rawLogName,"kearfottRaw.out");       
     }
     printf("raw log file name: %s\n", rawLogName);
     _rawLog = open(rawLogName, O_CREAT|O_APPEND|O_WRONLY);
     if (_rawLog < 0) {
       perror("open():");
       Syslog::write("unable to open raw log\n");
     }

     //set up connections to dvl and ins servers
     _kearfottOutput = new KearfottOutput();
     _dvlOutput = new DvlOutput();

     //set up dvl and INS logging
     _kearfottLog = new KearfottLog( this, DataLog::BinaryFormat );
     _dvlLog = new DvlLog(this, DataLog::BinaryFormat);

     // Initialize the device itself
     initialize();

}


Kearfott::~Kearfott()
{
  if (_device) delete _device;
  delete _kearfottOutput;
  delete _dvlOutput;
  delete _kearfottLog;
  delete _dvlLog;
  if (_gpsIF) delete _gpsIF;
  if (_depthIF) delete _depthIF;
  if (_ctdIF) delete _ctdIF;

}

DeviceIF::Status Kearfott::initialize()
{
KEARFOTT_OP_TYPE opData;
KEARFOTT_CONFIG_TYPE config;

  //set up serial port 
  _device->setLineFormat( &KearfottLineFormat );
  _device->raw(100,20); //_device->nonBlocking();
  _device->clearPort();
  _device->commsDebugMode(SerialDevice::DebugOff);


  //zero out raw nav data
  memset(&_rawNavData, 0, sizeof(KEARFOTT_RAW_NAV_TYPE));
  memset(&_opData, 0, sizeof(KEARFOTT_OP_TYPE));
  _opValid = _configValid = False;

  //try to set up interfaces to GPS, depth sensor, and CTD

  if (_externalSensors) {
    try {
      _gpsIF = new GpsIF("Gps", 2);
      printf("gpsif is 0x%x\n", _gpsIF);
    }
    catch(...) {
      Syslog::write("%s: unable to open gps interface\n", name());
      _gpsIF = NULL;
    }

    try {
      _depthIF = new DepthSensorIF("DepthSensor", 2);
      printf("depthif is 0x%x\n", _depthIF);
    }
    catch(...) {
      Syslog::write("%s: unable to open depth interface\n", name());
      _depthIF = NULL;
    }

    try {
      _ctdIF = new CtdIF("seabirdServer", 2);
      printf("ctdIF is 0x%x\n", _ctdIF);
    }
    catch(...) {
      Syslog::write("%s: unable to open ctd interface\n", name());
      _ctdIF = NULL;
    }    
  }

  return DeviceIF::Ok;
}


DeviceIF::Status Kearfott::sendOpData(KEARFOTT_OP_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;
unsigned char *ptr;

    // save a copy for sending periodic updates
    memcpy(&_opData, data, sizeof(KEARFOTT_OP_TYPE));
    _opValid = True;

    msg.cmdID = accept_operational; 
    msg.numBytes = 21;
    ptr = &(msg.data[0]);
    memcpy(ptr, &(data->validity), 1); ptr += 1;
    memcpy(ptr, &(data->modes), 1); ptr += 1;
    memcpy(ptr, &(data->init_lat), 4); ptr += 4;
    memcpy(ptr, &(data->init_lon), 4); ptr += 4;
    memcpy(ptr, &(data->idepth), 4); ptr += 4;
    memcpy(ptr, &(data->inithdg), 2); ptr += 2;
    memcpy(ptr, &(data->salinity), 2); ptr += 2;
    memcpy(ptr, &(data->cepos), 2); ptr += 2;
    memcpy(ptr, &(data->forcomm), 1); ptr += 1;
    msg.checksum = checksum(&msg);
    sendMsg(&msg);
    /*
    printf("calling tcdrain()\n");
    tcdrain(_device->getFd());
    delay(40);
    printf("done calling tcdrain()\n");
    if (readEcho(&msg) == DeviceIF::Ok) {
      printf("echo of operational received\n");
    }
    */
    return DeviceIF::Ok;

}

DeviceIF::Status Kearfott::getConfigData(KEARFOTT_CONFIG_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;

  msg.cmdID = 6;
  msg.numBytes = 0;
  msg.checksum = checksum(&msg);
  sendMsg(&msg);
  /*
  tcdrain(_device->getFd());
  delay(40);
  if (readEcho(&msg) == DeviceIF::Ok) {
    printf("echo of get configuration received\n");
  }
  */
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::sendConfigData(KEARFOTT_CONFIG_TYPE *data)
{
KEARFOTT_MSG_TYPE msg;
unsigned char *ptr;

    memcpy(&_configData, data, sizeof(KEARFOTT_CONFIG_TYPE));
    _configValid = True;
    
    msg.cmdID = accept_configuration; 
    msg.numBytes = 26;
    ptr = &(msg.data[0]);
    memcpy(ptr, &(data->valpha), 2); ptr += 2;
    memcpy(ptr, &(data->vbeta), 2); ptr += 2;
    memcpy(ptr, &(data->vgamma), 2); ptr += 2;
    memcpy(ptr, &(data->configBits), 1); ptr += 1;
    memcpy(ptr, &(data->satime), 1); ptr += 1;
    memcpy(ptr, &(data->vdelx), 2); ptr += 2;
    memcpy(ptr, &(data->vdely), 2); ptr += 2;
    memcpy(ptr, &(data->vdelz), 2); ptr += 2;
    memcpy(ptr, &(data->gpslx), 2); ptr += 2;
    memcpy(ptr, &(data->gpsly), 2); ptr += 2;
    memcpy(ptr, &(data->gpslz), 2); ptr += 2;
    memcpy(ptr, &(data->doplx), 2); ptr += 2;
    memcpy(ptr, &(data->doply), 2); ptr += 2;
    memcpy(ptr, &(data->doplz), 2); ptr += 2;
    msg.checksum = checksum(&msg);
    sendMsg(&msg);
    /*
    tcdrain(_device->getFd());
    if (readEcho(&msg) == DeviceIF::Ok) {
      printf("echo of config received\n");
    }
    */
    return DeviceIF::Ok;
 
}

DeviceIF::Status Kearfott::parseConfigMessage(KEARFOTT_MSG_TYPE *msg)
{
  printf("configuration message received\n");
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseNavigationMessage(KEARFOTT_MSG_TYPE *msg)
{
const char *ptr;

  ptr = (const char *)(msg->data);
  memcpy(&_rawNavData.cycles, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.mode, ptr, 1); ptr += 1;
  memcpy(&_rawNavData.monitor, ptr, 1); ptr += 1;
  memcpy(&_rawNavData.lat, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.lon, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.depth, ptr, 4); ptr += 4;
  memcpy(&_rawNavData.bheight, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.roll, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.pitch, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.heading, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyx, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyy, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.vbodyz, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accelx, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accely, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.accelz, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.prate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.qrate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.rrate, ptr, 2); ptr += 2;
  memcpy(&_rawNavData.utcTime, ptr, 4); ptr += 4;


  _scaledNavData.cycles = _rawNavData.cycles;
  _scaledNavData.mode = _rawNavData.mode;
  _scaledNavData.monitor = _rawNavData.monitor;
  _scaledNavData.lat = Math::degToRad(_rawNavData.lat*8.3819e-8);
  _scaledNavData.lon = Math::degToRad(_rawNavData.lon*8.3819e-8);
  _scaledNavData.depth = _rawNavData.depth/10.0;
  _scaledNavData.bheight = _rawNavData.bheight/100.0;
  _scaledNavData.roll = Math::degToRad(_rawNavData.roll*0.0055);
  _scaledNavData.pitch = Math::degToRad(_rawNavData.pitch*0.0055);
  _scaledNavData.heading = Math::degToRad(_rawNavData.heading*0.0055);
  _scaledNavData.vbodyx = _rawNavData.vbodyx/1024.0;
  _scaledNavData.vbodyy = _rawNavData.vbodyy/1024.0;
  _scaledNavData.vbodyz = _rawNavData.vbodyz/1024.0;
  _scaledNavData.accelx = _rawNavData.accelx/1024.0;
  _scaledNavData.accely = _rawNavData.accely/1024.0;
  _scaledNavData.accelz = _rawNavData.accelz/1024.0;
  _scaledNavData.prate = _rawNavData.prate/8192.0;
  _scaledNavData.qrate = -_rawNavData.qrate/8192.0;
  _scaledNavData.rrate = -_rawNavData.rrate/8192.0;
  _scaledNavData.utcTime = _rawNavData.utcTime/16384.0;

  NavUtils::geoToUtm(_scaledNavData.lat, _scaledNavData.lon, 10,
		     &_scaledNavData.northing,
		     &_scaledNavData.easting);

  memcpy(&(_kearfottOutput->data.navData),
	 &_scaledNavData,
	 sizeof(KEARFOTT_SCALED_NAV_TYPE));
  _kearfottOutput->write();
  _kearfottLog->write();

  _navMsgCnt++;

  return DeviceIF::Ok;
}

void Kearfott::printRawNavType(KEARFOTT_RAW_NAV_TYPE *rawNav)
{
  printf("cntr: %d  mode: 0x%x  monitor: 0x%x\n",
	 rawNav->cycles, rawNav->mode, rawNav->monitor);
  printf("lat: %f  lon: %f depth: %f  height: %f\n",
	 Math::radToDeg(rawNav->lat*8.3819e-8), 
	 Math::radToDeg(rawNav->lon*8.3919e-8),
	 rawNav->depth/10.0, rawNav->bheight/100.0);
  printf("roll: %f pitch: %f hdg: %f VN: %f VE: %f VDOWN: %f\n",
	 rawNav->roll*0.0055, rawNav->pitch*0.0055, rawNav->heading*0.0055,
	 Math::radToDeg((float)((float)rawNav->vbodyx/1024.0)),
	 Math::radToDeg((float)((float)rawNav->vbodyy/1024.0)),
	 Math::radToDeg((float)((float)rawNav->vbodyz/1024.0)));
  printf("ax: %f ay: %f az: %f pr: %f qr: %f rr: %f\n",
	 (float)((float)rawNav->accelx/1024.0),
	 (float)((float)rawNav->accely/1024.0),
	 (float)((float)rawNav->accelz/1024.0),
	 (float)((float)rawNav->prate/8192.0),
	 (float)((float)rawNav->qrate/8192.0),
	 (float)((float)rawNav->rrate/8192.0));
  printf("time %f sec\n", (float)((float)rawNav->utcTime/16384.0));
}

void Kearfott::printScaledNavType(KEARFOTT_SCALED_NAV_TYPE *scaledNav)
{
  printf("cntr: %d  mode: 0x%x  monitor: 0x%x\n",
	 scaledNav->cycles, scaledNav->mode, scaledNav->monitor);
  printf("lat: %f  lon: %f depth: %f  height: %f\n",
	 Math::radToDeg(scaledNav->lat), 
	 Math::radToDeg(scaledNav->lon),
	 scaledNav->depth, scaledNav->bheight);
  printf("roll: %f pitch: %f hdg: %f VN: %f VE: %f VDOWN: %f\n",
	 Math::radToDeg(scaledNav->roll),
	 Math::radToDeg(scaledNav->pitch),
	 Math::radToDeg(scaledNav->heading),
	 scaledNav->vbodyx,
	 scaledNav->vbodyy,
	 scaledNav->vbodyz);
  printf("ax: %f ay: %f az: %f pr: %f qr: %f rr: %f\n",
	 scaledNav->accelx,
	 scaledNav->accely,
	 scaledNav->accelz,
	 scaledNav->prate,
	 scaledNav->qrate,
	 scaledNav->rrate);
  printf("time %f sec\n",scaledNav->utcTime);
}

void Kearfott::displayNavigationMessages(int skip)
{
  _displayNavMessages = True;
  _navSkip = skip;
}

DeviceIF::Status Kearfott::parseSlowTestMessage(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseHighSpeedIMUData(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseDopplerPD4(KEARFOTT_MSG_TYPE *msg)
{
const char *ptr;

  ptr = (const char *)(msg->data);
  ptr += 2;
  memcpy(&_dopplerPD4.dvlid, ptr, 1); ptr += 1;
  printf("dvlID is 0x%x\n", _dopplerPD4.dvlid);
  memcpy(&_dopplerPD4.datstr, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.numbyte, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.sysconfig, ptr, 1); ptr += 1;
  printf("sys configuration is 0x%x\n", _dopplerPD4.sysconfig);
  memcpy(&_dopplerPD4.xvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.yvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.zvelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.evelbtm, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm1, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm2, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm3, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.bm4, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.botstat, ptr, 1); ptr += 1;
  printf("xvel:%d yvel:%d zvel:%d evel:%d\n",
	 _dopplerPD4.xvelbtm, _dopplerPD4.yvelbtm,
	 _dopplerPD4.zvelbtm, _dopplerPD4.evelbtm);
  printf("bm1:%d bm2:%d bm3:%d bm4:%d\n",
	 _dopplerPD4.bm1, _dopplerPD4.bm2,
	 _dopplerPD4.bm3, _dopplerPD4.bm4);
  printf("bottom status is 0x%x\n", _dopplerPD4.botstat);
  memcpy(&_dopplerPD4.xvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.yvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.zvelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.evelref, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refstrt, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refend, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.refstat, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_hr, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_min, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_sec, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.toping_hun, ptr, 1); ptr += 1;
  memcpy(&_dopplerPD4.bit, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.spdsnd, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.temp, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.checksum, ptr, 2); ptr += 2;
  memcpy(&_dopplerPD4.ktnav, ptr, 2); ptr += 2;
  _dvlOutput->data.pingTime = _dopplerPD4.toping_hr*3600+
    _dopplerPD4.toping_min*60 +_dopplerPD4.toping_sec +
    _dopplerPD4.toping_hun/100.0;
  _dvlOutput->data.badComms = 0;
  _dvlOutput->data.bottomTrackVelocity[0] = _dopplerPD4.xvelbtm/1000.0;
  _dvlOutput->data.bottomTrackVelocity[1] = _dopplerPD4.yvelbtm/1000.0;
  _dvlOutput->data.bottomTrackVelocity[2] = _dopplerPD4.zvelbtm/1000.0;
  _dvlOutput->data.bottomTrackVelocity[3] = _dopplerPD4.evelbtm/1000.0;
  _dvlOutput->data.waterMassVelocity[0] = _dopplerPD4.xvelref/1000.0;
  _dvlOutput->data.waterMassVelocity[1] = _dopplerPD4.yvelref/1000.0;
  _dvlOutput->data.waterMassVelocity[2] = _dopplerPD4.zvelref/1000.0;
  _dvlOutput->data.waterMassVelocity[3] = _dopplerPD4.evelref/1000.0;
  _dvlOutput->data.bottomStatus = _dopplerPD4.botstat;
  _dvlOutput->data.waterStatus = _dopplerPD4.refstat;
  _dvlOutput->data.range = _scaledNavData.bheight;
  _dvlOutput->write();
  _dvlLog->write();

  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseGPS_PVTA(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseGPS_PVTB(KEARFOTT_MSG_TYPE *msg)
{
unsigned short sval;

  memcpy(&sval, &(msg->data[28]),2);
  //  printf("channel 1 status A 0x%x\n", sval);
  memcpy(&sval, &(msg->data[30]),2);
  //  printf("channel 1 status B 0x%x\n", sval);
  memcpy(&sval, &(msg->data[32]),2);
  //  printf("channel 2 status A 0x%x\n", sval);
  memcpy(&sval, &(msg->data[34]),2);
  //  printf("channel 2 status B 0x%x\n", sval);
  memcpy(&sval, &(msg->data[36]),2);
  //  printf("channel 3 status A 0x%x\n", sval);
  memcpy(&sval, &(msg->data[38]),2);
  //  printf("channel 3 status B 0x%x\n", sval);
  memcpy(&sval, &(msg->data[40]),2);
  //  printf("channel 4 status A 0x%x\n", sval);
  memcpy(&sval, &(msg->data[42]),2);
  //  printf("channel 4 status B 0x%x\n", sval);
  memcpy(&sval, &(msg->data[44]),2);
  //  printf("channel 5 status A 0x%x\n", sval);
  memcpy(&sval, &(msg->data[46]),2);
  //  printf("channel 5 status B 0x%x\n", sval);
  memcpy(&sval, &(msg->data[48]), 2);
  //  printf("figure of merit word is 0x%x\n", sval);
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseGPS_GGA(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseGPS_GSA(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseGPS_VTG(KEARFOTT_MSG_TYPE *msg)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseSlowTestMessage(unsigned char *data)
{
  return DeviceIF::Ok;
}

DeviceIF::Status Kearfott::parseFastTestMessage(unsigned char *data)
{
  return DeviceIF::Ok;
}


DeviceIF::Status Kearfott::sendMsg(KEARFOTT_MSG_TYPE *msg)
{
char val;

 
    val = 0xaa; _device->write(&val, 1);
    val = msg->cmdID; _device->write(&val, 1);
    val = msg->numBytes; _device->write(&val, 1);
    if (msg->numBytes) {
      _device->write((char const *)msg->data, msg->numBytes);
    }
    val = msg->checksum; _device->write(&val, 1);

    return DeviceIF::Ok;
}

void Kearfott::logMsg(KEARFOTT_MSG_TYPE *msg)
{
char val;

 
    val = 0xaa; write(_rawLog, &val, 1);
    val = msg->cmdID; write(_rawLog, &val, 1);
    val = msg->numBytes; write(_rawLog, &val, 1);
    if (msg->numBytes) {
      write(_rawLog, (char const *)msg->data, msg->numBytes);
    }
    val = msg->checksum; write(_rawLog, &val, 1);
}


DeviceIF::Status Kearfott::parseMsg(KEARFOTT_MSG_TYPE *msg)
{
  switch (msg->cmdID)
    {
    case accept_configuration:
    case accept_operational:
    case return_configuration:
      printf("received echo of %d\n", msg->cmdID);
      return DeviceIF::Ok;
    case navigation_data:
      //      printf("received navigation data\n");
      return parseNavigationMessage(msg);
    case configuration_data:
      printf("received configuration data\n");
      return parseConfigMessage(msg);
    case slow_test_message:
      //      printf("received slow test message\n");
      return parseSlowTestMessage(msg);
    case high_speed_IMU_data:
      //      printf("received high speed navigation message\n");
      return parseHighSpeedIMUData(msg);
    case doppler_PD4:
      //      printf("received doppler data\n");
      return parseDopplerPD4(msg);
    case gps_PVT_A:
      //      printf("received pvt a data\n");
      return parseGPS_PVTA(msg);
    case gps_PVT_B:
      //      printf("received pvt b data\n");
      return parseGPS_PVTB(msg);
    case gps_GGA:
      //      printf("received gps gga data\n");
      return parseGPS_GGA(msg);
    case gps_GSA:
      //      printf("received gps gsa data\n");
      return parseGPS_GSA(msg);
    case gps_VTG:
      //      printf("received gps vtg data\n");
      return parseGPS_VTG(msg);
    default:
      printf("unknown command ID %d\n", msg->cmdID);
      return DeviceIF::Error;
    }
}

DeviceIF::Status Kearfott::readMsg(KEARFOTT_MSG_TYPE *msg)
{
char buffer[64]; //most we should have to read until we see another packet
Boolean readError = False;

 if (_device) {
   try {
     // Read until we get the header from kearfott
     char term[2];
     int nbytes;
     term[0] = 0xAA; term[1] = 0x00;
     _device->readUntil(term, 1000);
     _device->readNChars((char *)&msg->cmdID, 1, 250); //check for valid ID
     _device->readNChars((char *)&msg->numBytes,
			 1, 250); //check for valid numBytes
     do {
       if (isatty(_device->getFd())) {
	 nbytes = ::dev_read(_device->getFd(), (char *)msg->data, msg->numBytes,
			     msg->numBytes, 10, 0, 0, 0);
       } else {
	 nbytes = ::read(_device->getFd(), (char *)msg->data, msg->numBytes);
       }
     } while (nbytes == -1);

     //    nbytes = _device->readNChars((char *)msg->data, msg->numBytes, 250);
     if (nbytes != msg->numBytes) {
       printf("only read %d bytes\n", nbytes);
       perror("readNChars");
     }
     _device->readNChars((char *)&msg->checksum, 1, 250);
   }

   catch (SerialDevice::TimedOut) {
     readError = True;
     Syslog::write("%s::readMsg - serial device timed out", name());
   }
   catch (SerialDevice::BufferFull) {
     readError = True;
     Syslog::write("%s::readMsg - serial device buffer full", name());
   }
   catch (Exception e) {
     readError = True;
     Syslog::write ("SerialDevice::read() - %s", strerror(errno));    
     Syslog::write("%s::readMsg - caught exception:  abort", name());
     throw;
   }
 } else {
     // Read until we get the header from kearfott
     char c;
     int nbytes;
     do {
       nbytes = read(_fd, &c, 1);
     } while (nbytes != EOF && c != 0xAA);
     if (nbytes == EOF) return DeviceIF::Error;
     nbytes = read(_fd, (char *)&msg->cmdID, 1); //check for valid ID
     if (nbytes == EOF) return DeviceIF::Error;
     nbytes = read(_fd, (char *)&msg->numBytes, 1);
     if (nbytes == EOF) return DeviceIF::Error;
     nbytes = read(_fd, (char *)msg->data, msg->numBytes);
     if (nbytes != msg->numBytes) {
       printf("only read %d bytes\n", nbytes);
       perror("readNChars");
     }
     read(_fd, (char *)&msg->checksum, 1);
 }

  if (msg->cmdID == 1 && msg->numBytes != 46) {
    Syslog::write("wierd nav message\n");
    return DeviceIF::Error;
  }
  if (msg->cmdID == 21 && msg->numBytes != 49) {
    Syslog::write("wierd PD4 message\n");
    return DeviceIF::Error;

  }
  
  if ((msg->checksum != checksum(msg)) || readError == True) {
    //    printf("checksum error, msg ID %d, byte cnt %d\n",
    //	   msg->cmdID, msg->numBytes);
    return DeviceIF::Error;
  } else {
    logMsg(msg);
    return DeviceIF::Ok;
  }

}

DeviceIF::Status Kearfott::readEcho(KEARFOTT_MSG_TYPE *msg)
{
KEARFOTT_MSG_TYPE inMsg;

 while (readMsg(&inMsg) == DeviceIF::Ok) {
   if (inMsg.cmdID == msg->cmdID) {
     printf("echo of cmdid %d(%d) received\n", inMsg.cmdID, inMsg.numBytes);
     return DeviceIF::Ok;
   } else {
     parseMsg(&inMsg);
   }
 }
 return DeviceIF::Error;

}

unsigned char Kearfott::checksum(KEARFOTT_MSG_TYPE *msg)
{

     unsigned char i, sum=0;

     sum ^= msg->cmdID;
     sum ^= msg->numBytes;

     for (i = 0; i < msg->numBytes; i++) {
       sum ^= msg->data[i];
     }

     return sum;
}

void Kearfott::run()
{
KEARFOTT_MSG_TYPE inMsg;
time_t lastTime = time(NULL);
time_t lastCurrentDataTime = time(NULL);

 while (1) {
   if (readMsg(&inMsg) == DeviceIF::Ok){
     parseMsg(&inMsg);
     if (inMsg.cmdID == 1 && _displayNavMessages) {
       if (_navMsgCnt++ >= _navSkip) {
	 _navMsgCnt = 0;
	 printScaledNavType(&_scaledNavData);
       }
     }
   } else {
     //     exit(0);
   }

   if ((time(NULL) - lastCurrentDataTime) > 2) {
     lastCurrentDataTime = time(NULL);
     if (_opValid) sendOpData(&_opData);
<<<<<<< Kearfott.cc
//     if (_configValid) sendConfigData(&_configData);
=======
     //     if (_configValid) sendConfigData(&_configData);
>>>>>>> 1.13
   }

   if (_externalSensors) {
     if ((time(NULL) - lastTime) > 1) {
       lastTime = time(NULL);
       Syslog::write("%s: updating with external sensors\n", name());
       updateExternalSensors();
     }
   }
<<<<<<< Kearfott.cc
  //   delay(10);
=======
>>>>>>> 1.13
 }

}

void Kearfott::updateExternalSensors()
{
GpsIF::Fix fix;
double currentDepth;
TimeIF::TimeSpec sampleTime;
DeviceIF::Status gpsStatus, depthStatus;
KEARFOTT_OP_TYPE opdata;

    // set initial values 
    opdata.validity = 0; 
    opdata.idepth = 0;
    opdata.modes = GPSENAB|DOPENAB|DOPPING;

    /*
    if (_gpsIF) {
      gpsStatus = _gpsIF->getFix(&fix);
      if (gpsStatus == DeviceIF::Ok) {
	if (fix.quality != GpsIF::Invalid) {
	  if (fix.nsHemisphere == GpsIF::Southern) fix.latitude  *= -1.;
	  if (fix.ewHemisphere == GpsIF::Western) fix.longitude *= -1.;
	  printf("updating position with lat %f lon %f\n",
		 Math::radToDeg(fix.latitude),
		 Math::radToDeg(fix.longitude));
	  opdata.init_lat = Math::radToDeg(fix.latitude)/8.3819e-8;
	  opdata.init_lon = Math::radToDeg(-fix.longitude)*8.3819e-8;
	  opdata.cepos = 1000;
	  opdata.validity |= VPOS;
	}
      }
    }
    */

    if (_depthIF) {
      depthStatus = _depthIF->depth(&currentDepth, &sampleTime);
      if (depthStatus == DeviceIF::Ok) {
	int ival;
	ival = (int)(currentDepth * 100);
	printf("updating depth with %f(%d)\n", currentDepth, ival);
	opdata.idepth = ival;
	opdata.validity |= VDPTH|VERTDAMP_PRESS;
      }
    }

    //    if (_ctdIF) {
      opdata.salinity = 3350;
      opdata.validity |= VSALIN;
      //    }

    //only send update if we are in aided nav mode
    if (_rawNavData.mode >=0x06) sendOpData(&opdata);
 
}