/****************************************************************************/ /* 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.46 2012/10/26 17:06:29 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 #include #include #include #include #include #include #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 "DvlOutput.h" //#include "GpsOutput.h" #include "GpsUtils.h" #include "NavUtils.h" #include SerialDevice::lineFormat KearfottLineFormat = { 115200, 8, 1, "ODD" }; //short pvtBuf[67]; //int ln250_gps_port, ln250_tmark_port; const double cos30 = sqrt(3.)/2.; char rawFileBuffer[1024*16]; int nRawFileBytes; void swapNBytes(char *p, int n) { char val; int i; for (i = 0; i < n>>1; i++) { val = p[i]; p[i] = p[n-i-1]; p[n-i-1] = val; } } static double b8dec_double(char *p) { unsigned long exp; union u_tag { double d; unsigned long l[2]; char c[8]; } u; int i; char tmp; for(i=0; i<8; i++){ u.c[i] = *p++; } swapNBytes((char *)&u, 8); if(u.l[0] == 0x00000000)return(0.0); exp = (u.l[0] & 0x7ff00000) - 0x00200000; u.l[0] = (u.l[0] & 0x800fffff) | exp; if(u.l[0] == 0x80000000)return(0.0); return( u.d ); } static float b4dec_float(char *p) { unsigned long exp; union u_tag { float f; unsigned long l; unsigned char c[4];} u; unsigned char tmp; int i; u.l = (*p++ << 24); u.l |= (*p++ << 16); u.l |= (*p++ << 8); u.l |= (*p++ ); // swapNBytes((char *)&u, 4); if(u.l == 0x00000000)return(0.0f); exp = (u.l & 0x7f800000) - 0x01000000; u.l = (u.l & 0x807fffff) | exp; if(u.l == 0x80000000)return(0.0f); // swapNBytes((char *)&u, 4); return( u.f ); } static void double_decb8(char *p, double in) { unsigned long exp; union u_tag { double d; unsigned long l[2]; char c[8]; } u; int i; if( fabs(in) < 1.0e-36){ u.d = 0.0; } else { if( in > 1.0e36) u.d = 1.0e36; else if( in < -1.0e36) u.d = 1.0e36; else u.d = in; exp = (u.l[0] & 0x7ff00000) + 0x00200000; u.l[0] = (u.l[0] & 0x800fffff) | exp; } for(i=0; i<8; i++){ *p++ = u.c[i]; } } static void float_decb4(char *p, float in) { unsigned long exp; union u_tag { float f; unsigned long l; } u; if( fabs(in) < 1.0e-36f){ u.f = 0.0f; } else { if( in > 1.0e36f) u.f = 1.0e36f; else if( in < -1.0e36f) u.f = 1.0e36f; else u.f = in; exp = (u.l & 0x7f800000) + 0x01000000; u.l = (u.l & 0x807fffff) | exp; } *p++ = ( u.l >> 16); *p++ = ( u.l >> 24); *p++ = ( u.l ); *p++ = ( u.l >> 8); } static void word_b2(char *p, unsigned short in) { *p++ = ( in & 0xff); *p++ = ( (in & 0xff00) >> 8); } // // The LoggableGps multiple-inheritance business requires that we include // the following GPS functions here // long Kearfott::sampleTime() { return _fix.sampleTime.seconds; } short Kearfott::hours() { return _fix.ggatime.hours; } short Kearfott::minutes() { return _fix.ggatime.minutes; } short Kearfott::seconds() { return _fix.ggatime.seconds; } short Kearfott::centiSeconds() { return _fix.ggatime.centiSeconds; } double Kearfott::latitude() { return _fix.latitude; } char Kearfott::nsHemisphere() { return GpsUtils::hemisphere(_fix.nsHemisphere); } double Kearfott::longitude() { return _fix.longitude; } char Kearfott::ewHemisphere() { return GpsUtils::hemisphere(_fix.ewHemisphere); } GpsIF::Quality Kearfott::quality() { return _fix.quality; } short Kearfott::nSatellites() { return _fix.nSatellites; } double Kearfott::hdop() { return _fix.hdop; } double Kearfott::altitude() { return _fix.altitude; } double Kearfott::geoidHeight() { return _fix.geoidHeight; } short Kearfott::dgpsDataAge() { return _fix.dgpsDataAge; } short Kearfott::dgpsStationId() { return _fix.dgpsStationId; } 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, ins and gps servers. // _kearfottOutput = new KearfottOutput(); _dvlOutput = new DvlOutput(); //_gpsOutput = new GpsOutput(); //set up dvl and INS logging _kearfottLog = new KearfottLog( this, DataLog::BinaryFormat ); _dvlLog = new DvlLog(this, DataLog::BinaryFormat); //_gpsLog = new KearfottGpsLog(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; nRawFileBytes = 0; 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, 0666); // _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(); //_gpsOutput = new GpsOutput(); //set up dvl and INS logging _kearfottLog = new KearfottLog( this, DataLog::BinaryFormat ); _dvlLog = new DvlLog(this, DataLog::BinaryFormat); //_gpsLog = new KearfottGpsLog(this, DataLog::BinaryFormat); // Initialize the device itself initialize(); // ln250_gps_port = open("//12/dev/ser4", O_WRONLY); // ln250_tmark_port = open("//12/dev/ser5", O_WRONLY); // printf("ln250 port is %d\n", ln250_gps_port); } Kearfott::~Kearfott() { if (_device) delete _device; delete _kearfottOutput; delete _dvlOutput; //delete _gpsOutput; delete _kearfottLog; delete _dvlLog; if (_rawLog != -1) close(_rawLog); //delete _gpsLog; if (_depthIF) delete _depthIF; if (_ctdIF) delete _ctdIF; if (_acommsIF) delete _acommsIF; } DeviceIF::Status Kearfott::initialize() { KEARFOTT_OP_TYPE opData; KEARFOTT_CONFIG_TYPE config; //set up serial port _device->setLineFormat( &KearfottLineFormat ); _device->raw(255,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; memset(&_lasttime, 0, sizeof(Kearfott::_lasttime)); //try to set up interfaces to GPS, depth sensor, and CTD if (_externalSensors) { try { _acommsIF = new AcousticModemIF("kearfott", "BenthosModemServer", 20); printf("acoustic modem if is 0x%x\n", _acommsIF); } catch(...) { Syslog::write("%s: unable to open modem interface\n", name()); _acommsIF = NULL; } try { _depthIF = new DepthSensorIF("DepthSensor", 30); Syslog::write("Kearfott:: depthif is 0x%x\n", _depthIF); } catch(...) { Syslog::write("%s: unable to open depth interface\n", name()); _depthIF = NULL; } try { _ctdIF = new FastCatIF("fastcat", "FastCat", 5); Syslog::write("Kearfott:: ctdIF is 0x%x\n", _ctdIF); } catch(...) { Syslog::write("%s: unable to open ctd interface\n", name()); _ctdIF = NULL; } } // _reson7K = new Reson7K("134.89.32.107"); // if (_reson7K->_mbDataSocket->ready()) { // Syslog::write("Reson7K Center Socket connection ready!"); // } 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; double dval; 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*0.00000008381903171539306640625); _scaledNavData.lon = Math::degToRad(_rawNavData.lon*0.00000008381903171539306640625); _scaledNavData.depth = (double)_rawNavData.depth/(double)100.0; _scaledNavData.bheight = (double)_rawNavData.bheight/(double)100.0; _scaledNavData.roll = Math::degToRad(_rawNavData.roll*0.0055); //comment in for kearfott z down _scaledNavData.pitch = Math::degToRad(_rawNavData.pitch*0.0055); _scaledNavData.heading = Math::degToRad(_rawNavData.heading*0.0055); _scaledNavData.vbodyx = _rawNavData.vbodyx/(double)1024.0; _scaledNavData.vbodyy = _rawNavData.vbodyy/(double)1024.0; _scaledNavData.vbodyz = _rawNavData.vbodyz/(double)1024.0; _scaledNavData.accelx = _rawNavData.accelx/(double)1024.0; _scaledNavData.accely = _rawNavData.accely/(double)1024.0; _scaledNavData.accelz = _rawNavData.accelz/(double)1024.0; _scaledNavData.prate = _rawNavData.prate/(double)8192.0; _scaledNavData.qrate = -_rawNavData.qrate/(double)8192.0; _scaledNavData.rrate = -_rawNavData.rrate/(double)8192.0; _scaledNavData.utcTime = _rawNavData.utcTime/(double)16384.0; //fill in the time when the data came in struct timespec currentTime; double deltaTime; clock_gettime(CLOCK_REALTIME, ¤tTime); deltaTime = ((double)(currentTime.tv_sec+currentTime.tv_nsec/1.0e9))- ((double)(_scaledNavData.sampleTime.seconds+_scaledNavData.sampleTime.nanoSeconds/1.0e9)); if (deltaTime > .12) Syslog::write("Kearfott::deltaTime is %lf\n", deltaTime); _scaledNavData.sampleTime.seconds = currentTime.tv_sec; _scaledNavData.sampleTime.nanoSeconds = currentTime.tv_nsec; 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", rawNav->lat*8.3819e-8, 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, rawNav->vbodyx/1024.0, rawNav->vbodyy/1024.0, 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, Math::radToDeg(scaledNav->prate), Math::radToDeg(scaledNav->qrate), Math::radToDeg(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; memcpy(&_dopplerPD4.datstr, ptr, 1); ptr += 1; memcpy(&_dopplerPD4.numbyte, ptr, 2); ptr += 2; memcpy(&_dopplerPD4.sysconfig, ptr, 1); ptr += 1; 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; 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.dataStatus = _dopplerPD4.botstat | _dopplerPD4.refstat; _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.beam1 = _dopplerPD4.bm1/cos30/100.0; _dvlOutput->data.beam2 = _dopplerPD4.bm2/cos30/100.0; _dvlOutput->data.beam3 = _dopplerPD4.bm3/cos30/100.0; _dvlOutput->data.beam4 = _dopplerPD4.bm4/cos30/100.0; _dvlOutput->data.toping = _dopplerPD4.toping_hr*3600 + _dopplerPD4.toping_min*60 + _dopplerPD4.toping_sec + _dopplerPD4.toping_hun/100.0; _dvlOutput->data.bottomStatus = _dopplerPD4.botstat; _dvlOutput->data.waterStatus = _dopplerPD4.refstat; _dvlOutput->data.range = _scaledNavData.bheight; _dvlOutput->data.temp = _dopplerPD4.temp/100.; // _dvlOutput->data.bottomDetectStatus = // (_dvlOutput->data.range < 300.0)?OK:ERROR; if( _dopplerPD4.bm1 + _dopplerPD4.bm2 + _dopplerPD4.bm3 + _dopplerPD4.bm4 ) _dvlOutput->data.bottomDetectStatus = True; else _dvlOutput->data.bottomDetectStatus = False; _dvlOutput->write(); _dvlLog->write(); return DeviceIF::Ok; } void Kearfott::printDoppler(RDI_PD4_TYPE *pd4) { printf("xvel:%d yvel:%d zvel:%d evel:%d\n", pd4->xvelbtm, pd4->yvelbtm, pd4->zvelbtm, pd4->evelbtm); printf("bm1:%d bm2:%d bm3:%d bm4:%d\n", pd4->bm1, pd4->bm2, pd4->bm3, pd4->bm4); printf("bottom status is 0x%x\n", pd4->botstat); printf("DOPPLER BIT results: 0x%x\n", pd4->bit); printf("DOPPLER temp: %f\n\n", pd4->temp/100.0); } DeviceIF::Status Kearfott::parseGPS_PVTA(KEARFOTT_MSG_TYPE *msg) { /* float lat, lon, msl_alt, abs_alt, ve, vn, vu; char fval[4]; int i; for (i = 0; i < 61; i++) { memcpy(&fval, &(msg->data[i]), 4); //swapNBytes((char *)&fval, 4); lat = b4dec_float((char *)&fval); printf("lat[%d] is %f\n", i, Math::radToDeg(lat)); } swapNBytes((char *)&(msg->data[18]), 4); lat = b4dec_float((char *)&(msg->data[18])); swapNBytes((char *)&(msg->data[22]), 4); lon = b4dec_float((char *)&(msg->data[22])); swapNBytes((char *)&(msg->data[38]), 4); msl_alt = b4dec_float((char *)&(msg->data[38])); swapNBytes((char *)&(msg->data[42]), 4); abs_alt = b4dec_float((char *)&(msg->data[42])); printf("latitude is %f, lon is %f\n", Math::radToDeg(lat), Math::radToDeg(lon)); printf("msl altitude is %f abs alt is %f\n", msl_alt, abs_alt); swapNBytes((char *)&(msg->data[46]), 4); ve = b4dec_float((char *)&(msg->data[46])); swapNBytes((char *)&(msg->data[50]), 4); vn = b4dec_float((char *)&(msg->data[50])); swapNBytes((char *)&(msg->data[54]), 4); vu = b4dec_float((char *)&(msg->data[54])); printf("ve %f vn %f vu %f\n", ve, vn, vu); */ /* unsigned short csum; short sval; unsigned long lval; unsigned char *data; double dval; float fval; tcsendbreak(ln250_tmark_port,10); pvtBuf[0] = 0x81ff; pvtBuf[1] = 0x0003; //swapNBytes((char *)&pvtBuf[1], 2); pvtBuf[2] = 61; //swapNBytes((char *)&pvtBuf[2], 2); pvtBuf[3] = 0x0000; csum = pvtBuf[0]+pvtBuf[1]+pvtBuf[2]+pvtBuf[3]; csum = ((unsigned long)65536 - csum)&0xffff; pvtBuf[4] = csum; //swapNBytes((char *)&pvtBuf[4], 2); //swap bytes printf("gps time is %lf\n", b8dec_double((char *)&(msg->data[2]))); printf("utc time is %lf\n", b8dec_double((char *)&(msg->data[10]))); printf("lat is %f\n", b4dec_float((char *)&(msg->data[22]))); printf("lon is %f\n", b4dec_float((char *)&(msg->data[24]))); memcpy(pvtBuf+5, msg->data+2, 60); */ return DeviceIF::Ok; } DeviceIF::Status Kearfott::parseGPS_PVTB(KEARFOTT_MSG_TYPE *msg) { unsigned short usval; unsigned short sval; int i; /* unsigned char nullBuffer[32]; memcpy(pvtBuf+35, msg->data, 62); usval = 0; for (i=0; i<61; i++) { usval += pvtBuf[i+5]; } usval = ((unsigned long)65536 - usval)&0xffff; pvtBuf[66] = usval; //now, swap bytes swapNBytes((char *)&pvtBuf[5], 8); swapNBytes((char *)&pvtBuf[9], 8); swapNBytes((char *)&pvtBuf[13],2); swapNBytes((char *)&pvtBuf[14], 2); swapNBytes((char *)&pvtBuf[15], 4); swapNBytes((char *)&pvtBuf[17], 4); swapNBytes((char *)&pvtBuf[19], 4); swapNBytes((char *)&pvtBuf[21], 4); swapNBytes((char *)&pvtBuf[23], 4); swapNBytes((char *)&pvtBuf[25], 4); swapNBytes((char *)&pvtBuf[27], 4); swapNBytes((char *)&pvtBuf[29], 4); swapNBytes((char *)&pvtBuf[31], 4); swapNBytes((char *)&pvtBuf[33], 4); swapNBytes((char *)&pvtBuf[35], 4); swapNBytes((char *)&pvtBuf[37], 4); swapNBytes((char *)&pvtBuf[39], 4); swapNBytes((char *)&pvtBuf[45], 4); swapNBytes((char *)&pvtBuf[47], 4); swapNBytes((char *)&pvtBuf[49], 2); swapNBytes((char *)&pvtBuf[50], 2); swapNBytes((char *)&pvtBuf[51], 2); swapNBytes((char *)&pvtBuf[52], 2); swapNBytes((char *)&pvtBuf[53], 2); swapNBytes((char *)&pvtBuf[54], 2); swapNBytes((char *)&pvtBuf[55], 2); swapNBytes((char *)&pvtBuf[56], 2); swapNBytes((char *)&pvtBuf[57], 2); swapNBytes((char *)&pvtBuf[58], 2); swapNBytes((char *)&pvtBuf[59], 2); swapNBytes((char *)&pvtBuf[60], 2); swapNBytes((char *)&pvtBuf[61], 2); swapNBytes((char *)&pvtBuf[62], 2); swapNBytes((char *)&pvtBuf[63], 2); swapNBytes((char *)&pvtBuf[64], 4); // swapNBytes((char *)&pvtBuf[66], 2); //send to ln250 memset(nullBuffer, 0, 32); write(ln250_gps_port, pvtBuf, 67*2); write(ln250_gps_port, nullBuffer, 32); fdatasync(ln250_gps_port); */ // 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) { Boolean debug = False; dprintf("Kearfott::parseGPS_GGA() %s", msg->data ); if (_displayNavMessages) { Syslog::write("Kearfott::parseGPS_GGA():%s", msg->data); } try { GpsUtils::parseFix( (const char *) msg->data, &_fix); //Cast?? } catch (Exception e) { // // Data is not good: Make sure output buffer relects that. Added by // RHenthorn, 6.24.2003, to try to fix bug at interface between good and // bad quality fixes. // //_fix.quality = GpsIF::Invalid; //memcpy((void *)&_gpsOutput->data.fix, (void *)&_fix, sizeof(GpsIF::Fix)); //_gpsOutput->write(); //_gpsLog->write(); //Syslog::write("Kearfott::parseGPS_GGA() Error - %s", e.msg); return DeviceIF::Error; } // // GpsUtils additionally writes the GPS time into fix.ggatime. // timespec sampleTime; // // Fill in date and time (NMEA GGA standard apparently does NOT include // this!) // clock_gettime(CLOCK_REALTIME, &sampleTime); _fix.sampleTime.seconds = sampleTime.tv_sec; _fix.sampleTime.nanoSeconds = sampleTime.tv_nsec; // // Write to shared memory. // //memcpy((void *)&_gpsOutput->data.fix, (void *)&_fix, sizeof(GpsIF::Fix)); //_gpsOutput->write(); // // Update the Gps log here. Don't update it in the GSA or VGT parsing // functions because that would repeat data from the other NMEA strings. We // update the log here because GGA has the Gps time, and so there should be // minimal delay between the logged qnx time and the Gps time. This // approach assumes that the three strings always cycle in order. // //_gpsLog->write(); double latitude = Math::radToDeg(_fix.latitude); double longitude = Math::radToDeg(_fix.longitude); if (_fix.nSatellites >= 4 && _displayNavMessages) { Syslog::write("Kearfott::parseGPS_GGA - lat(%f) lon(%f)\n", latitude, longitude); } return DeviceIF::Ok; } DeviceIF::Status Kearfott::parseGPS_GSA(KEARFOTT_MSG_TYPE *msg) { Boolean debug = False; dprintf("Kearfott::parseGPS_GSA() %s", msg->data ); try { GpsUtils::parseGSA( (const char *) msg->data, &_gsa); //Cast?? } catch (Exception e) { // // Consider setting the fix mode to 1. // Syslog::write("Kearfott::parseGPS_GSA(): Error - %s", e.msg); return DeviceIF::Error; } // // Write to shared memory. // //memcpy((void *)&_gpsOutput->data.gsa, (void *)&_gsa, // sizeof(GpsIF::GsaStruct)); //_gpsOutput->write(); return DeviceIF::Ok; } DeviceIF::Status Kearfott::parseGPS_VTG(KEARFOTT_MSG_TYPE *msg) { Boolean debug = False; dprintf("Kearfott::parseGPS_VGT() %s", msg->data ); try { GpsUtils::parseVGT( (const char *) msg->data, &_vgt); //Cast?? } catch (Exception e) { // // Consider setting the fix mode to 1. // Syslog::write("Kearfott::parseGPS_VGT(): Error - %s", e.msg); return DeviceIF::Error; } // // Write to shared memory. // //memcpy((void *)&_gpsOutput->data.vgt, (void *)&_vgt, // sizeof(GpsIF::GsaStruct)); // _gpsOutput->write(); 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 buff[258]; buff[0]= 0xaa; buff[1]= msg->cmdID; buff[2]= msg->numBytes; memcpy(&(buff[3]), msg->data, msg->numBytes); buff[msg->numBytes+3] = msg->checksum; _device->write(buff, msg->numBytes + 4); return DeviceIF::Ok; } void Kearfott::logMsg(KEARFOTT_MSG_TYPE *msg) { char val; unsigned char buf[259]; int cnt=0; buf[cnt++] = 0xaa; buf[cnt++] = msg->cmdID; buf[cnt++] = msg->numBytes; if (msg->numBytes) { memcpy(buf+3, msg->data, msg->numBytes); cnt += msg->numBytes; } buf[cnt++] = msg->checksum; if (nRawFileBytes+cnt > 1024*16) { write(_rawLog, rawFileBuffer, nRawFileBytes); nRawFileBytes = 0; Syslog::write("writing raw log"); } memcpy(&rawFileBuffer[nRawFileBytes], buf, cnt); nRawFileBytes += cnt; // write(_rawLog, &buf, cnt); // fsync(_rawLog); } DeviceIF::Status Kearfott::parseMsg(KEARFOTT_MSG_TYPE *msg) { // // Add a terminating null in the data string to prevent strchr from running // past the end of the string in the parsing code in GpsUtils::parsexxx. // if( msg->numBytes < 0 || msg->numBytes > 255 ) { Syslog::write(" Kearfott::parseMsg: Error - " "Message %d has an illegal number of bytes %d\n", msg->cmdID, msg->numBytes); return DeviceIF::Error; } msg->data[msg->numBytes] = '\0'; // switch (msg->cmdID) { case accept_configuration: case accept_operational: case return_configuration: //printf("received echo of %d\n", msg->cmdID); //Note no "break" above return DeviceIF::Ok; case navigation_data: // printf("received navigation data\n"); parseNavigationMessage(msg); //yield for 50ms. Short enough that we're around for the next nav message return DeviceIF::Ok; 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: if (_displayNavMessages) 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); printf("%s\n", msg->data); return DeviceIF::Error; } } DeviceIF::Status Kearfott::readMsg(KEARFOTT_MSG_TYPE *msg) { unsigned char buffer[256]; //most we should have to read until we see another packet Boolean readError = False; if (_device) { try { unsigned char header[3]; int nbytes; do { nbytes = ::dev_read(_device->getFd(), header, 1, 1, 0, 10, 0, 0); if (nbytes < 0) return DeviceIF::Error; } while (header[0] != 0xAA); nbytes = ::dev_read(_device->getFd(), &(header[1]), 2, 2, 0, 10, 0, 0); if (nbytes != 2) return DeviceIF::Error; msg->cmdID = header[1]; msg->numBytes = header[2]; nbytes = ::dev_read(_device->getFd(), buffer, msg->numBytes+1, msg->numBytes+1, 0, 10, 0, 0); /* 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+1) { printf("only read %d bytes, expected %d bytes\n", nbytes, msg->numBytes+1); perror("readNChars"); readError = True; } memcpy(msg->data, buffer, msg->numBytes); //copy out data msg->checksum = buffer[msg->numBytes]; } 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 (readError) return DeviceIF::Error; if (msg->cmdID == 1 && msg->numBytes != 46/*52*/) { Syslog::write("nav message wrong length, expected 52, got %d\n", msg->numBytes); 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)) { printScaledNavType(&_scaledNavData); // printRawNavType(&_rawNavData); printDoppler(&_dopplerPD4); } } } else { // exit(0); } /* if ((time(NULL) - lastCurrentDataTime) > 2) { lastCurrentDataTime = time(NULL); if (_opValid) sendOpData(&_opData); } */ if ((time(NULL) - lastTime) > 1) { lastTime = time(NULL); if (_externalSensors) { updateExternalSensors(); } else if (_opValid) { sendOpData(&_opData); } } /* if (_externalSensors) { if ((time(NULL) - lastTime) > 1) { lastTime = time(NULL); //Syslog::write("%s: updating with external sensors\n", name()); updateExternalSensors(); } } */ // delay(10); } } void Kearfott::updateExternalSensors() { AcousticModemIF::SMAC_SAN_RECORD fix; double currentDepth; TimeIF::TimeSpec sampleTime; DeviceIF::Status modemStatus, depthStatus; KEARFOTT_OP_TYPE opdata; Boolean newFix; if (_displayNavMessages) Syslog::write("updating external sensors\n"); // set initial values opdata.validity = 0; opdata.idepth = 0; opdata.modes = GPSENAB|DOPPING|DOPENAB; opdata.forcomm = 0; //bottom track only mode //update from USBL tracking system (looks like a GPS interface-wise if (_acommsIF) { modemStatus = _acommsIF->getSANFix(&fix, &newFix); if ((modemStatus == DeviceIF::Ok) && newFix) { Syslog::write("updating INS with SAN data\n"); Syslog::write("updating position with lat %f lon %f\n", fix.wfLat,fix.wfLong); opdata.init_lat = (int)(fix.wfLat/(8.381903171539307e-008)); opdata.init_lon = (int)(fix.wfLong/(8.381903171539307e-008)); opdata.cepos = 20000; //200m opdata.validity |= VPOS; opdata.forcomm = 0; } } if (_depthIF) { depthStatus = _depthIF->depth(¤tDepth, &sampleTime); if (depthStatus == DeviceIF::Ok) { int ival; ival = (int)(currentDepth * 100); opdata.idepth = ival; opdata.validity |= VDPTH|VERTDAMP_PRESS; if (currentDepth > 10.0) opdata.modes |= DOPENAB; } } if (_ctdIF) { TimeIF::TimeSpec sampleTime; float sal; _ctdIF->salinity(&sal, &sampleTime); opdata.salinity = (int)(sal * 100.0); opdata.validity |= VSALIN; } else { opdata.salinity = 3422; opdata.validity |= VSALIN; } sendOpData(&opdata); if (_rawNavData.mode == 0x0a) { Syslog::write("%s: raw nav mode is BIT Fail, exiting", name()); exit(1); } if (_rawNavData.mode != 0x06 && _rawNavData.mode != 0x09) { Syslog::write("Kearfott - exiting because of bad mode\n"); exit(1); } }