#include #include "Navigation.h" #include "Syslog.h" #include "WorkSiteIF.h" #include "LayeredControlIF.h" #include "SimulatorIF.h" #include "LowAltitudeFollowing.h" #include "VehicleConfigurationIF.h" #include "FastTime.h" #include "TimeP.h" #include "MathP.h" #include "NavUtils.h" #include "matrixMath.h" #ifndef PI #define PI 3.14159265358979323846 #endif #define R2D(r) (r*180.0/PI) #define TaskIFtimeout 10 // // This is how to bring in simulated data for debugging: // // #include "SimulatorIF.h" // SimulatorIF sim("simulator"); // double simPos[3], simVel[3], simEuler[3], simOmega[3]; // -Then, inside a method- sim.state( simPos, simVel, simEuler, simOmega ); // Navigation::Navigation(int millisec) : PeriodicTask("navigation") { Boolean debug = False; dprintf("New Navigation component constructor"); WorkSiteIF workSite("workSite"); _z0 = workSite.getStartDepth(); Syslog::write("Navigation::Navigation - Setting start depth to %.1f meters.", _z0); initializeState(); _log = new NavigationLog(this); try { _input = new NavigationInput(MessageQueue::ReadWrite); } catch(...) { Syslog::write("Navigation:: Error - Couldn't open message queue."); } try { _output = new NavigationOutput(SharedData::Write); } catch(...) { Syslog::write("Navigation:: Error - Couldn't open output mem"); } // // Maximum allowable angle of the forward-looking sonar when only the // Echosounder is providing altitude, as measured from the vehicle z (down) // axis towards the nose. Any greater and the sonar beam will have too low a // grazing angle with the bottom. _maxMount = PI/6.; VehicleConfigurationIF vehicleConfig("vehicleConfig"); _logOnly = 1; _logOnly = vehicleConfig.logOnly(); Syslog::write("Navigation::Navigation _logOnly = %d", _logOnly); // // Specify whether to use the echo sounder or the DVL,or both, as the // altimeter, if both are present. // // This implementation below where a number is read in from vehicle.cfg is // crude. Probably a better way is to use the StringAttribute family to // read in a string from vehicle.cfg, but it is unfortunately rather // complicated. Do it later... // _altimeterInstrument = NOT_SPECIFIED; if( vehicleConfig.altimeterInstrument() == 0. ) { _altimeterInstrument = DVL; Syslog::write("Navigation - Using only the Dvl to compute altitude."); } else if( vehicleConfig.altimeterInstrument() == 1. ) { _altimeterInstrument = ECHOSOUNDER; Syslog::write("Navigation - Using only the echo sounder to compute altitude."); } else if( vehicleConfig.altimeterInstrument() == 2. ) { _altimeterInstrument = DVL_FIXED_ECHO_PERP; Syslog::write("Navigation - Using both the Dvl and the echo sounder " "with the perpendicular-distance method to compute altitude."); } else if( vehicleConfig.altimeterInstrument() == 3. ) { _altimeterInstrument = DVL_FIXED_ECHO_MIN; Syslog::write("Navigation - Using both the Dvl and the echo sounder " "with the minimum-distance method to compute altitude."); } else if( vehicleConfig.altimeterInstrument() == 4. ) { _altimeterInstrument = DVL_SCANNING_ECHO; Syslog::write("Navigation - Using the Dvl and the echo sounder " "in the scanning mode to compute altitude."); } else if( vehicleConfig.altimeterInstrument() == 5. ) { _altimeterInstrument = MULTIBEAM; if( !_logOnly ) { Syslog::write("Navigation - Using the Imagenex multibeam " "to compute altitude."); } } else Syslog::write("Navigation - No altimeter present; do not " "compute altitude."); // Create core sensors // // The integer argument after &_coreSensors is the number of // consecutive bad readings that will be rejected. The following argument // is the maximum allowable depth change, in one sample interval, that // can occur without being declared bad. // _depthSensor = new DepthSensor("Nav->DepthSensor", &_coreSensors, 40, 500.0); _tailCone = new TailCone("Nav->TailCone", &_coreSensors, 100); _useIns = False; //create an INS if the useIns flag is set, otherwise create //an AHRS interface if ( vehicleConfig.useIns() == 1. ) _useIns = True; if (_useIns) { _ins = new Ins("Nav->KearfottServer", &_coreSensors, INS_AHRS_TIMEOUT); _ahrs = NULL; } else { _ahrs = new Ahrs("Nav->Ahrs", &_coreSensors, 100 ); _ins = NULL; } _dvl = new Dvl("Nav->Dvl", &_coreSensors); if( _altimeterInstrument == MULTIBEAM ) _multibeam = new Multibeam("Nav->Multibeam", &_auxSensors, 15); else _multibeam = NULL; //create auxiliary sensors _gps = new Gps("Nav->Gps", &_auxSensors, (const NavigationIF::Position *)&_state.position, MaxGpsFixDepth); // _lbl = new Lbl("Lbl", &_auxSensors); _lbl = NULL; _echoSounder = new EchoSounder("Nav->EchoSounder", &_auxSensors); _velocimeter = new Velocimeter("Nav->Velocimeter", &_auxSensors); _usbl = new Usbl("Nav->Usbl", &_auxSensors); // // Move this into the auxiliary sensor framework if it works. Syslog::write("Navigation: Opening Acoustic Modem Interface:"); try { _acommsIF = new AcousticModemIF("Nav->Navigation", "BenthosModemServer", 20); Syslog::write("Navigation: acommsIF = 0x%x\n", _acommsIF); } catch(...) { Syslog::write("Navigation: %s: unable to open modem interface\n", name()); _acommsIF = NULL; } _terrainAidIF = 0; _enableTRN = True; if( _enableTRN ) { try { _terrainAidIF = new TerrainAidIF("TerrainAidIFServer", 10); } catch (...) { _terrainAidIF = 0; Syslog::write("Navigation - Could not open TerrainAid IF."); } } _lastTerNavTime = 0.; _dx = 0.; //Terrain Nav biases _dy = 0.; //Assume LBL is not running Lbl_up = False; // Create TaskInterfaces, and check to see that all core sensor // servers are running. connectSensors(); // // Verify that either the Dvl or the Echosounder or both are present // and operating as required by the _altimeterInstrument switch. // 30 Oct 2013 rsm. Need to rework this, or probably just elminate it. // verifyOASensors(); // // Set the flag Lbl_up if we can connect to the lbl server. We'll only check // this once, and assume that if the server is up, it will remain up for // the duration of the mission. // if (_lbl && _lbl->taskIF()) { Lbl_up = True; //initialize matrices for Jerome's LBL algorithm //comment the following line out if using BAM-DY algorithm init_matrices(); } #ifdef FASTTIME // check whether FastTime enabled // [could cache the value, instead of checking environment each time] char *fast_time_en=getenv(FAST_TIME_ENV); if (fast_time_en && (strcmp(fast_time_en,FAST_TIME_EN)==0) ) { Syslog::write("Navigation: Setting simulator callback"); // For simualtion purposes, Navigation will be triggered by // a NewOutput event from Simulator rather than a periodic timer // SimulatorIF *simulator = new SimulatorIF(SimulatorIFServerName); _eventService->subscribe(simulator, SimulatorIF::NewOutput, (EventService::Callback )Navigation::callback); } else #endif addPeriodicCallback(millisec, (CallbackMethod )Navigation::callback); _firstCallback = True; // // Get stuff from worksite.cfg. // _magneticVar = workSite.magneticVariation(); _utmZone = workSite.utmZone(); _state.position.latitude = workSite.latitude(); _state.position.longitude = workSite.longitude(); // // Initialize the position state. Below, position.x is the Northing, // and position.y is the Easting, and they're in UTM coordinates. // NavUtils::geoToUtm(_state.position.latitude, _state.position.longitude, _utmZone, &_state.position.x, &_state.position.y); dprintf(" Navigation: The utm zone is %d\n", workSite.utmZone()); dprintf(" Navigation: The lat/long in workSite.cfg, %f, %f\n", R2D(workSite.latitude()), R2D(workSite.longitude())); dprintf(" Navigation: The lat/long in workSite.cfg, " "cvt'ed to UTM is %.2f, %.2f\n", _state.position.x, _state.position.y); // // Initialize time variables to prevent a big jump at start-up. // struct timespec timeSpec; Time::gettime(&timeSpec); _currentTime = Time::seconds(&timeSpec); m_lastUpdateTime = dvl_lastUpdateTime = _currentTime; // // Use the Lbl for nav, or just log it? // _useLbl = False; if( vehicleConfig.useLbl() == 1. ) _useLbl = True; m_deltaT = 0.; // // Distance between the Dvl and the echo sounder. Put this in vehicle.cfg later. // m_distDvlPsa = 3.38; //meters // // Angle between the body-fixed z axis and the Dvl beams projected into // the xz plane: m_delta = PI/8.; if( _echoSounder->valid() && _altimeterInstrument != DVL) { // // Compute constants as defined in the report "AUV Guidance, Navigation // and Control Improvements." // // Beware that xzMountAngle = pi/2 - thetaM. // // Also, if xzMountAngle < pi/4, the vertex of the triangle is more than // distDvlPsa above the vehicle. This is probably not what you want. // Declare this an error. // m_thetaM = PI/2. - _echoSounder->xzMountAngle(); dprintf(" Navigation: m_thetaM = %.3f\n", m_thetaM); m_sinThetaM = sin( m_thetaM ); m_cosThetaM = cos( m_thetaM ); m_sinThetaMpDel = sin( m_thetaM + m_delta ); m_cosThetaMpDel = cos( m_thetaM + m_delta ); m_cosDelta = cos( m_delta ); // if( fabs( _echoSounder->xzMountAngle() ) < PI/4. ) { if( _altimeterInstrument == DVL_FIXED_ECHO_PERP ) { Syslog::write("Navigation::Navigation - Error. " "xzMountAngle must be > 45 deg. for " "Perpendicular-Distance OA algorithm.\n"); initiateAbort(); } } else { //m_DoverCosThM = m_distDvlPsa / cos( m_thetaM ); m_a = m_distDvlPsa * ( tan(m_thetaM)*sin(m_delta)/m_cosThetaMpDel + 1./cos(m_thetaM) ); //m_DtanThM = m_distDvlPsa * tan( m_thetaM ); m_b = m_distDvlPsa * m_sinThetaM/m_cosThetaMpDel; } dprintf("Navigation::Navigation() m_a = %f.2 \n", m_a); dprintf("Navigation::Navigation() m_b = %f.2 \n", m_b); } //if( _echoSounder->valid() ) else if( !_echoSounder->valid() && _altimeterInstrument != DVL) { Syslog::write("Navigation::Navigation() - Error. The echo sounder IF isn't \n" "yet connected; can't read it's data."); //initiateAbort(); } Syslog::write("Navigation::Navigation(). " "_es->valid() = %d at t = %d.", _echoSounder->valid(), Time::milliseconds()); } Navigation::~Navigation() { // Delete NavSensor objects NavSensor *sensor; int i; for (i = 0; i < _coreSensors.size(); i++) { _coreSensors.get(i, &sensor); delete sensor; } for (i = 0; i < _auxSensors.size(); i++) { _auxSensors.get(i, &sensor); delete sensor; } if (_acommsIF) delete _acommsIF; if( _terrainAidIF ) delete _terrainAidIF; delete _log; if (_input) delete _input; if (_output) delete _output; } void Navigation::initializeState() { _state.attitude.roll = _state.attitude.pitch = _state.attitude.yaw = 0.; _state.attitude.omega_B_x = _state.attitude.omega_B_y = _state.attitude.omega_B_z = 0.; _state.position.latitude = _state.position.longitude = 0.; _state.position.x = _state.position.y = 0.; _state.position.xIns = _state.position.yIns = 0.; _state.position.z = _z0; _state.position.xRate = _state.position.yRate = _state.position.zRate = 0.; _state.position.altitude = 1000.; _state.position.depthRate = _state.position.altitudeRate = 0.; m_dvlAltitude = 1000.; m_echoAltitude = 1000.; _avoidRange = 1000.; // // Change the rhs to _vehicleConfig->maxDockRange + 1. when it is // implemented. _state.position.usblSlantRange = 25.; _northing = _easting = 0.; _state.position.flsVertRange = m_echoAltitude; _state.position.flsHorzRange = 1000.; _state.position.useFls = False; _lastOA = m_echoAltitude; _lastOR = _state.position.flsHorzRange; _multibeamAltitude = 0.; _nGood = 0; _medianBeamNo = 0; } // During a fast simulation, Navigation is triggered by the new outputs from the // simulator. But the simulator is waiting for upstream events that all begin with // Navigation. So when running a fast sim, we need this push to get started. // void Navigation::startSampling() { #ifdef FASTTIME // check whether FastTime enabled // [could cache the value, instead of checking environment each time] char *fast_time_en=getenv(FAST_TIME_ENV); if (fast_time_en && strcmp(fast_time_en,FAST_TIME_EN)==0) { Syslog::write("\t\t\t\t============== Navigation giving simulation a push start in 2..."); sleep(2); callback(); } else #endif PeriodicTask::startSampling(); } void Navigation::connectSensors() { int i; // Create TaskInterfaces core sensors, and check to see that all // servers are present Boolean coreMissing = False; for (i = 0; i < _coreSensors.size(); i++) { NavSensor *sensor; _coreSensors.get(i, &sensor); // Create the TaskInterface and connect to its server Syslog::write("Navigation::connectSensors core - connecting to %s at t = %d.", sensor->name(), Time::milliseconds()); sensor->connect(TaskIFtimeout); // Verify that server is out there if (!sensor->taskIF()) { Syslog::write("Navigation: Server for core sensor \"%s\" not found", sensor->name()); coreMissing = True; } } // Create TaskInterfaces for auxiliary sensors for (i = 0; i < _auxSensors.size(); i++) { NavSensor *sensor; _auxSensors.get(i, &sensor); // Create the TaskInterface and connect to its server Syslog::write("Navigation::connectSensors aux - connecting to %s at t = %d.", sensor->name(), Time::milliseconds()); sensor->connect(TaskIFtimeout); // Verify that server is out there if (!sensor->taskIF()) { Syslog::write("Navigation: Server for aux sensor \"%s\" not found", sensor->name()); } } if (coreMissing) { // Fatal error if core sensor(s) missing Syslog::write("Server for one or more core sensors not found"); initiateAbort(); } } void Navigation::verifyOASensors() { // // Definitions: // // The following are true if: // // _device->valid() - The device is connected, the server thread is present // and operating, _status == Ok, and the current // measurement is valid. // // _device->status()- Is an enum { Ok, Initializing, Offline, Error}. If // the device isn't present, _status == Initializing. // // _device->taskIF()- The task interface exists. (createTask() completed) // // _device->taskIF()->connected() - The server is present and connected? // // The "if" statement below checks ->taskIF(), and not ->valid, because we // just want to know if the device is present and healthy, not if the current // measurement is valid. // if( !_echoSounder->taskIF() && !_dvl->taskIF() ) { // // Both the echo sounder and the Dvl are declared above as auxiliary // sensors, but we'll require that at least one of them is present. // Syslog::write(" Navigation::verifyOASensors - Error. Neither the" " Echosounder or the Dvl" " was found. Aborting.\n"); initiateAbort(); } // // Verify that the DVL is selected as the altimeter when the echo sounder is absent. // // if( !_echoSounder->taskIF() ) { // // The echo sounder is not present, but the DVL is. // if( _altimeterInstrument != DVL ) { Syslog::write(" Navigation::verifyOASensors - Error. The" " Echosounder is required" " by the altimeterInstrument\n" " switch in vehicle.cfg, but is not present.\n"); initiateAbort(); } } else if( !_dvl->taskIF() ) { // // DVL is not present, but the echo sounder is. However, the echo // sounder may not be pointing down. Abort if the echo sounder is not // explicity selected in vehicle.cfg to be used as the altimeter. // if( _altimeterInstrument != ECHOSOUNDER ) { Syslog::write(" Navigation::verifyOASensors - Error. The echo" " sounder is present, but was not selected \n" " to be used as the altimeter in vehicle.cfg. Abort.\n"); initiateAbort(); } if( !_echoSounder->isScanning() && _echoSounder->xzMountAngle() > _maxMount ) { Syslog::write(" Navigation::verifyOASensors - Error." " The echo sounder is mounted at too shallow an angle\n" " to provide altitude, and the Dvl is absent. Abort.\n"); initiateAbort(); } // // if scanning, check that the scan is off and the train + xzMount < maxMount. } else { // Both are present. vehicle.cfg must specify which to use. if( _altimeterInstrument == NOT_SPECIFIED ) { Syslog::write("Neither the echo sounder nor the Dvl was specified " "as the altimeter in vehicle.cfg. Aborting.\n"); initiateAbort(); } // // Check that if scanning, check that the scan is off and the train + // xzMount < maxMount for perp and min alt algorithms. } } void Navigation::callback() { struct timespec timeSpec; Time::gettime(&timeSpec); _currentTime = Time::seconds(&timeSpec); if( _firstCallback ) { _firstCallback = False; _navLoopInterval = 0.; _navStartTime = _currentTime; } else _navLoopInterval = _currentTime - _lastTime; _lastTime = _currentTime; //Syslog::write(" "); //Syslog::write(" "); //Syslog::write("Navigation:: Executing at %.3f seconds.", // _currentTime - _navStartTime); //Process input, if necessary: NavigationInput::Command cmdObj; while ((_input->read(&cmdObj)) > 0) { switch (cmdObj._cmd) { case NavigationInput::None: Syslog::write("Navigation::Received empty command at t=%.2f\n", _currentTime); break; case NavigationInput::resetUTM: // // Add the offsets: _state.position.x += cmdObj._northingOffset; _state.position.y += cmdObj._eastingOffset; _northing = _state.position.x; _easting = _state.position.y; Syslog::write("Navigation::RESET UTM COORDINATES to " "(%.1f, %.1f)\n at t = %.2f", _northing, _easting, _currentTime); // // update latitude & longitude values: NavUtils::utmToGeo(_state.position.x, _state.position.y, _utmZone, &_state.position.latitude, &_state.position.longitude); break; case NavigationInput::setRange: _avoidRange = cmdObj._avoidRange; Syslog::write("Navigation::Set avoidRange to " "%.1fm t = %.2f sec.", _avoidRange, _currentTime); break; default: break; } if( _input->msgsPending()) Syslog::write("Navigation:: %d messages pending.\n", _input->msgsPending()); } // Read sensors readSensors(); // Process data and write to output processSensorData(); } void Navigation::readSensors() { int i; Boolean abort; NavSensor *sensor; Boolean debug = False; // Read each core sensor for (i = 0; i < _coreSensors.size(); i++) { _coreSensors.get(i, &sensor); dprintf("Navigation::readSensors() - sensor %s", sensor->name()); sensor->read(); if (sensor->failed()) { // Too many bad hits for this core sensor; abort! Syslog::write("Navigation - %d consecutive bad readings from sensor %s", sensor->nConsecutiveBad(), sensor->name()); Syslog::write("Navigation - aborting!"); initiateAbort(); } } // Read each aux sensor for (i = 0; i < _auxSensors.size(); i++) { _auxSensors.get(i, &sensor); sensor->read(); } } void Navigation::processSensorData() { Boolean reset; reset = 0; if (_ahrs && _ahrs->valid()) { _ahrs->attitude(&_state.attitude); // // BEWARE: The AHRS puts out projected, or space-fixed, roll and pitch // angles. THESE ARE NOT EULER ANGLES. The following relation converts // the space-fixed roll angle into a 2-1 Euler. See Wertz, p.226. // // At 20 Deg pitch, the Euler roll and the space-fixed roll differ // by about 20 milli-Radians. // _state.attitude.roll = atan( cos(_state.attitude.pitch)*tan(_state.attitude.roll) ); if (_ahrs->magneticCompass()) { // Correct heading for magnetic variation _state.attitude.yaw += _magneticVar; // Add curve-fit correction. This includes Dvl misalignment. // _state.attitude.yaw -= // _compassError.evalCompassBias(_state.attitude.yaw); _state.attitude.yaw = PI + Math::modPi( _state.attitude.yaw - PI); // Correct heading for magnetic variation // _state.attitude.yaw = // PI + Math::modPi( _state.attitude.yaw + _magneticVar - PI); } } else { // Estimate from previous? } if (_depthSensor->valid()) { _state.position.z = _depthSensor->depth(); } else { // Estimate from previous? } _dvlFailed = False; _echoSounderFailed = False; _multibeamFailed = False; _state.position.useFls = False; // // Now, we'll use either the Dvl or the altimeter/rangefinder to measure // altitude. // switch( _altimeterInstrument ) { case DVL: // // Only update if the dvl sees the bottom, and the data is new. // changed in Somoa - compute an altitude if there is new data // the altitude() function will set altitude to 1000.0 if there are // no valid beams. Use to check the valid() flag hjt if (_dvl->newData()) { m_dvlAltitude = _dvl->altitude(&_state.attitude); _state.position.altitude = m_dvlAltitude; } else if( _dvl->failed() ) _dvlFailed = True; #if 0 // // This code substitutes the Dvl magnetic compass for the Crossbow, and // corrects the heading for magnetic variation. // (Sib & Hans, Spokes, August 02) // _state.attitude.yaw = PI + Math::modPi( Math::degToRad(_dvl->data.heading-90.0) + _magneticVar - PI); #endif break; case ECHOSOUNDER: if( _echoSounder->valid() ) { _state.position.altitude = _echoSounder->altitude(&_state.attitude, &_state.position); m_echoAltitude = _state.position.altitude; _state.position.useFls = True; } else if( _echoSounder->failed() ) _echoSounderFailed = True; break; case DVL_FIXED_ECHO_PERP: // // For testing/debug, first log the Dvl altitude: // if( _dvl->valid() && _dvl->newData() && _dvl->data.bottomDetectStatus) m_dvlAltitude = _dvl->altitude(&_state.attitude); // if( _logOnly ) _state.position.altitude = m_dvlAltitude; // // Since the Dvl samples slower than the Echosounder, skip this // computation if the Dvl doesn't have new data // if( _echoSounder->valid() && _dvl->valid() && _dvl->newData() && _dvl->data.bottomDetectStatus) { // // Filter the Psa here, if needed. // // Psa outlier & dropout rejection here, if needed. // // See rsm's Auv-Gnc report for diagrams and definitions. double rPp = _echoSounder->range() + m_a; double hp, den, rDp; // // The following equation relies on the Dvl being oriented with // the alignment notch forward. (Beam 1 to port bow, beam 2 to // starboard quarter, etc.). // rDp = (_dvl->data.beam1 + _dvl->data.beam3 )*m_cosDelta/2. + m_b; // // The problem with the eqn below is that it computes altitude // wrto the inertial z axis, while the Psa measurement is still // in the body xz plane. You must also compensate rPp with roll // to use the eqn below. // //rDp = //_dvl->data.beam1*cos(_state.attitude.roll + m_delta)/2. + //_dvl->data.beam3*cos(_state.attitude.roll - m_delta)/2. + m_b; den = rDp*rDp + rPp*rPp - 2.*rDp*rPp*m_sinThetaMpDel; // // den, at this point, is the square of the distance between the // points where the Dvl and Psa beams intersect the ground. It // should never be less than a meter. if( den < 1. ) { Syslog::write("Navigation::processSensorData(). Error - \n" "Psa and Dvl beams less than one meter apart on the " "ground."); // // Now what? Abort? den = 1.; } else den = sqrt( den ); // // hp is the altitude of the triangle formed by rDp and rPp (Dvl // and Psa ranges) with the bottom. // hp = rDp * rPp * m_cosThetaMpDel / den; if( hp < rDp ) m_gamma = acos( hp/rDp ); else m_gamma = 0.; // // Later - If gamma > pi/8, we're approaching a very steep hill // or a cliff. Consider putting in logic to turn the vehicle // around, or spiral up for awhile. Possibly only trigger this // if vehicle has already pitched up, and the Psa range is still // getting small. // //if( hp > 0. ) // _state.position.altitude = hp - rDp*m_DtanThM/hp; //else // _state.position.altitude = 0.; double perpRange; // // The following commented-out equation is correct, but not // really what we desire. I replaced it with the uncommented // one. See the report. // perpRange = hp - m_b*m_cosDelta/cos(m_gamma + m_delta); perpRange = hp - m_b*cos(m_gamma); if( _logOnly ) m_dvlAltitude = perpRange; else _state.position.altitude = perpRange; } else if( _echoSounder->failed() ) _echoSounderFailed = True; else if( _dvl->failed() ) _dvlFailed = True; break; case DVL_FIXED_ECHO_MIN: if (_dvl->valid() && _dvl->newData() && _echoSounder->valid()) { // // Take the least of either the Dvl altitude or the altitude // projected ahead of the vehicle by the echo sounder. // double altitude; altitude = _dvl->altitude(&_state.attitude); m_dvlAltitude = altitude; m_echoAltitude = _echoSounder->altitude(&_state.attitude, &_state.position); if( m_echoAltitude < altitude ) { altitude = m_echoAltitude; } _state.position.useFls = True; if( _logOnly ) { _state.position.altitude = m_dvlAltitude; _state.position.useFls = False; } else _state.position.altitude = altitude; _state.position.flsVertRange = m_echoAltitude; //_state.position.flsHorzRange = } else if( _echoSounder->failed() ) _echoSounderFailed = True; else if( _dvl->failed() ) _dvlFailed = True; break; case DVL_SCANNING_ECHO: if (_dvl->valid() && _dvl->newData() && _echoSounder->valid()) { // // Take the least of either the Dvl altitude or the altitude // found by horizon detection. double altitude; altitude = _dvl->altitude(&_state.attitude); m_dvlAltitude = altitude; //m_echoAltitude = _echoSounder->altitude(&_state.attitude, // &_state.position); _echoSounder->obstacleDetect( &_state.attitude, &_state.position, &_rangeToObstacle); m_echoAltitude = _rangeToObstacle.vertMin; m_echoHorizRange = _rangeToObstacle.horizMin; _state.position.useFls = True; Boolean debug = False; dprintf("Nav/ m_echoHorizRange = %.2f at t=%d.", m_echoHorizRange, Time::milliseconds()); if( m_echoAltitude < altitude ) { altitude = m_echoAltitude; } if( m_echoAltitude == INVALID_ALTITUDE || m_dvlAltitude == INVALID_ALTITUDE ) { altitude = INVALID_ALTITUDE; } if( _logOnly ) { _state.position.altitude = m_dvlAltitude; _state.position.useFls = False; } else _state.position.altitude = altitude; _state.position.flsVertRange = m_echoAltitude; _state.position.flsHorzRange = m_echoHorizRange; } else if( _echoSounder->failed() ) _echoSounderFailed = True; else if( _dvl->failed() ) _dvlFailed = True; break; case MULTIBEAM: if (_multibeam->valid() ) { double altitude, obstacleAltitude, obstacleRange; double obstacleSlopeAlt, obstacleSlopeRange; _multibeam->compute(&_state.attitude, &_state.position, _avoidRange, &altitude, &_nadirCnts, &_nGood, &_medianBeamNo, &obstacleAltitude, &obstacleRange, &_obstacleCnts, &_minBeamRange, &_minBeamCnts, &_thetaB, &_lsAlt, &obstacleSlopeAlt, &obstacleSlopeRange, &_obstacleSlopeCnts); _state.position.useFls = False; _state.position.flsVertRange = obstacleAltitude; _state.position.flsHorzRange = obstacleRange; _state.position.thetaB = _thetaB; _state.position.obstacleSlopeAlt = obstacleSlopeAlt; _state.position.obstacleSlopeRange = obstacleSlopeRange; //_state.position.altitude = obstacleAltitude; _state.position.altitude = altitude; // // For logging, use the scanner's plumbing: _multibeamAltitude = altitude; m_echoAltitude = obstacleAltitude; m_echoHorizRange = obstacleRange; _rangeToObstacle.vertLast = _lastOA; _rangeToObstacle.horizLast = _lastOR; _lastOA = obstacleAltitude; _lastOR = obstacleRange; Boolean debug = False; dprintf("Nav/ horizontal range = %.2f at t=%d.", obstacleRange, Time::milliseconds()); m_dvlAltitude = _dvl->altitude(&_state.attitude); if( _logOnly ) { _state.position.useFls = False; _state.position.altitude = m_dvlAltitude; if( _dvl->failed() ) _dvlFailed = True; } } else if( _multibeam->failed() ) _multibeamFailed = True; break; default: Syslog::write(" Navigation: Error - altimeterInstrument == %d" " doesn't exist. Abort.\n", _altimeterInstrument ); initiateAbort(); break; } if( _echoSounderFailed ) { // // failed() is true when _nConsecutiveBad > _maxConsecutiveBad. Syslog::write("Navigation::processSensorData(). Error. \n" " The Echo Sounder has stopped responding after %d " " read attempts. Aborting.\n", _echoSounder->maxConsecutiveBad()); initiateAbort(); } if( _dvlFailed ) { // // failed() is true when _nConsecutiveBad > _maxConsecutiveBad. Syslog::write("Navigation::processSensorData(). Error. \n" " The Dvl has stopped responding after %d " " read attempts. Aborting.\n", _dvl->maxConsecutiveBad()); initiateAbort(); } if( _multibeamFailed ) { // // failed() is true when _nConsecutiveBad > _maxConsecutiveBad. Syslog::write("Navigation::processSensorData(). Error. \n" " The multibeam has stopped responding after %d " " read attempts. Aborting.\n", _multibeam->maxConsecutiveBad()); initiateAbort(); } // // Measure the forward speed through the water, commonly known as "u". // First, check to see if the Dvl is valid. If not, try to get the water // speed from the tail cone. // // If the Dvl is valid, and it has new data, and it has water track, // update _waterSpeed. Otherwise, do NOT update _waterSpeed, so that // _waterSpeed retains its last value. // // NOTE: We needn't include INS or GPS here for a possible water speed // measurement, since when these measurements are available we don't have // to dead-reckon. // if( _dvl->valid() ) { if( _dvl->newData() && (_dvl->data.waterStatus == 0) ) { _waterSpeed = _dvl->data.waterMassVelocity[0]; } } else if( _tailCone->valid() ) { // // The Dvl didn't get a valid bottom-track OR water-track measurement on // it's last measurement. // _waterSpeed = _tailCone->waterSpeed(); } else if( !_gps->valid() && !_ins->valid() ) { // // If, in addition, neither the GPS or the INS is valid, abort. We // don't want the vehicle moving with no estimate of position. // Syslog::write(" Navigation:: Error - Neither the Dvl nor the Tailcone\n" " are valid, so we don't have water speed, and can't \n" " dead reckon. Abort. \n"); initiateAbort(); } // // Begin NAVIGATION algorithms. Determine x/y position. // _gpsValid = _gps->valid(); _state.position.gpsValid = _gpsValid; if ( _gpsValid ) { Boolean debug = False; // // Update position from GPS fix // dprintf("Navigation:: - using GPS reading..."); _state.position.latitude = _gps->fix.latitude; if( _gps->fix.nsHemisphere == GpsIF::Southern ) _state.position.latitude *= -1.; _state.position.longitude = _gps->fix.longitude; if( _gps->fix.ewHemisphere == GpsIF::Western ) _state.position.longitude *= -1.; NavUtils::geoToUtm(_state.position.latitude, _state.position.longitude, _utmZone, &_state.position.x, &_state.position.y); _northing = _state.position.x; _easting = _state.position.y; dprintf("GPS northing = %f, GPS easting = %f",_northing,_easting); //reset LBL algorithm to use this GPS fix as new initial fix if (Lbl_up){ _lbl->lbl_params.east_init = _state.position.y; _lbl->lbl_params.north_init = _state.position.x; reset = 1; } } // // Acoustic modem update from USBL tracking system. // AcousticModemIF::SMAC_SAN_RECORD fix; DeviceIF::Status modemStatus; Boolean newFix; if (_acommsIF && !_useIns) { Boolean debug = False; modemStatus = _acommsIF->getSANFix(&fix, &newFix); if ((modemStatus == DeviceIF::Ok) && newFix) { // // Convert wfLat & wfLong from decimal degrees to radians: // _state.position.latitude = (double)fix.wfLat*PI/180.; _state.position.longitude = (double)fix.wfLong*PI/180.; double lastNorthing = _state.position.x; double lastEasting = _state.position.y; double offset; NavUtils::geoToUtm(_state.position.latitude, _state.position.longitude, _utmZone, &_state.position.x, &_state.position.y); offset = sqrt( pow( (lastNorthing - _state.position.x),2.) + pow( (lastEasting - _state.position.y),2.) ); Syslog::write("Navigation: SAN - New position is " "lat %.5f lon %.5f decimal degrees\n" "Navigation: SAN - t = %.2f, depth = %.2f meters\n" "Navigation: SAN - Offset is %.2f meters", fix.wfLat,fix.wfLong, _currentTime, _state.position.z, offset); _northing = _state.position.x; _easting = _state.position.y; //reset LBL algorithm to use this GPS fix as new initial fix if (Lbl_up) { _lbl->lbl_params.east_init = _state.position.y; _lbl->lbl_params.north_init = _state.position.x; reset = 1; } } } // // Call the Lbl/Jerome algorithm and log the solution. // if( Lbl_up ) lblPosition(reset); // // Now, if _useLbl == True, use the Lbl/Jerome solution for control. // Otherwise, use the DVL. If the Dvl is invalid, dead-reckon. // if( Lbl_up && _useLbl ) { _state.position.x = _filter_north; _state.position.y = _filter_east; // update latitude & longitude values here NavUtils::utmToGeo(_state.position.x, _state.position.y, _utmZone, &_state.position.latitude, &_state.position.longitude); } else if( _dvl->valid() ) //The Dvl has bottom or water lock. { Boolean debug = False; if( _dvl->newData() ) //The Dvl updates more slowly than this Nav loop. { double cpsi, spsi, ctheta, stheta, cphi, sphi, T_LV_B[3][3]; cpsi = cos(_state.attitude.yaw); spsi = sin(_state.attitude.yaw); ctheta = cos(_state.attitude.pitch); stheta = sin(_state.attitude.pitch); cphi = cos(_state.attitude.roll); sphi = sin(_state.attitude.roll); // // Form the direction cosine matrix that recoordinatizes a vector // from the body frame (B) into the local-level-local-vertical (LV) // navigation frame, which is (North, East, Down). See Fossen, // p.10, Eqn 2.11. // // N is the inertially fixed, or Newtonian frame. // // The transformation T_B_LV, is 3-2-1 Euler. The matrix below, // T_LV_B, is the transpose. // T_LV_B[0][0] = cpsi*ctheta; T_LV_B[1][0] = spsi*ctheta; T_LV_B[2][0] = -stheta; T_LV_B[0][1] = -spsi*cphi + cpsi*stheta*sphi; T_LV_B[1][1] = cpsi*cphi + sphi*stheta*spsi; T_LV_B[2][1] = ctheta*sphi; T_LV_B[0][2] = spsi*sphi + cpsi*cphi*stheta; T_LV_B[1][2] = -cpsi*sphi + stheta*spsi*cphi; T_LV_B[2][2] = ctheta*cphi; // // Transform the bottom-referenced velocity, as measured by the dvl, // from the body frame into the navigation frame. If only // water-referenced velocity is available, we'll substitute it in // place of the other, keeping in mind that in this case the // notation vel_B_N_LV isn't quite correct, as it specifies an // intertial (bottom) reference. // if( _dvl->data.bottomStatus == 0 ) TVMult( vel_B_N_LV, T_LV_B, _dvl->data.bottomTrackVelocity ); else if( _dvl->data.waterStatus == 0 ) TVMult( vel_B_N_LV, T_LV_B, _dvl->data.waterMassVelocity ); else Syslog::write(" Navigation:: Error - _dvl->valid is true, but neither\n" " water mass nor bottom referenced velocities" " are valid.\n"); // // Save the velocity that is in the LV (or Navigation) frame: // _state.position.xRate = vel_B_N_LV[0]; _state.position.yRate = vel_B_N_LV[1]; _state.position.zRate = vel_B_N_LV[2]; if( fabs(vel_B_N_LV[2]) < 10.0 ) _state.position.depthRate = vel_B_N_LV[2]; else _state.position.depthRate = 0.; } // if( _dvl->newData() ) // // Compute the time since the MVC received its last Dvl update, // m_deltaT. _navLoopInterval is the time since the last // navigation computation (that is, the last pass thorough this // routine). // m_deltaT = _currentTime - m_lastUpdateTime; if( _dvl->newData() ) m_lastUpdateTime = _currentTime; // // We know that the Dvl is alive, and that it's last sample was good. If // its last sample was less than two seconds ago, we'll use the last Dvl // reading to dead reckon. // if( (m_deltaT > 0.) && (m_deltaT < 2.0) ) { // // Integrate to get the position change, and add to the state: // _state.position.x += _navLoopInterval * vel_B_N_LV[0]; _state.position.y += _navLoopInterval * vel_B_N_LV[1]; // update latitude & longitude values here NavUtils::utmToGeo(_state.position.x, _state.position.y, _utmZone, &_state.position.latitude, &_state.position.longitude); dprintf(" Navigation - Deadreckoning with DVL.\n"); dprintf(" Navigation - Dvl North increment = %f\n", _navLoopInterval * vel_B_N_LV[0]); dprintf(" Navigation - East increment = %f\n", _navLoopInterval * vel_B_N_LV[1]); dprintf(" Navigation - deltaT = %f\n",m_deltaT); dprintf(" Navigation - _navLoopInterval = %f\n",_navLoopInterval); dprintf(" Navigation - deltaNorm = %f\n", Vnorm(vel_B_N_LV) - Vnorm(_dvl->data.bottomTrackVelocity) ); } else { // // The Dvl update is too old. Something must be wrong. Dead reckon // using prop speed. Don't reset m_lastUpdateTime here because the // measurement wasn't usable, and we didn't perform a dead reckoning // step with the Dvl data. // Syslog::write(" Navigation:: deltaT = %.2f is impermissible.\n", m_deltaT); deadReckon(); } // if( m_deltaT ... ) } else //if( _dvl->valid() ) { // // The Dvl is not valid. Here, deadReckon() adds the position change // since the last (5Hz) sampling period to the state. Beware that the // code below will execute if the Dvl is alive and well, but didn't get a // good water OR bottom-track update on the last ping. // Boolean debug = False; dprintf(" Navigation:: Deadreckon because Dvl is invalid.\n"); deadReckon(); m_lastUpdateTime = _currentTime; } // if( _dvl->valid() ) //if we are using the kearfott, just overwrite the relevant fields //with its state vector dvl_deltaT = _currentTime - dvl_lastUpdateTime; if( _dvl->newData() ) dvl_lastUpdateTime = _currentTime; if (dvl_deltaT > 30.0) { Syslog::write(" Navigation:: deltaT = %.2f, aborting\n", dvl_deltaT); initiateAbort(); } if (_useIns) { if ( _ins->valid() ) { Boolean debug = False; // // Update position from INS fix // dprintf("Navigation:: - using INS reading..."); _state.position.latitude = _ins->inertialState.lat; _state.position.longitude = _ins->inertialState.lon; NavUtils::geoToUtm(_state.position.latitude, _state.position.longitude, _utmZone, &_state.position.x, &_state.position.y); _northing = _state.position.x; _easting = _state.position.y; _state.position.xIns = _state.position.x; _state.position.yIns = _state.position.y; dprintf("ins northing = %f, GPS easting = %f",_northing,_easting); dprintf("ins latitude = %f, GPS longitude = %f", Math::radToDeg(_state.position.latitude), Math::radToDeg(_state.position.longitude)); //update lbl position estimate if (Lbl_up){ _lbl->lbl_params.east_init = _state.position.y; _lbl->lbl_params.north_init = _state.position.x; reset = 1; } //update AHRS data _state.attitude.roll = _ins->inertialState.roll; _state.attitude.pitch = _ins->inertialState.pitch; _state.attitude.yaw = _ins->inertialState.yaw; // The following three rates are really angular velocity components, // not derivatives of the Euler angles. _state.attitude.omega_B_x = _ins->inertialState.rollRate; _state.attitude.omega_B_y = _ins->inertialState.pitchRate; _state.attitude.omega_B_z = _ins->inertialState.yawRate; // // 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. _state.position.xRate = _ins->inertialState.vbodyx; _state.position.yRate = _ins->inertialState.vbodyy; _state.position.zRate = _ins->inertialState.vbodyz; _state.position.depthRate = _ins->inertialState.vbodyz; //abort the mission via an exit if the INS is not in a valid state // if (_ins->inertialState.status != 0x06 && // _ins->inertialState.status != 0x09) { // Syslog::write("Navigation - Oops, the INS is in a bad state %2x\n", // _ins->inertialState.status); // exit(-1); // } } //if ( _ins->valid() ) } //if (_useIns) if( _terrainAidIF ) { _terrainAidIF->get( &_terrainNavData ); if( _terrainNavData.time != _lastTerNavTime ) { // // There is new data from TerrainAid. Syslog::write("Navigation:: TerrainNav has new data at %.2f. " "Valid = %d\n", _currentTime-_navStartTime, _terrainNavData.valid ); _lastTerNavTime = _terrainNavData.time; if( _terrainNavData.valid ) { _dx = _terrainNavData.x - _state.position.x; _dy = _terrainNavData.y - _state.position.y; Syslog::write("Navigation:: TerrainNav correction at %.2f:\n" "\tdNorthing = %.2f dEasting = %.2f", _currentTime-_navStartTime, _dx, _dy); } } _state.position.x += _dx; _state.position.y += _dy; _northing = _state.position.x; _easting = _state.position.y; } // // Compute the bearing of the transponder, if there is one. Pass the // range through. // if( _usbl->valid() ) { _state.position.usblValid = True; // // Set New Data flag if the data is new. // if( _usbl->_sampleTime.seconds > _lastUsblTime ) _state.position.usblNewData = True; else _state.position.usblNewData = False; _lastUsblTime = _usbl->_sampleTime.seconds; _state.position.usblsec = _usbl->_sampleTime.seconds; _state.position.usblSlantRange = _usbl->_rangeMag; double rx = _usbl->_rangeB[0]; double ry = _usbl->_rangeB[1]; _state.position.usblHorizRange = sqrt( rx*rx + ry*ry ); // // Compute the angle to the transponder with respect to the vehicle // frame. The Usbl can't see behind it, and also can't see less than a // meter or so ahead so we must have rx > 1.0 meters for valid data. // if( rx > 1.0 ) _state.position.usblBearing = atan( ry/rx ); } else if( _usbl->_rangeGood ) { _state.position.usblSlantRange = _usbl->_rangeMag; _state.position.usblValid = False; } else _state.position.usblValid = False; // Make data available to server and trigger NavigationIF::NewOutput event _output->write(&_state); triggerEvent(NavigationIF::NewOutput); // Write data to log m_dvlValid = _dvl->valid(); m_dvlNewData = _dvl->newData(); _log->write(); // // Pass the time that was logged out of the idl interface. TimeIF::TimeSpec *timeptr; timeptr = _log->getTimeSpec(); _state.position.updateTime.seconds = timeptr->seconds; _state.position.updateTime.nanoSeconds = timeptr->nanoSeconds; _state.attitude.updateTime.seconds = timeptr->seconds; _state.attitude.updateTime.nanoSeconds = timeptr->nanoSeconds; } void Navigation::deadReckon() { Boolean debug = False; // // Dead Reckoning Computation: // double ds, dn, de; dprintf("Navigation::deadReckon() - waterSpeed=%.3f, dt = %f", _waterSpeed,_navLoopInterval); ds = _navLoopInterval * _waterSpeed * cos( _state.attitude.pitch ); dn = ds * cos( _state.attitude.yaw ); de = ds * sin( _state.attitude.yaw ); _state.position.x += dn; _state.position.y += de; // update latitude & longitude values here NavUtils::utmToGeo(_state.position.x, _state.position.y, _utmZone, &_state.position.latitude, &_state.position.longitude); dprintf("Navigation::deadreckon() - x = %f, y = %f, dn = %f, de = %f", _state.position.x, _state.position.y,dn,de); } void Navigation::lblPosition(int reset) { double good_tof[MAX_BEACON_NUMBER]; Boolean debug = False; static int callctr = 0; _nfix = 0.0; _efix = 0.0; for (int i = 1; i <= _lbl->lbl_params.number_of_beacon; i++) good_tof[i] = 0.0; callctr++; if( _lbl->lbl_params.ping_time != 0.0 ) callctr = 0; dprintf(" Navigation::lblPosition - %d calls to j. p. since last ping.\n", callctr); jerome_position(reset,&_lbl->lbl_params,_currentTime,_navLoopInterval, _state.attitude.pitch,_state.attitude.yaw, _state.position.z,_waterSpeed,&_nfix,&_efix, good_tof,&_filter_north,&_filter_east,&_filter_depth, &_north_current,&_east_current,&_speed_bias,&_heading_bias); if( (_nfix != 0.0) || (_efix != 0.0) ) { dprintf("lblPosition() - x = %.2f, y = %.2f \n" "north fix = %.2f, east fix = %.2f \n" "north current = %.2f east current = %.2f\n" "Time = %.2f\n", _filter_north, _filter_east,_nfix,_efix, _north_current, _east_current, _currentTime); } } void Navigation::initiateAbort() { LayeredControlIF *layeredControl; try { LayeredControlIF *layeredControl = new LayeredControlIF("layeredControl"); if (layeredControl) { if (!layeredControl->abortingMission()) { layeredControl->abortMission(); Syslog::write("Navigation - aborting mission\n"); } delete layeredControl; } } catch (...) { Syslog::write("Navigation : couldn't abort mission, exiting\n"); exit(1); } // Just exit. Should really request abort from Supervisor/LayeredControl? //exit(1); }