/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : NavigationLog.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include "NavigationLog.h" #include "Navigation.h" #include "TimeP.h" #include "Syslog.h" #define LOG_LBL NavigationLog::NavigationLog(Navigation *navigation) : DataLogWriter(NavigationLogName, DataLog::BinaryFormat, AutoTimeStamp) { setMnemonic("dataNav"); addField((_x = new DoubleData("mPos_x"))); addField((_y = new DoubleData("mPos_y"))); addField((_z = new DoubleData("mDepth"))); _x->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _y->setAsciiFormat("%13.2f"); addField((_gpsNorth = new DoubleData("mGpsNorth"))); addField((_gpsEast = new DoubleData("mGpsEast"))); addField((_gpsValid = new IntegerData("mGpsValid"))); _gpsNorth->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _gpsEast ->setAsciiFormat("%13.2f"); //Necessary for large UTM values. addField((_roll = new DoubleData("mPhi"))); addField((_pitch = new DoubleData("mTheta"))); addField((_yaw = new DoubleData("mPsi"))); addField((_omega_B_x = new DoubleData("mOmega_x"))); addField((_omega_B_y = new DoubleData("mOmega_y"))); addField((_omega_B_z = new DoubleData("mOmega_z"))); addField((mMAltimeterRange = new DoubleData("mPsaRange"))); addField((mMAltitude = new DoubleData("mAltitude"))); addField((multibeamAltitude = new DoubleData("multibeamAltitude"))); addField((mDvlAltitude = new DoubleData("mDvlAltitude"))); addField((mEchoAltitude = new DoubleData("mEchoAltitude"))); addField((mEchoHorizRange = new DoubleData("mEchoHorizRange"))); addField((mEchoAltitudeLast = new DoubleData("mEchoAltitudeLast"))); addField((mEchoHorizRangeLast = new DoubleData("mEchoHorizRangeLast"))); addField((mEchoNumOfPings = new IntegerData("mEchoNumOfPings"))); addField((_nadirCnts = new IntegerData("nadirCnts"))); addField((_obstacleCnts = new IntegerData("obstacleCnts"))); addField((_nGoodNadirBeams = new IntegerData("nGoodNadirBeams"))); addField((_medianNadirBeamNo = new IntegerData("medianNadirBeamNo"))); addField((_minBeamRange = new DoubleData("minBeamRange"))); addField((_minBeamCnts = new IntegerData("minBeamCnts"))); addField((_thetaB = new DoubleData("thetaB"))); addField((_lsAlt = new DoubleData("lsAlt"))); addField((_obstacleSlopeAlt = new DoubleData("obstacleSlopeAlt"))); addField((_obstacleSlopeRange = new DoubleData("obstacleSlopeRange"))); addField((mMWaterSpeed = new DoubleData("mWaterSpeed"))); addField((m_dvlValid = new IntegerData("mDvlValid"))); addField((m_dvlNewData = new IntegerData("mDvlNewData"))); addField((m_deltaT = new DoubleData("mDeltaT"))); // Setup units and longname fields for logging // _x->setUnits("Meters"); _x->setLongName("Vehicle Northing (WGS 84 Zone 10S)"); _y->setUnits("Meters"); _y->setLongName("Vehicle Easting (WGS 84 Zone 10S)"); _z->setUnits("Meters"); _z->setLongName("Vehicle Depth"); _gpsNorth->setUnits("Meters"); _gpsNorth->setLongName("Northing (WGS 84 Zone 10S) based upon GPS fix"); _gpsEast->setUnits("Meters"); _gpsEast->setLongName("Easting (WGS 84 Zone 10S) based upon GPS fix"); _gpsValid->setUnits("Unitless"); _gpsValid->setLongName("GPS fix Status code"); _roll->setUnits("Degrees"); _roll->setLongName("Vehicle roll"); _pitch->setUnits("Degrees"); _pitch->setLongName("Vehicle pitch"); _yaw->setUnits("Degrees"); _yaw->setLongName("Vehicle yaw"); _omega_B_x->setUnits("Degrees/second"); _omega_B_x->setLongName("Vehicle roll rate"); _omega_B_y->setUnits("Degrees/second"); _omega_B_y->setLongName("Vehicle pitch rate"); _omega_B_z->setUnits("Degrees/second"); _omega_B_z->setLongName("Vehicle yaw rate"); mMAltimeterRange->setUnits("Meters"); mMAltimeterRange->setLongName("Altimeter range"); mMAltitude->setUnits("Meters"); mMAltitude->setLongName("Vehicle altitude above bottom"); mMWaterSpeed->setUnits("Meters/second"); mMWaterSpeed->setLongName("Current speed based upon DVL data"); m_dvlValid->setLongName("Dvl valid flag in Navigation"); m_dvlNewData->setLongName("Navigation thinks the Dvl has new data"); m_deltaT->setUnits("Seconds"); m_deltaT->setLongName("Time between Dvl updates"); #ifdef LOG_LBL addField((_nfix = new DoubleData("nfix"))); addField((_efix = new DoubleData("efix"))); _nfix->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _efix->setAsciiFormat("%13.2f"); //Necessary for large UTM values. addField((_filter_north = new DoubleData("filter_north" ))); addField((_filter_east = new DoubleData("filter_east" ))); _nfix->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _efix->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _filter_north->setAsciiFormat("%13.2f"); //Necessary for large UTM values. _filter_east ->setAsciiFormat("%13.2f"); //Necessary for large UTM values. addField((_filter_depth = new DoubleData("filter_depth" ))); addField((_north_current = new DoubleData("north_current"))); addField((_east_current = new DoubleData("east_current" ))); addField((_speed_bias = new DoubleData("speed_bias" ))); addField((_heading_bias = new DoubleData("heading_bias" ))); // Setup units and longname fields for logging // _nfix->setUnits("Meters"); _nfix->setLongName("Northing (WGS 84 Zone 10S) based upon baseline fix"); _efix->setUnits("Meters"); _efix->setLongName("Easting (WGS 84 Zone 10S) based upon baseline fix"); _filter_north->setUnits("Meters"); _filter_north->setLongName("Kalman filter northing (WGS 84 Zone 10S)"); _filter_east->setUnits("Meters"); _filter_east->setLongName("Kalman filter easting (WGS 84 Zone 10S)"); _filter_depth->setUnits("Meters"); _filter_depth->setLongName("Kalman filter depth"); _north_current->setUnits("Meters/second"); _north_current->setLongName("Northward flowing current estimate"); _east_current->setUnits("Meters/second"); _east_current->setLongName("Eastward flowing current estimate"); _speed_bias->setUnits("Meters/second"); _speed_bias->setLongName("Speed bias based upon long baseline fixes"); _heading_bias->setUnits("Degrees"); _heading_bias->setLongName("Heading bias based upon long baseline fixes"); #endif addField((_lat = new DoubleData("latitude"))); addField((_lon = new DoubleData("longitude"))); _lat->setAsciiFormat("%5.8f"); //Necessary for large UTM values. _lon->setAsciiFormat("%5.8f"); //Necessary for large UTM values. _navigation = navigation; } NavigationLog::~NavigationLog() { delete _x; delete _y; delete _z; delete _gpsNorth; delete _gpsEast; delete _gpsValid; delete _roll; delete _pitch; delete _yaw; delete _omega_B_x; delete _omega_B_y; delete _omega_B_z; delete mMAltimeterRange; delete mMAltitude; delete multibeamAltitude; delete mDvlAltitude; delete mEchoAltitude; delete mEchoHorizRange; delete mEchoAltitudeLast; delete mEchoHorizRangeLast; delete mEchoNumOfPings; delete _nGoodNadirBeams; delete _medianNadirBeamNo; delete mMWaterSpeed; delete m_dvlValid; delete m_dvlNewData; delete m_deltaT; delete _nadirCnts; delete _obstacleCnts; delete _minBeamRange; delete _minBeamCnts; delete _thetaB; delete _lsAlt; delete _obstacleSlopeAlt; delete _obstacleSlopeRange; delete _lat; delete _lon; #ifdef LOG_LBL delete _nfix; delete _efix; delete _filter_north; delete _filter_east; delete _filter_depth; delete _north_current; delete _east_current; delete _speed_bias; delete _heading_bias; #endif } void NavigationLog::setFields() { Boolean debug = False; // Set x, y, and z values _x->setValue(_navigation->_state.position.x); _y->setValue(_navigation->_state.position.y); _z->setValue(_navigation->_state.position.z); _gpsNorth->setValue(_navigation->_northing); _gpsEast ->setValue(_navigation->_easting); _gpsValid->setValue( (int) _navigation->_gpsValid); _roll-> setValue(_navigation->_state.attitude.roll); _pitch-> setValue(_navigation->_state.attitude.pitch); _yaw-> setValue(_navigation->_state.attitude.yaw); _omega_B_x-> setValue(_navigation->_state.attitude.omega_B_x); _omega_B_y-> setValue(_navigation->_state.attitude.omega_B_y); _omega_B_z-> setValue(_navigation->_state.attitude.omega_B_z); mMAltimeterRange->setValue(_navigation->_echoSounder->range()); mMAltitude-> setValue(_navigation->_state.position.altitude); multibeamAltitude-> setValue(_navigation->_multibeamAltitude); mDvlAltitude-> setValue(_navigation->m_dvlAltitude); mEchoAltitude-> setValue(_navigation->m_echoAltitude); dprintf("Nav/Log m_echoHorizRange= %.2f at t=%d", _navigation->m_echoHorizRange, Time::milliseconds()); mEchoHorizRange-> setValue(_navigation->m_echoHorizRange); mEchoAltitudeLast-> setValue(_navigation->_rangeToObstacle.vertLast); mEchoHorizRangeLast-> setValue(_navigation->_rangeToObstacle.horizLast); mEchoNumOfPings-> setValue(_navigation->_rangeToObstacle.numberOfPings); _nGoodNadirBeams-> setValue(_navigation->_nGood); _medianNadirBeamNo-> setValue(_navigation->_medianBeamNo); mMWaterSpeed-> setValue(_navigation->_waterSpeed); m_dvlValid->setValue( _navigation->m_dvlValid); m_dvlNewData->setValue(_navigation->m_dvlNewData); m_deltaT->setValue( _navigation->m_deltaT); _lat->setValue(_navigation->_state.position.latitude); _lon->setValue(_navigation->_state.position.longitude); _nadirCnts->setValue(_navigation->_nadirCnts); _obstacleCnts->setValue(_navigation->_obstacleCnts); _minBeamRange->setValue(_navigation->_minBeamRange); _minBeamCnts-> setValue(_navigation->_minBeamCnts); _thetaB-> setValue(_navigation->_thetaB); _lsAlt-> setValue(_navigation->_lsAlt); _obstacleSlopeAlt ->setValue(_navigation->_state.position.obstacleSlopeAlt); _obstacleSlopeRange->setValue(_navigation->_state.position.obstacleSlopeRange); #ifdef LOG_LBL _nfix->setValue(_navigation->_nfix); _efix->setValue(_navigation->_efix); _filter_north-> setValue(_navigation->_filter_north); _filter_east-> setValue(_navigation->_filter_east); _filter_depth-> setValue(_navigation->_filter_depth); _north_current-> setValue(_navigation->_north_current); _east_current-> setValue(_navigation->_east_current); _speed_bias-> setValue(_navigation->_speed_bias); _heading_bias-> setValue(_navigation->_heading_bias); #endif }