/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : TerrainAid.cc */ /* Author : Rob McEwen */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : Added measurements for IDT data */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include "TerrainAidDriver.h" #include "PeriodicTask.h" #include "SerialDevice.h" #include "WorkSiteIF.h" #include "VehicleConfigurationIF.h" #include "NavUtils.h" #include "TimeP.h" #include "Syslog.h" #include "netcdf.h" #include "TerrainNavClient.h" #include #include #include #include #include #include "DeltaTIF.h" #include "MbTrnRecvIF.h" #include "MbTrnRecv.h" #include "matrixMath.h" #define READ_TIMEOUT 2800 //Milliseconds #define MAXRECORDBYTES 512 #define ERRCODE 2 #define ERR(e) {printf("Error: %s\n", nc_strerror(e)); exit(ERRCODE);} ////////////////////////////////////////////////////////////////////////////// // This is the Terrain Aid Task class that is triggered periodically by the // system. It simply forwards calls to the Terrain Aid object that does // all the work. // TerrainAidTask::TerrainAidTask( Boolean test, Boolean sim, char *config ) : PeriodicTask("terrainAidDriver") { Boolean debug = True; _ta_object = new TerrainAid(test, sim, config); addPeriodicCallback(_ta_object->samplePeriod(), (CallbackMethod)TerrainAidTask::execute); dprintf("TerrainAidTask::TerrainAidTask - ctor complete"); } TerrainAidTask::~TerrainAidTask() { if (_ta_object) delete _ta_object; } void TerrainAidTask::initialize() { _ta_object->initialize(); } // This is the method setup in the constructor as the periodic callback. // It simply calls the method in TarrainAid that used to be the callback. // void TerrainAidTask::execute() { // Used to be TerrainAid::readData() // _ta_object->execute(); } ////////////////////////////////////////////////////////////////////////////// // This is the Terrain Aid class that is called periodically, usually by the // Terrain Aid Task. It does all the actual work of getting the position // offset estimate from TRN. Now that it has been separated from PeriodicTask, // it can be used in other contexts. It is exactly the same as before, except // that it is not a PeriodicTask. // TerrainAid::TerrainAid( Boolean test, Boolean sim, char *config ) : _config(config), _useIDTData(False), _useMbTrnData(False), _useDvlSide(False), _attributes(config), NotSpecified(-1), _test(test), _sim(sim) { Boolean debug = True; _msgQ = new TerrainAidMsg(); _log = new TerrainAidLog(this, DataLog::BinaryFormat); _output= new TerrainAidOutput(); _output->data.trnConnected = False; _output->data.valid = False; // // Zero interface pointers in case one or more doesn't open. _nav = NULL; _parosci = NULL; _dvl = NULL; _dvlSide = NULL; _mbtrn = NULL; _idt = NULL; // Load the configuration file // char configFile[128]; strcpy(configFile, System::configurationFile(_config)); configFile[127]='\0'; dprintf("Configuration file = %s\n", configFile); createCfgAttributes(); loadConfigFile(configFile); System::copyToLogDir(configFile); printCfgAttributes(); if( !_test ) { try { _nav = new NavigationIF(NavigationIFServerName); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to NavigationIF\n"); } try { _parosci = new DepthSensorIF(DepthSensorIFServerName); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to DepthSensorIF\n"); } if (_useIDTData) { try { _idt = new DeltaTIF("Delta_TServer"); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to Delta_TServerIF\n"); } } else if (_useMbTrnData) { // MB-sys data // try { _mbtrn = new MbTrnRecvIF(MbTrnRecvIFServerName); } catch(Exception e) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to MbTrnRecvIF: %s\n", e.msg); } } if( !_useDvlSide ) { try { _dvl = new DvlIF(DvlIFServerName); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to DvlIF\n"); } } else { try { _dvlSide = new DvlSideIF(DvlSideIFServerName); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to DvlSideIF\n"); } } } // Pull the plug now unless all required interfaces are open // if ( !_nav || !_parosci || (!_useDvlSide && !_dvl ) || ( _useDvlSide && !_dvlSide) || ( _useIDTData && !_idt ) || ( _useMbTrnData && !_mbtrn ) ) { Syslog::write("TerrainAid - required interfaces not opened - Fail!"); exit(0); } _useIns = false; if( !_test ) { dprintf("Vehicle Configuration now\n"); VehicleConfigurationIF _vehicleConfig("vehicleConfig"); // if(_vehicleConfig.useIns() == 1) _useIns = true; _useIns = true; } char tempLogDir[256]; char tempConfig[256]; char tempMapFile[256]; strcpy(tempConfig, _vehicleCfgName ); strcpy(tempMapFile, _mapFileName); strcpy(_vehicleCfg, System::configurationFile(_vehicleCfgName)); dprintf("Vehicle configuration file = %s\n", _vehicleCfg); strcpy(_mapFile, System::configurationFile(_mapFileName)); dprintf( "Map file = %s\n", _mapFile ); if( _phiBias != NotSpecified ) Syslog::write("TerrainAidDriver:: RollOffset is %.2f", _phiBias); #if 0 double initConditions[2]; WorkSiteIF workSite("workSite"); NavUtils::geoToUtm(workSite.latitude(), workSite.longitude(), workSite.utmZone(), &initConditions[0], &initConditions[1]); Syslog::write("TerrainAidDriver:: Initial Conditions are %.2f %.2f", initConditions[0], initConditions[1]); //double initConditions[2] = {4072000.,598000.}; _tercom = new TerrainNav(_mapFile, initConditions); #endif WorkSiteIF workSite("workSite"); _utmZone = workSite.utmZone(); Syslog::write("TerrainAidDriver::TerrainAidDriver() - Utm zone = %d", _utmZone); //Initialize TerrainNav object with Particle Filter // Use TerrainNav or TerrainNavClient depending on the // terrainNavServer attribute. RGH Feb-June 2013 // try { if (0 == strcmp(_terrainNavServer, "NotSpecified")) { // Use TRN_MAPFILES environment variable for location of local // map files. // char *filename = strdup(_mapFile); char *mapPath = (mapPath = getenv("TRN_MAPFILES"))? mapPath : ""; sprintf(_mapFile, "%s/%s", mapPath, filename); _tercom = new TerrainNav(_mapFile, _vehicleCfg, 2); _output->data.trnConnected = True; free(filename); } else { // Server will use TRN_MAPFILES environment variable on the server // for location of map files, so just use the filename here. // Syslog::write("TerrainAidDriver: Using TerrainNavClient at %s on port %d and map %s", _terrainNavServer, _terrainNavPort, tempMapFile); //Syslog::write("TerrainAidDriver: Files %s, %s", // _mapFileName, _vehicleCfgName); if (latestLogDirName(tempLogDir, sizeof(tempLogDir)-6) == 0) { sprintf(tempLogDir, "latest/"); Syslog::write("TerrainAidDriver - failed to dereference logs/latest"); } else { Syslog::write("TerrainAidDriver - logs/latest is %s", tempLogDir); } _tercom = new TerrainNavClient(_terrainNavServer, _terrainNavPort, tempMapFile, tempConfig, _particlesName, tempLogDir, 2, _map_type); if (_tercom->is_connected()) { Syslog::write("TerrainAidDriver::Connected to server."); _output->data.trnConnected = True; } else { Syslog::write("TerrainAidDriver::Failure to connect to server!"); _output->data.trnConnected = False; } } } catch(Exception e) { Syslog::write("TerrainAid::Caught exception creating TerrainNav"); Syslog::write("TerrainAid::Exception: %s", e.msg); exit(0); } try { _tercom->setInterpMeasAttitude(true); //_tercom->setVehicleDriftRate(1.5); //choose filter settings based on whether kearfott is available and if //filter forcing is set if(!_useIns || _forceLowGradeFilter) _tercom->useLowGradeFilter(); else _tercom->useHighGradeFilter(); //turn on filter reintialization if set in terrainAid.cfg _tercom->setFilterReinit(_allowFilterReinits); //turn on modified weighting if set in terrainAid.cfg _tercom->setModifiedWeighting(_useModifiedWeighting); } catch(Exception e) { Syslog::write("TerrainAid::Caught exception in TerrainNav"); Syslog::write("TerrainAid::Exception: %s", e.msg); _output->data.trnConnected = False; } _navData = new poseT; _mleEst = new poseT; _mmseEst = new poseT; // For DVL data _dvlData = new measT; _dvlData->numMeas = 4; _dvlData->ranges = new double[_dvlData->numMeas]; _dvlData->alphas = new double[_dvlData->numMeas]; _dvlData->dataType = 1; //1 denotes DVL data _dvlData->measStatus = new bool[_dvlData->numMeas]; // For IDT DeltaT data _idtData = new measT; _idtData->numMeas = 120; _idtData->ranges = new double[_idtData->numMeas]; _idtData->alphas = new double[_idtData->numMeas]; _idtData->dataType = 5; //5 denotes IDT data _idtData->measStatus = new bool[_idtData->numMeas]; // For MB-sys data // The number of beams from MBTRN will vary, so // allocate enough memory in our resusable measT // object for the maximum number of beams. // _mbTrnData = new measT; _mbTrnData->dataType = TRN_SENSOR_MB; //2 denotes MB data _mbTrnData->crossTrack = new double[MAX_FILTERED_VALUES]; _mbTrnData->covariance = new double[MAX_FILTERED_VALUES]; _mbTrnData->ranges = new double[MAX_FILTERED_VALUES]; _mbTrnData->alphas = new double[MAX_FILTERED_VALUES]; _mbTrnData->alongTrack = new double[MAX_FILTERED_VALUES]; _mbTrnData->altitudes = new double[MAX_FILTERED_VALUES]; _mbTrnData->beamNums = new int[MAX_FILTERED_VALUES]; _mbTrnData->measStatus = new bool[MAX_FILTERED_VALUES]; _filterState = 1; _reinitFilter = 0; _numReinits = 0; // This callback setup has moved to TerrainAidTask class // //addPeriodicCallback(_samplePeriod, (CallbackMethod)TerrainAid::execute); _cntr = 0; _first = True; dprintf("TerrainAidDriver::TerrainAidDriver - ctor complete"); // Initialize TRN status // _output->data.deltaNorthing = 0.; _output->data.deltaEasting = 0.; _output->write(); } TerrainAid::~TerrainAid() { Syslog::write(" TerrainAidDestructor Executing."); delete _msgQ; delete _log; delete _output; if(_nav) delete _nav; if(_parosci) delete _parosci; delete _dvlData->ranges; delete _dvlData->measStatus; delete _dvlData; delete _idtData->ranges; delete _idtData->measStatus; delete _idtData; delete _navData; delete _mleEst; delete _mmseEst; delete _tercom; _mbTrnData->clean(); delete _mbTrnData; } void TerrainAid::initialize() { } // 04/24/2015 // // Decoupling getting estimate in readData() and publishing of estimate // in publishData() for serveral reasons. Mainly, the desire to precisely // control when the new estimate is available to match sim to // real-time missions. // // This function is not called during simulation. Rather, the Simualtor // calls readData() and publishData() with the simulated duration handled // there. // void TerrainAid::execute() { // Process messages in queue one at a time // _msg._msg = TerrainAidMsg::None; // A value other than None will trigger if (_msgQ->read(&_msg) > 0) { Syslog::write("TerrainAidDriver:: _msg->type = %d", _msg._msg); } // If we need to ensure that a time D goes by before publishing new data // get the current time here. T_start = Time.now // readData(); // Latency L may be a second or more // Calculate the latency. L = Time.now - T_start // Wait for the remainder of time D. W = D - L // // Rock would like W to approach zero. // if (W > 0) // sleep(W) // else // report("TRN latency exceeded D!") // publishData(); } void TerrainAid::readData() { int i, j; Boolean debug = False; TimeIF::TimeSpec depthTime; DeviceIF::Status parosciStatus = DeviceIF::Error; DeviceIF::Status dvlStatus = DeviceIF::Error; double zBias = 0.; _output->read(); // // Read in navigation data and sensor values: // if(!_test) _nav->state( &_position, &_attitude ); if(!_test) { if( !_useDvlSide ) { dvlStatus = _dvl->get( &_dvlIFData, &_notUsed); } else { dvlStatus = _dvlSide->get( &_dvlSideIFData, &_notUsed); } } if( _useDvlSide ) { memcpy( &_dvlIFData, &_dvlSideIFData, sizeof(DvlIF::Data) ); } if( dvlStatus == DeviceIF::Ok ) _dvlValid = True; else _dvlValid = False; if(!_test) parosciStatus = _parosci->depth( &_depth, &depthTime ); // // The parosci depth sensor may have a bias. Assume that this code // initializes when the vehicle is on the surface, and subtract the bias. // if(_first) { zBias = 0.0; //_position.z; _first = False; _navTime0 = ((double) _position.updateTime.seconds) + ((double) _position.updateTime.nanoSeconds)/1.E9; _dvlTime0 = _dvlIFData.pingTime; Syslog::write("TerrainAidDriver:: _navTime0 = %.2f", _navTime0); Syslog::write("TerrainAidDriver:: _dvlTime0 = %.2f", _dvlTime0); } // // Moved the Trn correction into Navigation, 20 Mar 2017. This is // upstream, so here we need to feed the uncorrected INS to Trn. // _navData->x = _position.x + _output->data.deltaNorthing; // _navData->y = _position.y + _output->data.deltaEasting;; // _navData->z = _position.z - zBias; _navData->x = _position.xNoFix + _output->data.deltaNorthing; _navData->y = _position.yNoFix + _output->data.deltaEasting; _navData->z = _position.z - zBias; //Set GPS/DVL valid flags _navData->dvlValid = _dvlValid; _navData->gpsValid = _position.gpsValid; //Set velocity information to DVL measured velocity if(_dvlIFData.bottomStatus == 0) { _navData->vx = _dvlIFData.bottomTrackVelocity[0]; _navData->vy = _dvlIFData.bottomTrackVelocity[1]; _navData->vz = _dvlIFData.bottomTrackVelocity[2]; _navData->ve = _dvlIFData.bottomTrackVelocity[3]; _navData->bottomLock = true; } else if(_dvlIFData.waterStatus == 0) { _navData->vx = _dvlIFData.waterMassVelocity[0]; _navData->vy = _dvlIFData.waterMassVelocity[1]; _navData->vz = _dvlIFData.waterMassVelocity[2]; _navData->ve = _dvlIFData.waterMassVelocity[3]; _navData->bottomLock = false; } else { _navData->bottomLock = false; _navData->dvlValid = false; } //IF using Kearfott, convert vehicle velocity from RDI to SNAME frame if(_useIns) { _navData->vy = -_navData->vy; _navData->vz = -_navData->vz; } _navData->phi = _attitude.roll; if( _phiBias != NotSpecified ) _navData->phi -= _phiBias; _navData->theta = _attitude.pitch; _navData->psi = _attitude.yaw; // // The vbody components below can be coordinatized in EITHER the // body frame or the NED frame. This is set by an EEPROM parameter // in the Kearfott. I believe it is set to NED now. _navData->vn_x = _position.xRate; _navData->vn_y = _position.yRate; _navData->vn_z = _position.zRate; _navData->wx = _attitude.omega_B_x; _navData->wy = _attitude.omega_B_y; _navData->wz = _attitude.omega_B_z; // // In seconds since this process started. _position.updateTime is Unix time. _navData->time = (double) _position.updateTime.seconds + ((double) _position.updateTime.nanoSeconds)/1.E9 - _navTime0; //printf("_navData seconds = %d\n", _position.updateTime.seconds); // Use DVL or IDT or MB-sys for measure updates? // Test value from config file. // if (_useIDTData) { // We're using IDT // _measData = _idtData; //Reminder: this is a pointer. // // Get latest IDT data with timestamp and copy into _idtData object. // _idt->get(DeltaTIF::Side, &_idtIFData); _idtData->time = (double)_idtIFData.update_time.seconds + ((double)_idtIFData.update_time.nanoSeconds)/1.E9; _idtData->time = _navData->time; if( _useDvlSide ) { _idtData->phi = 0.; _idtData->theta = 0.; _idtData->psi = 0.; } else { _idtData->phi = _dvlIFData.roll; _idtData->theta = _dvlIFData.pitch; _idtData->psi = _dvlIFData.heading; } for (int i = 0; i < _idtIFData.nbeams; i++) { _idtData->ranges[i] = _idtIFData.beam_ranges[i]; _idtData->measStatus[i] = true; } dprintf("TerrainAidDriver::readData() - IDT[45] = %.2f, IDT[75] = %.2f\n", _idtData->ranges[45], _idtData->ranges[75]); } else if (_useMbTrnData) { // We're using MB-sys // _measData = _mbTrnData; //Reminder: this is a pointer. // // Get latest MB-sys data with timestamp and copy into _mbTrnData object. // _mbtrn->get_data(&_mbTrnIFData); // Convert sounding to measT // _mbTrnData->time = _mbTrnIFData.ts; //_mbTrnData->time = _navData->time; _mbTrnData->dataType = 2; _mbTrnData->numMeas = _mbTrnIFData.nbeams; // // Initialize the position state. Below, position.x is the Northing, // and position.y is the Easting, and they're in UTM coordinates. // NavUtils::geoToUtm( _mbTrnIFData.lat, _mbTrnIFData.lon, _utmZone, &(_mbTrnData->x), &(_mbTrnData->y)); _mbTrnData->z = _mbTrnIFData.depth; // // Dave's along-track/cross-track/down frame: _mbTrnData->phi = 0.; _mbTrnData->theta = 0.; _mbTrnData->psi = _mbTrnIFData.hdg; for (int i = 0; i < _mbTrnIFData.nbeams; i++) { _mbTrnData->alongTrack[i] = _mbTrnIFData.beams[i].rhox; _mbTrnData->crossTrack[i] = _mbTrnIFData.beams[i].rhoy; _mbTrnData->altitudes[i] = _mbTrnIFData.beams[i].rhoz; _mbTrnData->beamNums[i] = _mbTrnIFData.beams[i].beam_num; double rho[3] = {_mbTrnIFData.beams[i].rhox, _mbTrnIFData.beams[i].rhoy, _mbTrnIFData.beams[i].rhoz}; double rhoNorm = Vnorm( rho ); _mbTrnData->ranges[i] = rhoNorm; if( rhoNorm > 1 ) { _mbTrnData->measStatus[i] = true; } else { _mbTrnData->measStatus[i] = false; } } //#if 0 // Use MB-sys data for _navData, too // _navData->x = _mbTrnData->x; _navData->y = _mbTrnData->y; _navData->z = _mbTrnData->z; _navData->phi = _mbTrnData->phi; _navData->theta = _mbTrnData->theta; _navData->psi = _mbTrnData->psi; _navData->time = _mbTrnData->time; Syslog::write("\t\t\TerrainAidDriver <- MBTRN NAV data (t x/y/z): %.2f %.2f %.2f %.2f", _navData->time, _navData->x, _navData->y, _navData->z); //#endif } else { // We're using DVL (the default) // _measData = _dvlData; // _dvlData->time is Seconds in the day in an unknown time zone. // _dvlData->time = _dvlIFData.pingTime - _dvlTime0; //double dvlScale = 1.1547; //Dvl range scale factor // 2010/11/23 rsm. Fixed it in dvl/Dvl.cc. double dvlScale = 1.; _dvlData->time = _navData->time; _dvlData->ranges[0] = _dvlIFData.beam1*dvlScale; _dvlData->ranges[1] = _dvlIFData.beam2*dvlScale; _dvlData->ranges[2] = _dvlIFData.beam3*dvlScale; _dvlData->ranges[3] = _dvlIFData.beam4*dvlScale; _dvlData->phi = _dvlIFData.roll; _dvlData->theta = _dvlIFData.pitch; _dvlData->psi = _dvlIFData.heading; //printf("_dvlData.pingTime= %.2f\n", _dvlIFData.pingTime); //Determine the dvl beam status using bottomStatus //set all status flags to true, we filter on the stanford side for(i = 0; i < 4; i++) { _dvlData->measStatus[i] = true; } if(_test) { //Syslog::write("TerrainAidDriver:: Running with _test = True. "); // // Soquel Canyon. _navData->x = 4074900.0; // _navData->y = 591100.0; _navData->y = 590700.0; // _navData->z = 100.0; _navData->z = 200.0; #if 0 _navData->vx = 0.; _navData->vy = 0.; _navData->vz = 0.; _navData->phi = 0.; _navData->theta = 0.; _navData->psi = 0.; _navData->p = 0.; _navData->q = 0.; _navData->r = 0.; _dvlData->phi = 0.; _dvlData->theta = 0.; _dvlData->psi = 0.; #endif _navData->vx = 1.0; _navData->vy = 0.0; _navData->vz = 0.0; _dvlData->ranges[0] = 50; _dvlData->ranges[1] = 55; _dvlData->ranges[2] = 60; _dvlData->ranges[3] = 65; _navData->dvlValid = 1; _navData->gpsValid = 0; _navData->bottomLock = 1; _dvlData->measStatus[0] = _dvlData->measStatus[1] = true; _dvlData->measStatus[2] = _dvlData->measStatus[3] = false; } } if (_tercom->is_connected()) { try { //Syslog::write("TerrainNav update at nav time=%.6f", _navData->time); //Syslog::write("TerrainNav update at ,eas time=%.6f", _measData->time); dprintf("_navData-> (x,y) = (%.2f, %.2f) \n", _navData->x, _navData->y); dprintf("_tercom->outstandingMeas() = %d\n", _tercom->outstandingMeas()); if(_navData->time <= _measData->time) { _tercom->motionUpdate(_navData); // MeasUpdate Argument: 1 => Dvl; 2 => Multibeam, 3=> Single beam _tercom->measUpdate( _measData, _measData->dataType); } else { _tercom->measUpdate( _measData, _measData->dataType); _tercom->motionUpdate(_navData); } //dprintf("TerrainAid::readData() - measUpdate alphas[0] = %f", _measData->alphas[0]); // // Estimate location. 1=> MLE; 2=> MMSE _tercom->estimatePose( _mleEst, 1); _tercom->estimatePose( _mmseEst, 2); if(_tercom->lastMeasSuccessful()) { //display tercom estimate biases dprintf("ARL Estimation Bias (Max. Likelihood): (t = %.2f)\n", _mleEst->time); dprintf("ARL North: %.4f, East: %.4f, Depth: %.4f\n", _mleEst->x - _navData->x, _mleEst->y - _navData->y, _mleEst->z - _navData->z); dprintf("ARL Estimation Bias (Mean): (t = %.2f)\n", _mmseEst->time); dprintf("ARL North: %.4f, East: %.4f, Depth: %.4f\n", _mmseEst->x - _navData->x, _mmseEst->y - _navData->y, _mmseEst->z - _navData->z); dprintf("ARL North Sigma: %.2f, East Sigma: %.2f, Depth Sigma: %.2f\n\n", sqrt(_mmseEst->covariance[0]), sqrt(_mmseEst->covariance[2]), sqrt(_mmseEst->covariance[5])); } //Syslog::write("Estimated North Bias at time = %.2f : %.2f", _mmseEst->time, _mmseEst->x-_navData->x); //Syslog::write("Estimated East Bias at time = %.2f : %.2f", _mmseEst->time, _mmseEst->y-_navData->y); //Syslog::write("Estimated Depth Bias at time = %.2f : %.2f", _mmseEst->time, _mmseEst->z-_navData->z); //Syslog::write("Estimated northing and easting = %.2f : %.2f", _mmseEst->x, _mmseEst->y); } catch(Exception e) { Syslog::write("TerrainAid::Caught exception in TerrainNav"); Syslog::write("TerrainAid::Exception: %s", e.msg); _output->data.trnConnected = False; } } // trnConnected else { _output->data.trnConnected = False; } _output->write(); if(_sim) { //foobarSyslog::write("TerrainAidDriver:: Running with _sim = True. "); // if( _navData->time > 120. ) // { // _mleEst->covariance[0] = 2.; // _mleEst->covariance[1] = 3.; // _mleEst->x = _navData->x + 15; // _mleEst->y = _navData->y + 19; // } } // Publishing of estimate results moved verbatim to publishData() below | // V } void TerrainAid::publishData() { // // Determine if the last TRN calculation is a valid estimate. //If so, close the loop. // // These must be removed from the "if" below for non-inertial iceberg work. // (_maxNorthingError != NotSpecified) && // (_maxEastingError != NotSpecified) && //(fabs(_mmseEst->x-_navData->x) <= _maxNorthingError) && //(fabs(_mmseEst->y-_navData->y) <= _maxEastingError) ) Boolean valid = False; _output->read(); if( _tercom->is_connected() && ( _mmseEst->time > 0. ) && (_maxNorthingCov != NotSpecified) && (_maxEastingCov != NotSpecified) && (_maxNorthingError != NotSpecified) && (_maxEastingError != NotSpecified) && (_mmseEst->covariance[0] <= _maxNorthingCov) && (_mmseEst->covariance[2] <= _maxEastingCov) && (fabs(_mmseEst->x-_navData->x) <= _maxNorthingError) && (fabs(_mmseEst->y-_navData->y) <= _maxEastingError) ) { // Syslog::write("TerrainAidDriver:: _mmseEst->x,y = (%.2f, %.2f)", // _mmseEst->x, _mmseEst->y); // Syslog::write("TerrainAidDriver:: _navData->x,y = (%.2f, %.2f)", // _navData->x, _navData->y); // Syslog::write("TerrainAidDriver:: mmseEst - navData = (%.2f, %.2f)", // _mmseEst->x-_navData->x, _mmseEst->y-_navData->y); // Syslog::write("TerrainAidDriver:: _max(N/E)Error = (%.2f, %.2f)", // _maxNorthingError, _maxEastingError); valid = True; // // Syslog::write("TerrainAidDriver:: valid = %d.\n",valid); // // Additional, but optional, checks: if( _maxPsiBergCov != NotSpecified ) { if( _mmseEst->covariance[44] <= _maxPsiBergCov ) valid = True; else valid = False; } if( _maxDepthCov != NotSpecified ) { if( _mmseEst->covariance[5] <= _maxDepthCov ) valid = True; else valid = False; } } // Do we want to skip logging and publishing unless valid? // //if (!valid) // return; // Pass out the mle estimate and variance: // _output->data.x = _mmseEst->x; _output->data.y = _mmseEst->y; _output->data.z = _mmseEst->z; // _output->data.covariance[0] = _mmseEst->covariance[0]; // _output->data.covariance[1] = _mmseEst->covariance[2]; _output->data.var_x = _mmseEst->covariance[0]; _output->data.var_y = _mmseEst->covariance[2]; _output->data.var_z = _mmseEst->covariance[5]; _output->data.var_psi_berg = _mmseEst->covariance[44]; _output->data.valid = valid; _output->data.time = _mmseEst->time; _output->data.psi_berg = _mmseEst->psi_berg; _output->data.psi_dot_berg = _mmseEst->psi_dot_berg; _cntr++; // Change this to "long" when the Ctd vehicle is sychronized with CVS. // if (_tercom->is_connected()) { try { _output->data.cntr = (short) _cntr; _numReinits = _tercom->getNumReinits(); _filterState = _tercom->getFilterState(); _output->data.reinitFilter = False; } catch(Exception e) { Syslog::write("TerrainAid::Caught exception in TerrainNav"); Syslog::write("TerrainAid::Exception: %s", e.msg); _output->data.trnConnected = False; } // Check the message type and take action if necessary // if (_msg._msg != TerrainAidMsg::None) { if (_msg._msg == TerrainAidMsg::ReinitFilter) { _tercom->reinitFilter(True); _output->data.reinitFilter = True; // So it shows on the plot } } } // trnConnected else { _output->data.trnConnected = False; } _log->write(); _output->write(); } void TerrainAid::createCfgAttributes() { char nameStr[64]; // // Don't forget that StringAttribute allocates the memory itself, so there // is no need to allocate it here. // sprintf( nameStr, "mapFileName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_mapFileName, "NotSpecified" ) ); sprintf( nameStr, "map_type" ); _attributes.add( new IntegerAttribute(nameStr, nameStr, &_map_type, 1 ) ); sprintf( nameStr, "particlesName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_particlesName, "NotSpecified" ) ); sprintf( nameStr, "vehicleCfgName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_vehicleCfgName, "NotSpecified" ) ); sprintf( nameStr, "terrainNavServer" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_terrainNavServer, "NotSpecified" ) ); sprintf( nameStr, "terrainNavPort" ); _attributes.add( new IntegerAttribute(nameStr, nameStr, &_terrainNavPort, 27027 ) ); sprintf( nameStr, "forceLowGradeFilter" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_forceLowGradeFilter, NotSpecified ) ); sprintf( nameStr, "allowFilterReinits" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_allowFilterReinits, NotSpecified ) ); sprintf( nameStr, "useModifiedWeighting" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_useModifiedWeighting, NotSpecified ) ); sprintf( nameStr, "samplePeriod" ); _attributes.add( new IntegerAttribute(nameStr, nameStr, &_samplePeriod, NotSpecified ) ); _attributes.add( new FloatAttribute("maxNorthingCov", "Maximum northing covariance", &_maxNorthingCov, NotSpecified ) ); _attributes.add( new FloatAttribute("maxEastingCov", "Maximum easting covariance", &_maxEastingCov, NotSpecified ) ); _attributes.add( new FloatAttribute("maxDepthCov", "Maximum depth covariance", &_maxDepthCov, NotSpecified ) ); _attributes.add( new FloatAttribute("maxPsiBergCov", "Maximum psi berg covariance", &_maxPsiBergCov, NotSpecified ) ); _attributes.add( new FloatAttribute("maxNorthingError", "Maximum northing error", &_maxNorthingError, NotSpecified ) ); _attributes.add( new FloatAttribute("maxEastingError", "Maximum easting error", &_maxEastingError, NotSpecified ) ); _attributes.add( new FloatAttribute("RollOffset", "Roll Offset", &_phiBias, NotSpecified ) ); sprintf( nameStr, "useIDTData" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_useIDTData, False ) ); sprintf( nameStr, "useDVLSide" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_useDvlSide, False ) ); sprintf( nameStr, "useMbTrnData" ); _attributes.add( new BooleanAttribute(nameStr, nameStr, &_useMbTrnData, False ) ); } // Load the config attributes and do some sanity checking // void TerrainAid::loadConfigFile(const char* configFile) { AttributeParser::parse(configFile, &_attributes); // Validate the input data, that is, check ranges here. } /////////////////////////////////////////////////////////////////// // Return the log directory name pointed to by $AUV_LOG_DIR/latest. // Example: if latest->2016.351.03, filename will contain "2016.351.03" // namelen is the dimension of filename, i.e., sizeof(filename). This // function handles the len-1 issue. // char TerrainAid::latestLogDirName(char filename[], size_t namelen) { struct stat latest; struct stat log; struct dirent *dp; char *logs = getenv("AUV_LOG_DIR"); if (!logs) { Syslog::write("System::latestLogDirName() - AUV_LOG_DIR undefined"); return 0; } // Retrieve the stat info of the directory that latest points to // char latestpath[300]; sprintf(latestpath, "%s/latest", logs); if (access(latestpath, F_OK) < 0) { Syslog::write("System::latestLogDirName() - Could not find %s", latestpath); return 0; } stat(latestpath, &latest); // Open the logs directory // DIR *dirp = opendir(logs); if (!dirp) { Syslog::write("System::latestLogDirName() - Could not open %s", logs); return 0; } // Look for matching inodes // while (dp = readdir(dirp)) { if (dp->d_stat.st_ino == latest.st_ino) { if (strcmp(dp->d_name, "latest")) // Don't really have to do this, but... { filename[0] = '\0'; sprintf(filename, "%s-TRN/", basename(dp->d_name)); return 1; // Found it! } } } Syslog::write("System::latestLogDirName() - Could not find what %s points to", latestpath); return 0; } // Record the attributes in Syslog // void TerrainAid::printCfgAttributes() { // // Convert the integer byte values to engineering units in the argument // list below. // Syslog::write("TerrainAid -- configuration:\n"); Syslog::write("\tmapFileName = %s\n", _mapFileName); Syslog::write("\tmap_type = %d\n", _map_type); Syslog::write("\tparticlesName = %s\n", _particlesName); Syslog::write("\tvehicleCfgName = %s\n", _vehicleCfgName); Syslog::write("\tterrainNavServer = %s:%d\n", _terrainNavServer, _terrainNavPort); Syslog::write("\tforceLowGradeFilter = %s\n",(_forceLowGradeFilter)?"true":"false"); Syslog::write("\tallowFilterReinits = %s\n",(_allowFilterReinits)?"true":"false"); Syslog::write("\tuseModifiedWeighting = %s\n",(_useModifiedWeighting)?"true":"false"); Syslog::write("\tsamplePeriod = %d",_samplePeriod); Syslog::write("\tmaxNorthingCov = %.2f",_maxNorthingCov); Syslog::write("\tmaxEastingCov = %.2f",_maxEastingCov); Syslog::write("\tmaxDepthCov = %.2f",_maxDepthCov); Syslog::write("\tmaxPsiBergCov = %.2f",_maxPsiBergCov); Syslog::write("\tmaxNorthingError = %.2f",_maxNorthingError); Syslog::write("\tmaxEastingError = %.2f",_maxEastingError); Syslog::write("\tuseIDTData = %s\n",(_useIDTData)?"true":"false"); Syslog::write("\tuseDVLSide = %s\n",(_useDvlSide)?"true":"false"); Syslog::write("\tuseMbTrnData = %s\n",(_useMbTrnData)?"true":"false"); }