/****************************************************************************/ /* 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 "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 "MultibeamerIF.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), _attributes(config), NotSpecified(-1), _test(test), _sim(sim) { Boolean debug = True; _log = new TerrainAidLog(this, DataLog::BinaryFormat); _output= new TerrainAidOutput(); // // Zero interface pointers in case one or more doesn't open. _nav = NULL; _parosci = NULL; _dvl = NULL; _mb = NULL; _idt = NULL; _useIDTData = False; 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"); } try { _dvl = new DvlIF(DvlIFServerName); if (_useIDTData) _idt = new DeltaTIF("Delta_TServer"); } catch(...) { Syslog::write("TerrainAid:: -- Failed to initialize connection " "to DvlIF\n"); } } if (false) _mb = new MultibeamerIF("MultibeamerServer"); char configFile[128]; strcpy(configFile, System::configurationFile(_config)); configFile[127]='\0'; dprintf("Configuration file = %s\n", configFile); createCfgAttributes(); loadConfigFile(configFile); System::copyToLogDir(configFile); printCfgAttributes(); _useIns = false; if( !_test ) { VehicleConfigurationIF _vehicleConfig("vehicleConfig"); if(_vehicleConfig.useIns() == 1) _useIns = true; } char tempConfig[256]; char tempMapFile[256]; strcpy(tempConfig, _vehicleCfgName ); strcpy(tempMapFile, _mapFileName); strcpy(_vehicleCfg, System::configurationFile(_vehicleCfgName)); System::copyToLogDir(_vehicleCfg); dprintf("Vehicle configuration file = %s\n", _vehicleCfg); strcpy(_dvlCfg, System::configurationFile(_dvlCfgName)); System::copyToLogDir(_dvlCfg); dprintf("Dvl configuration file = %s\n", _dvlCfg); strcpy(_resonCfg, System::configurationFile(_resonCfgName)); System::copyToLogDir(_resonCfg); dprintf("Reson configuration file = %s\n", _resonCfg); 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 //Initialize TerrainNav object with Particle Filter // Use TerrainNav or TerrainNavClient depending on the // terrainNavServer attribute. RGH Feb-June 2013 // 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); delete 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", _terrainNavServer, _terrainNavPort); //Syslog::write("TerrainAidDriver: Files %s, %s", // _mapFileName, _vehicleCfgName); printf("Map File: %s\n", tempMapFile); printf("Got Here.\n"); _tercom = new TerrainNavClient(_terrainNavServer, _terrainNavPort, tempMapFile, tempConfig, 2, _map_type); Syslog::write("TerrainAidDriver::Connected to server."); } _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); _navData = new poseT; _mleEst = new poseT; _mmseEst = new poseT; _mbData = new mbT; // For DVL data _dvlData = new measT; _dvlData->numMeas = 4; _dvlData->ranges = 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->dataType = 5; //5 denotes IDT data _idtData->measStatus = new bool[_idtData->numMeas]; _filterState = 1; _numReinits = 0; // This callback setup has moved to TerrainAidTask class // //addPeriodicCallback(_samplePeriod, (CallbackMethod)TerrainAid::execute); _cntr = 0; _first = True; dprintf("TerrainAidDriver::TerrainAidDriver - ctor complete"); } TerrainAid::~TerrainAid() { Syslog::write(" TerrainAidDestructor Executing."); 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; if (_mbData->ranges) delete _mbData->ranges; delete _mbData; } 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() { // 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) dvlStatus = _dvl->get( &_dvlIFData, &_notUsed); 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); } _navData->x = _position.x; _navData->y = _position.y; _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->bottomLock = true; } else if(_dvlIFData.waterStatus == 0) { _navData->vx = _dvlIFData.waterMassVelocity[0]; _navData->vy = _dvlIFData.waterMassVelocity[1]; _navData->vz = _dvlIFData.waterMassVelocity[2]; _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; _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 for measure updates? // Test value from config file. // if (!_useIDTData) { _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; } } else { _measData = _idtData; // 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; for (int i = 0; i < _idtIFData.nbeams; i++) { _idtData->ranges[i] = _idtIFData.beam_ranges[i]; _idtData->measStatus[i] = true;; } } 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); } // // Estimate location. 1=> MLE; 2=> MMSE _tercom->estimatePose( _mleEst, 1); _tercom->estimatePose( _mmseEst, 2); #if 0 // Use reson multibeam data to update TRN // MultibeamerIF::MB_Data mb_d; _mb->get_data(&mb_d); _mbData->time = Time::seconds(&mb_d.updateTime); _mbData->numBeams = mb_d.nbeams; if (!_mbData->ranges) _mbData->ranges = new float[_mbData->numBeams]; for (int b = 0; b < _mbData->numBeams; b++) _mbData->ranges[b] = mb_d.ranges[b]; _tercom->mbUpdate(_mbData); #endif //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(...) { Syslog::write("TerrainAid::Caught exception in TerrainNav"); } 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. // Boolean valid = False; if( (_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) ) { valid = True; } // 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.valid = valid; _output->data.time = _mmseEst->time; _cntr++; // Change this to "long" when the Ctd vehicle is sychronized with CVS. // _output->data.cntr = (short) _cntr; _numReinits = _tercom->getNumReinits(); _filterState = _tercom->getFilterState(); _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, "vehicleCfgName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_vehicleCfgName, "NotSpecified" ) ); sprintf( nameStr, "dvlCfgName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_dvlCfgName, "NotSpecified" ) ); sprintf( nameStr, "resonCfgName" ); _attributes.add( new StringAttribute(nameStr, nameStr, &_resonCfgName, "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("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 ) ); } // 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. } // 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("\tvehicleCfgName = %s\n", _vehicleCfgName); Syslog::write("\tdvlFileName = %s\n", _dvlCfgName); Syslog::write("\tresonCfgName = %s\n", _resonCfgName); 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("\maxNorthingCov = %.2f",_maxNorthingCov); Syslog::write("\maxEastingCov = %.2f",_maxEastingCov); Syslog::write("\maxNorthingError = %.2f",_maxNorthingError); Syslog::write("\maxEastingError = %.2f",_maxEastingError); }