/** \file * * Contains the Rowe_600LCM class implementation. * * Copyright (c) 2014 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include "Rowe_600LCM.h" #include "Rowe_600LCMIF.h" #include "data/ConfigReader.h" #include "data/SimSlate.h" #include "data/UniversalDataReader.h" #include "data/UniversalDataWriter.h" #include "data/LcmInstance.h" #include "units/Units.h" #include #include #include // std::min #include "lrauv-lcmtypes/marine-sensors/marine_sensors/bottom_track.hpp" #define DVL_ROWE_ACCURACY (0.001) Rowe_600LCM::Rowe_600LCM( const Module* module ) : AsyncComponent( Rowe_600LCMIF::NAME, module ), debug_( false ), loadAtStartup_( true ), newBTData_( false ), newWTData_( false ), newDVLData_( false ), loadControl_( Rowe_600LCMIF::LOAD_CONTROL, !simulateHardware(), logger_, this ), powerOnTimeout_( 4.0 ), dvlDataTimeout_( 10.0 ), pausePeriod_( 0.4 ), dataTimestamp_( Timestamp::NOT_SET_TIME ), sampleTime_( 120.0 ), // TODO: reduce this if possible -- it would be better to fail sooner maxSpeedCfg_( 2.0 ), bottomTrackVelocityAccuracyCfg_( 0.002 ), waterTrackVelocityAccuracyCfg_( 0.004 ), altitudeAccuracyCfg_( 0.01 ), altitude_( nanf( "" ) ), beam1Range_( nanf( "" ) ), beam2Range_( nanf( "" ) ), beam3Range_( nanf( "" ) ), beam4Range_( nanf( "" ) ), veloInstX_( nanf( "" ) ), veloInstY_( nanf( "" ) ), veloInstZ_( nanf( "" ) ), wVeloInstX_( nanf( "" ) ), wVeloInstY_( nanf( "" ) ), wVeloInstZ_( nanf( "" ) ), velocityRelativeToGroundInVehicleFrame_( nanf( "" ) ), velocityRelativeToWaterInVehicleFrame_( nanf( "" ) ) { float initAccuracy = 1e13; // Universal outputs altitudeWriter_ = newUniversalWriter( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR, Units::METER, initAccuracy ); xVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_X_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); yVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Y_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); zVelWrtGroundWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Z_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); xVelWrtSeaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_X_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); yVelWrtSeaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Y_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); zVelWrtSeaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_Z_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); velocityWrtGroundWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_VELOCITY_WRT_GROUND, Units::METER_PER_SECOND, DVL_ROWE_ACCURACY ); velocityWrtWaterWriter_ = newUniversalBlobWriter( UniversalURI::PLATFORM_VELOCITY_WRT_SEA_WATER, Units::METER_PER_SECOND, 2 * DVL_ROWE_ACCURACY ); alt1Writer_ = newDataWriter( Rowe_600LCMIF::ALTITUDE1_READING ); alt2Writer_ = newDataWriter( Rowe_600LCMIF::ALTITUDE2_READING ); alt3Writer_ = newDataWriter( Rowe_600LCMIF::ALTITUDE3_READING ); alt4Writer_ = newDataWriter( Rowe_600LCMIF::ALTITUDE4_READING ); // Configuration inputs maxSpeedCfgReader_ = newConfigReader( Rowe_600LCMIF::MAX_SPEED_CFG ); bottomLcmChannelCfgReader_ = newConfigReader( Rowe_600LCMIF::BOTTOM_LCM_CHAN_NAME_CFG ); waterLcmChannelCfgReader_ = newConfigReader( Rowe_600LCMIF::WATER_LCM_CHAN_NAME_CFG ); dvlLcmChannelCfgReader_ = newConfigReader( Rowe_600LCMIF::DVL_LCM_CHAN_NAME_CFG ); lcmApplicationCfgReader_ = newConfigReader( Rowe_600LCMIF::LCM_APP_NAME_CFG ); bottomTrackVelocityAccuracyCfgReader_ = newConfigReader( Rowe_600LCMIF::BOTTOM_TRACK_VELOCITY_ACCURACY_CFG ); altitudeAccuracyCfgReader_ = newConfigReader( Rowe_600LCMIF::ALTITUDE_ACCURACY_CFG ); waterTrackVelocityAccuracyCfgReader_ = newConfigReader( Rowe_600LCMIF::WATER_TRACK_VELOCITY_ACCURACY_CFG ); if( simulateHardware() ) altitude_ = 30; // start with nonzero altitude so that it doesn't check every cycle at start of sim // This configures the advanced run modes. setRunState( START ); this->setAllowableFailures( 3 ); // TODO: Should this be a configuration variable? this->setRetryTimeout( 150 ); // TODO: These are set elsewhere as well... this->setFailureMissionCritical( false ); } Rowe_600LCM::~Rowe_600LCM() {} Component::RunState Rowe_600LCM::start() { if( debug_ ) logger_.syslog( "start", Syslog::INFO ); logger_.syslog( "Initializing", Syslog::INFO ); startTime_ = Timestamp::Now(); // Set the time this->setAllowableFailures( 5 ); this->setRetryTimeout( 600 ); if( !readConfig() ) { logger_.syslog( "Error loading configuration parameters during Rowe_600LCM::start()", Syslog::CRITICAL ); this->setFailure( FailureMode::DATA ); return STOP; } if( !isDataRequested() ) { return STOP; } if( debug_ ) logger_.syslog( "Checking LCM", Syslog::INFO ); if( !LcmInstance::IsValid() ) { logger_.syslog( "LCM not connected.", Syslog::FAULT ); this->setFailure( FailureMode::SOFTWARE ); return START; } else if( debug_ ) { logger_.syslog( "LCM OK", Syslog::INFO ); } dataTimestamp_ = Timestamp::Now(); if( !simulateHardware() ) { logger_.syslog( "Powering up", Syslog::INFO ); if( !loadControl_.powerUp() ) { logger_.syslog( "Failed to power up", Syslog::FAULT ); this->setFailure( FailureMode::HARDWARE ); return START; } } pausePeriod_.sleepFor(); return STARTING; } Component::RunState Rowe_600LCM::starting() { if( debug_ ) logger_.syslog( "starting", Syslog::INFO ); if( !simulateHardware() && ( startTime_.elapsed() < powerOnTimeout_ ) ) { pausePeriod_.sleepFor(); return STARTING; // give it some more time to power up } // Stop any previous instance of the Rowe interface. Intentionally ignoring return value. system( "killall roweadcp" ); logger_.syslog( "Stopping potential previous instance(s) of Rowe LCM interface", Syslog::INFO ); // Start the stand alone LCM interface application and pipe any output to null Str appCmd = lcmApplication_.asString() + " " + uartName_.asString() + " -b " + Str( baudInt_ ) + " >& /dev/null &"; if( ( system( appCmd.cStr() ) == 0 ) && ( LcmInstance::IsValid() ) ) { logger_.syslog( "Started Rowe LCM interface with command:" + appCmd, Syslog::INFO ); LcmInstance::GetInstance()->subscribe( lcmChannelBottom_.asString().cStr(), &Rowe_600LCM::handleBottomTrackMessage, this ); if( debug_ ) logger_.syslog( "LCM subscribed to channel:" + lcmChannelBottom_.asString(), Syslog::INFO ); LcmInstance::GetInstance()->subscribe( lcmChannelWater_.asString().cStr(), &Rowe_600LCM::handleWaterSpeedMessage, this ); if( debug_ ) logger_.syslog( "LCM subscribed to channel:" + lcmChannelWater_.asString(), Syslog::INFO ); LcmInstance::GetInstance()->subscribe( lcmChannelDVL_.asString().cStr(), &Rowe_600LCM::handleDVLMessage, this ); if( debug_ ) logger_.syslog( "LCM subscribed to channel:" + lcmChannelDVL_.asString(), Syslog::INFO ); } else { logger_.syslog( "Failed to start Rowe LCM interface.", Syslog::FAULT ); this->setFailure( FailureMode::HARDWARE ); return STOP; } pausePeriod_.sleepFor(); return RUNNABLE; } /// Pause for a short period (indicated by pauseTime) Component::RunState Rowe_600LCM::pause() { if( debug_ ) logger_.syslog( "pause", Syslog::INFO ); return STOP; // State not used } /// Should eventually follow a PAUSE request: should set continueTime Component::RunState Rowe_600LCM::paused() { if( debug_ ) logger_.syslog( "paused", Syslog::INFO ); return STOP; // State not used } Component::RunState Rowe_600LCM::resume() { if( debug_ ) logger_.syslog( "resume", Syslog::INFO ); return STOP; // State not used } Component::RunState Rowe_600LCM::resuming() { if( debug_ ) logger_.syslog( "resuming", Syslog::INFO ); return STOP; // State not used } Component::RunState Rowe_600LCM::runnable() { if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO ); Slate::ReadOnce( Rowe_600LCMIF::LOAD_AT_STARTUP, Units::BOOL, loadAtStartup_, logger_ ); if( !isDataRequested() || !loadAtStartup_ ) { if( !loadAtStartup_ ) { logger_.syslog( "Stopping now due to load at startup. No immediate restart required.", Syslog::IMPORTANT ); } return STOP; } // If not simulating, check the load control board. if( !simulateHardware() ) { // Log voltage and current and check for any faults loadControl_.requestVoltageAndCurrent(); if( loadControl_.hasError() ) { // Put anything that isn't an actual fault first if( loadControl_.errorString().find( "Software Overcurrent" ) ) { // TODO: Run a short mission to determine the offending subsystems. if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR ); // TODO: LCB firmware should be updated with software overcurrent values } // And things that set failures second else { logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT ); this->setFailure( FailureMode::HARDWARE ); return STOP; } } } // grab messages (if any) here. if( LcmInstance::IsValid() ) LcmInstance::GetInstance()->handleTimeout( pausePeriod_.asMillis() ); // Process the water track data if( newWTData_ ) // Don't bother looking if there isn't any new data { // nan means that the water track data is invalid. if( isnan( wVeloInstX_ ) || isnan( wVeloInstY_ ) || isnan( wVeloInstZ_ ) ) { setWaterWritersInvalid( true ); } else { // Got data setWaterWritersInvalid( false ); // Populate vector and flip sign on water data (water track is opposite of bottom track) velocityRelativeToWaterInVehicleFrame_.setU( -wVeloInstX_ ); velocityRelativeToWaterInVehicleFrame_.setV( -wVeloInstY_ ); velocityRelativeToWaterInVehicleFrame_.setW( -wVeloInstZ_ ); //logger_.syslog( "Valid WT Data.\nX:" + Str( -wVeloInstX_ ) + "\nY:" + Str( -wVeloInstY_ ) + "\nZ:" + Str( -wVeloInstZ_ ), Syslog::IMPORTANT ); // *** DEBUG } } // Process the bottom track data if( newBTData_ ) // Don't bother looking if there isn't any new data { // All nans mean that we don't have an altitude or speed over ground if( isnan( beam1Range_ ) && isnan( beam2Range_ ) && isnan( beam3Range_ ) && isnan( beam4Range_ ) ) { setAltitudeWritersInvalid( true ); setSOGWritersInvalid( true ); altitude_ = nanf( "" ); } // If any speed over ground is nan, we don't have a valid speed else if( isnan( veloInstX_ ) || isnan( veloInstY_ ) || isnan( veloInstZ_ ) ) { setSOGWritersInvalid( true ); } // If we got here, we have at least one beam of altitude and speed over ground else { setAltitudeWritersInvalid( false ); setSOGWritersInvalid( false ); velocityRelativeToGroundInVehicleFrame_.setU( veloInstX_ ); velocityRelativeToGroundInVehicleFrame_.setV( veloInstY_ ); velocityRelativeToGroundInVehicleFrame_.setW( veloInstZ_ ); //Calculate the altitude as minimum value. Would like to use min but we have to check for nan altitude_ = 1000; if( !isnan( beam1Range_ ) && beam1Range_ < altitude_ ) altitude_ = beam1Range_; if( !isnan( beam2Range_ ) && beam2Range_ < altitude_ ) altitude_ = beam2Range_; if( !isnan( beam3Range_ ) && beam3Range_ < altitude_ ) altitude_ = beam3Range_; if( !isnan( beam4Range_ ) && beam4Range_ < altitude_ ) altitude_ = beam4Range_; //logger_.syslog( "Valid BT Data. Alt:" + Str( altitude_ ), Syslog::IMPORTANT ); // *** DEBUG //logger_.syslog( "Beam1:" + Str( beam1Range_ ) + "\nBeam2:" + Str( beam2Range_ ) + "\nBeam3:" + Str( beam3Range_ ) + "\nBeam4:" + Str( beam4Range_ ), Syslog::IMPORTANT ); // *** DEBUG } } // Process DVL data if( newDVLData_ ) { bool dataValid = false; // If any speed over ground is nan, we don't have a valid speed if( isnan( veloInstX_ ) || isnan( veloInstY_ ) || isnan( veloInstZ_ ) ) { setSOGWritersInvalid( true ); } else { setSOGWritersInvalid( false ); // Data is valid dataValid = true; velocityRelativeToGroundInVehicleFrame_.setU( veloInstX_ ); velocityRelativeToGroundInVehicleFrame_.setV( veloInstY_ ); velocityRelativeToGroundInVehicleFrame_.setW( veloInstZ_ ); } // Set altitude writer invalid if nan if( isnan( altitude_ ) ) { newDVLData_ = false; // Give the DVL a little time to come up with a good value so universals are always overwritten by NavChartDB if( dataTimestamp_.elapsed() > dvlDataTimeout_ ) { setAltitudeWritersInvalid( true ); } } else { setAltitudeWritersInvalid( false ); // Data is valid dataValid = true; } // nan means that the water track data is invalid. if( isnan( wVeloInstX_ ) || isnan( wVeloInstY_ ) || isnan( wVeloInstZ_ ) ) { setWaterWritersInvalid( true ); } else { setWaterWritersInvalid( false ); dataValid = true; // Populate vector and flip sign on water data (water track is opposite of bottom track) velocityRelativeToWaterInVehicleFrame_.setU( -wVeloInstX_ ); velocityRelativeToWaterInVehicleFrame_.setV( -wVeloInstY_ ); velocityRelativeToWaterInVehicleFrame_.setW( -wVeloInstZ_ ); } if( dataValid ) { // Data timestamp is set by message handler // dataTimestamp_ = Timestamp::Now(); } } // Write any data if it's available if( newBTData_ || newWTData_ || newDVLData_ ) { newBTData_ = false; newWTData_ = false; newDVLData_ = false; writeData(); } // As long as data is flowing we're okay. If not, take action. if( dataTimestamp_.elapsed() > sampleTime_ ) { logger_.syslog( "Did not receive valid device response within the specified allowable sample time.", Syslog::FAULT ); this->setFailure( FailureMode::COMMUNICATIONS ); // You could argue that this is a data failure. return STOP; } // Do nothing. You are still within the allowable time for a valid response. pausePeriod_.sleepFor(); return RUNNABLE; } Component::RunState Rowe_600LCM::stop() { if( debug_ ) logger_.syslog( "stop", Syslog::INFO ); uninitialize(); pausePeriod_.sleepFor(); return STOPPING; } Component::RunState Rowe_600LCM::stopping() { if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO ); if( !simulateHardware() ) { loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize if( loadControl_.hasError() ) { logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT ); this->setFailure( FailureMode::HARDWARE ); } } return STOPPED; } Component::RunState Rowe_600LCM::stopped() { if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO ); Slate::ReadOnce( Rowe_600LCMIF::LOAD_AT_STARTUP, Units::BOOL, loadAtStartup_, logger_ ); if( isDataRequested() && loadAtStartup_ ) { if( debug_ )logger_.syslog( "Data requested. STOPPED ==> START", Syslog::INFO ); return START; } pausePeriod_.sleepFor(); return STOPPED; } bool Rowe_600LCM::readConfig( void ) { if( debug_ ) logger_.syslog( "in readConfig", Syslog::INFO ); bool ok = true; ok &= maxSpeedCfgReader_->read( Units::METER_PER_SECOND, maxSpeedCfg_ ); ok &= bottomTrackVelocityAccuracyCfgReader_->read( Units::METER_PER_SECOND, bottomTrackVelocityAccuracyCfg_ ); ok &= waterTrackVelocityAccuracyCfgReader_->read( Units::METER_PER_SECOND, waterTrackVelocityAccuracyCfg_ ); ok &= altitudeAccuracyCfgReader_->read( Units::METER, altitudeAccuracyCfg_ ); ok &= bottomLcmChannelCfgReader_->read( lcmChannelBottom_ ); ok &= waterLcmChannelCfgReader_->read( lcmChannelWater_ ); ok &= dvlLcmChannelCfgReader_->read( lcmChannelDVL_ ); ok &= lcmApplicationCfgReader_->read( lcmApplication_ ); ok &= Slate::ReadOnce( Rowe_600LCMIF::UART, uartName_, logger_ ); ok &= Slate::ReadOnce( Rowe_600LCMIF::BAUD, Units::BIT_PER_SECOND, baudInt_, logger_ ); return ok; } void Rowe_600LCM::uninitialize() { if( debug_ ) logger_.syslog( "uninitialize", Syslog::INFO ); // Stop any previous instance of the Rowe interface. Intentionally ignoring return value. logger_.syslog( "Stopping potential previous instance(s) of roweadcp LCM interface", Syslog::INFO ); system( "killall roweadcp" ); if( !simulateHardware() ) { logger_.syslog( "Powering down", Syslog::INFO ); if( !loadControl_.powerDown() ) { logger_.syslog( "Failed to power down", Syslog::FAULT ); this->setFailure( FailureMode::HARDWARE ); } } } /// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc ConfigURI Rowe_600LCM::getConfigURI( ConfigOption configOption ) const { return configOption == CONFIG_SIMULATE_HARDWARE ? Rowe_600LCMIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI; } bool Rowe_600LCM::isDataRequested() { // 2016-01-29, mjs: altitudeWriter_->isDataRequested() is returning false // even when AltitudeEnvelope is active, or when a mission explicitly // requests Rowe_600LCM.height_above_sea_floor via . // // See Issue #11 on bitbucket. // // For the time being, if the component is enabled (loadAtStartup is true) // then we assume data is requested. return true; /* return altitudeWriter_->isDataRequested() || velocityWrtGroundWriter_->isDataRequested() || velocityWrtWaterWriter_->isDataRequested() || verticalRangeWriter_->isDataRequested() || signalToNoiseRatioWriter_->isDataRequested() || bottomTrackAmplitudeWriter_->isDataRequested() || bottomTrackCorrelationWriter_->isDataRequested() || bottomTrackBeamVelocityWriter_->isDataRequested() || bottomTrackInstrumentVelocityWriter_->isDataRequested() || ensembleNumberWriter_->isDataRequested() || payloadSizeWriter_->isDataRequested() || beamVelocityWriter_->isDataRequested() || instrumentVelocityWriter_->isDataRequested() || amplitudeWriter_->isDataRequested() || correlationWriter_->isDataRequested(); // TODO: consider adding more writers, but it seems silly to have to add them all manually.... */ } void Rowe_600LCM::writeData( void ) { altitudeWriter_->writeWithAccuracy( Units::METER, altitude_, DVL_ROWE_ACCURACY, dataTimestamp_ ); xVelWrtGroundWriter_->write( Units::METER_PER_SECOND, veloInstX_, dataTimestamp_ ); yVelWrtGroundWriter_->write( Units::METER_PER_SECOND, veloInstY_, dataTimestamp_ ); zVelWrtGroundWriter_->write( Units::METER_PER_SECOND, veloInstZ_, dataTimestamp_ ); xVelWrtSeaWriter_->write( Units::METER_PER_SECOND, -wVeloInstX_, dataTimestamp_ ); yVelWrtSeaWriter_->write( Units::METER_PER_SECOND, -wVeloInstY_, dataTimestamp_ ); zVelWrtSeaWriter_->write( Units::METER_PER_SECOND, -wVeloInstZ_, dataTimestamp_ ); alt1Writer_->write( Units::METER, beam1Range_, dataTimestamp_ ); alt2Writer_->write( Units::METER, beam1Range_, dataTimestamp_ ); alt3Writer_->write( Units::METER, beam1Range_, dataTimestamp_ ); alt4Writer_->write( Units::METER, beam1Range_, dataTimestamp_ ); velocityWrtGroundWriter_->setAccuracy( Units::METER_PER_SECOND, bottomTrackVelocityAccuracyCfg_ ); // since writeWithAccuracy does not exist for blob writers velocityWrtGroundWriter_->write1DClass( Units::METER_PER_SECOND, velocityRelativeToGroundInVehicleFrame_, dataTimestamp_ ); velocityWrtWaterWriter_->setAccuracy( Units::METER_PER_SECOND, waterTrackVelocityAccuracyCfg_ ); // since writeWithAccuracy does not exist for blob writers velocityWrtWaterWriter_->write1DClass( Units::METER_PER_SECOND, velocityRelativeToWaterInVehicleFrame_, dataTimestamp_ ); } void Rowe_600LCM::handleBottomTrackMessage( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const marine_sensors::bottom_track* msg ) { //printf("Received message on channel \"%s\":\n", chan.c_str()); //printf(" timestamp = %lld\n", (long long)msg->epoch_usec); //printf(" valid: %s\n", (msg->valid)? "true" : "false"); //logger_.syslog( "\nTime:" + msg->epoch_usec, Syslog::IMPORTANT ); // logger_.syslog( "valid:" + Str( msg->valid ), Syslog::IMPORTANT ); // Store the incoming data beam1Range_ = msg->vertical_range[0]; beam2Range_ = msg->vertical_range[1]; beam3Range_ = msg->vertical_range[2]; beam4Range_ = msg->vertical_range[3]; veloInstX_ = msg->instr_speed[0]; veloInstY_ = msg->instr_speed[1]; veloInstZ_ = msg->instr_speed[2]; dataTimestamp_ = msg->epoch_usec / 1e6; newBTData_ = true; // for( int i = 0; i < 4; i++ ) // { // //printf(" range%d: %.1f\n", i, msg->vertical_range[i]); // // logger_.syslog( "range" + Str( i ) + ":" + Str( msg->vertical_range[i] ), Syslog::IMPORTANT ); // } } void Rowe_600LCM::handleWaterSpeedMessage( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const marine_sensors::vehicle_water_velocity* msg ) { wVeloInstX_ = msg->instr_speed[0]; wVeloInstY_ = msg->instr_speed[1]; wVeloInstZ_ = msg->instr_speed[2]; dataTimestamp_ = msg->epoch_usec / 1e6; newWTData_ = true; } void Rowe_600LCM::handleDVLMessage( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const marine_sensors::rowe_dvl* msg ) { // Grab speed through water wVeloInstX_ = msg->instrWaterSpeedMPS[0]; wVeloInstY_ = msg->instrWaterSpeedMPS[1]; wVeloInstZ_ = msg->instrWaterSpeedMPS[2]; // And speed over ground veloInstX_ = msg->instrGrndSpeedMPS[0]; veloInstY_ = msg->instrGrndSpeedMPS[1]; veloInstZ_ = msg->instrGrndSpeedMPS[2]; // Finally, altitude (there isn't individual beam data available in this mode) altitude_ = msg->altitudeM; dataTimestamp_ = msg->epoch_usec / 1e6; newDVLData_ = true; } void Rowe_600LCM::setAltitudeWritersInvalid( bool validity ) { altitudeWriter_->setInvalid( validity ); alt1Writer_->setInvalid( validity ); alt2Writer_->setInvalid( validity ); alt3Writer_->setInvalid( validity ); alt4Writer_->setInvalid( validity ); } void Rowe_600LCM::setSOGWritersInvalid( bool validity ) { xVelWrtGroundWriter_->setInvalid( validity ); yVelWrtGroundWriter_->setInvalid( validity ); zVelWrtGroundWriter_->setInvalid( validity ); } void Rowe_600LCM::setWaterWritersInvalid( bool validity ) { xVelWrtSeaWriter_->setInvalid( validity ); yVelWrtSeaWriter_->setInvalid( validity ); zVelWrtSeaWriter_->setInvalid( validity ); }