DUNE::Navigation::BasicNavigation Class Reference

Detailed Description

Abstract base class for navigation tasks.

This task is responsible to gather data from sensors and compute an estimate of the position of the system.

The navigation filter uses sensor data from GPS, LBL, DVL, AHRS, IMU and Pressure sensor. The state of the system is computed by means of an extended Kalman Filter algorithm. The estimated state is stored in the EstimatedState IMC message. The filter estimates, when possible, the outside stream velocity and stores it in a EstimatedStreamVelocity IMC message. Some of the state covariance parameters are stored inside NavigationUncertainty IMC message.

Some pre-filtering is done by rejecting GPS fixes and LBL ranges as well as applying a moving average filter to the altitude DVL measurements.

Public Member Functions
	BasicNavigation (const std::string &name, Tasks::Context &ctx)
virtual	~BasicNavigation (void)
virtual void	onUpdateParameters (void)
virtual void	onEntityResolution (void)
virtual void	onResourceInitialization (void)
virtual void	onResourceRelease (void)
void	consume (const IMC::Acceleration *msg)
void	consume (const IMC::AngularVelocity *msg)
void	consume (const IMC::DataSanity *msg)
void	consume (const IMC::Distance *msg)
void	consume (const IMC::Depth *msg)
void	consume (const IMC::DepthOffset *msg)
void	consume (const IMC::EulerAngles *msg)
void	consume (const IMC::EulerAnglesDelta *msg)
void	consume (const IMC::GpsFix *msg)
void	consume (const IMC::GroundVelocity *msg)
void	consume (const IMC::LblConfig *msg)
void	consume (const IMC::LblRange *msg)
void	consume (const IMC::Rpm *msg)
void	consume (const IMC::UsblFixExtended *msg)
void	consume (const IMC::WaterVelocity *msg)
Public Member Functions inherited from DUNE::Tasks::Periodic
	Periodic (const std::string &name, Context &ctx)
virtual	~Periodic (void)
void	setFrequency (double freq)
double	getFrequency (void) const
double	getRunTime (void) const
unsigned	getRunCount (void) const
virtual void	task (void)=0
Public Member Functions inherited from DUNE::Tasks::Task
	Task (const std::string &name, Context &context)
virtual	~Task (void)
const char *	getName (void) const
const char *	getSystemName (void) const
unsigned int	getSystemId (void) const
unsigned int	getEntityId (void) const
unsigned int	resolveEntity (const std::string &label) const
std::string	resolveEntity (unsigned int id) const
DebugLevel	getDebugLevel (void) const
uint16_t	getActivationTime (void) const
uint16_t	getDeactivationTime (void) const
unsigned int	resolveSystemName (const std::string &name) const
const char *	resolveSystemId (unsigned int id) const
void	loadConfig (void)
void	setPriority (unsigned int value)
unsigned int	getPriority (void) const
void	inf (const char *format,...) DUNE_PRINTF_FORMAT(2
void void	war (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void	err (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void void	cri (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void void void	debug (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void void void void	trace (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void void void void void	spew (const char *format,...) DUNE_PRINTF_FORMAT(2
void void void void void void void void	dispatch (IMC::Message *msg, unsigned int flags=0)
void	dispatch (IMC::Message &msg, unsigned int flags=0)
void	dispatchReply (const IMC::Message &original, IMC::Message &msg, unsigned int flags=0)
void	receive (const IMC::Message *msg)
void	reserveEntities (void)
void	resolveEntities (void)
void	acquireResources (void)
void	releaseResources (void)
void	initializeResources (void)
void	updateParameters (bool act_deact=true)
void	writeParamsXML (std::ostream &os) const
const char *	getEntityLabel (void) const
void	setEntityLabel (const std::string &label)
Public Member Functions inherited from DUNE::Tasks::AbstractTask
	AbstractTask (void)
virtual	~AbstractTask (void)
Public Member Functions inherited from DUNE::Concurrency::Thread
	Thread (void)
virtual	~Thread (void)
int	getProcessorUsage (void)
Public Member Functions inherited from DUNE::Concurrency::Runnable
	Runnable (void)
virtual	~Runnable (void)
void	start (void)
void	stop (void)
void	join (void)
void	stopAndJoin (void)
void	setPriority (Scheduler::Policy policy, unsigned priority)
unsigned	getPriority (void)
State	getState (void)
bool	isCreated (void)
bool	isStopping (void)
bool	isRunning (void)
bool	isStarting (void)
bool	isDead (void)

Protected Member Functions
double	getDepth (void) const
double	getAltitude (void)
double	getAcceleration (unsigned axis) const
double	getAngularVelocity (unsigned axis) const
double	getHeadingRate (void)
double	getEuler (unsigned axis) const
double	getEulerDelta (unsigned axis) const
float	getEulerDeltaTimestep (void) const
bool	gotDepthReadings (void) const
bool	gotEulerReadings (void) const
bool	gotAngularReadings (void) const
bool	gotAccelerationReadings (void) const
unsigned	getAhrsId (void) const
double	getLblRejectionValue (double exp_range) const
bool	rejectLbl (void) const
double	getTimeStep (void)
void	updateEuler (float filter)
void	updateEulerDelta (float filter)
void	updateAngularVelocities (float filter)
void	updateAcceleration (float filter)
void	updateDepth (float filter)
virtual void	reset (void)
virtual bool	setup (void)
virtual void	onConsumeLblConfig (void)
virtual void	updateKalmanGpsParameters (double hacc)
virtual void	runKalmanGPS (double x, double y)
virtual void	runKalmanLBL (int beacon, float range, double dx, double dy, double exp_range)
virtual void	runKalmanDVL (void)
virtual void	runKalmanUSBL (double x, double y)
virtual void	getSpeedOutputStates (unsigned *u, unsigned *v)=0
virtual unsigned	getNumberOutputs (void)=0
void	onDispatchNavigation (void)
bool	isActive (void)
void	reportToBus (void)
void	updateBuffers (float filter)
void	resetAcceleration (void)
void	resetAngularVelocity (void)
void	resetDepth (void)
void	resetEulerAngles (void)
void	resetEulerAnglesDelta (void)
void	checkUncertainty (bool abort=true)
void	checkDeclination (double lat, double lon, double height)
Protected Member Functions inherited from DUNE::Tasks::Task
void	setEntityState (IMC::EntityState::StateEnum state, Status::Code code)
void	setEntityState (IMC::EntityState::StateEnum state, const std::string &description)
IMC::EntityState::StateEnum	getEntityState (void) const
unsigned int	reserveEntity (const std::string &label)
template<typename E >
E *	reserveEntity (const std::string &label)
Entities::BasicEntity *	getLocalEntity (const std::string &label)
bool	stopping (void)
bool	isActive (void) const
bool	isActivating (void) const
bool	isDeactivating (void) const
void	waitForMessages (double timeout)
void	consumeMessages (void)
template<typename T >
Parameter &	param (const std::string &name, T &var)
template<typename Y , typename T >
Parameter &	param (const std::string &name, T &var)
template<typename T >
bool	paramChanged (T &var)
void	paramActive (Parameter::Scope def_scope, Parameter::Visibility def_visibility, bool def_value=false)
void	setParamSectionEditor (const std::string &name)
template<typename M , typename T >
void	bind (T *task_obj, void(T::*consumer)(const M *)=&T::consume)
template<typename T >
void	bind (T *task_obj, const std::vector< uint32_t > &list)
template<typename T , typename M >
void	bind (T *task_obj, const std::vector< uint32_t > &list, void(T::*consumer)(const M *)=&T::consume)
template<typename T >
void	bind (T *task_obj, const std::vector< std::string > &list)
void	requestActivation (void)
void	requestDeactivation (void)
void	activate (void)
void	activationFailed (const std::string &reason)
void	deactivate (void)
void	deactivationFailed (const std::string &reason)
virtual bool	onWriteParamsXML (std::ostream &os) const
virtual void	onEntityReservation (void)
virtual void	onReportEntityState (void)
virtual void	onResourceAcquisition (void)
virtual void	onRequestActivation (void)
virtual void	onRequestDeactivation (void)
virtual void	onActivation (void)
virtual void	onDeactivation (void)
virtual void	onQueryEntityParameters (const IMC::QueryEntityParameters *msg)
virtual void	onSetEntityParameters (const IMC::SetEntityParameters *msg)
virtual void	onPushEntityParameters (const IMC::PushEntityParameters *msg)
virtual void	onPopEntityParameters (const IMC::PopEntityParameters *msg)
Protected Member Functions inherited from DUNE::Concurrency::Thread
void	startImpl (void)
void	stopImpl (void)
void	joinImpl (void)
void	setPriorityImpl (Scheduler::Policy policy, unsigned priority)
unsigned	getPriorityImpl (void)

Protected Attributes
Navigation::KalmanFilter	m_kal
Navigation::Ranging	m_ranging
Navigation::StreamEstimator	m_stream_filter
int16_t	m_rpm
std::vector< double >	m_process_noise
std::vector< double >	m_measure_noise
std::vector< double >	m_state_cov
IMC::EstimatedState	m_estate
IMC::LblRangeAcceptance	m_lbl_ac
IMC::GpsFixRejection	m_gps_rej
IMC::DvlRejection	m_dvl_rej
IMC::NavigationUncertainty	m_uncertainty
IMC::NavigationData	m_navdata
IMC::GroundVelocity	m_gvel
IMC::WaterVelocity	m_wvel
Time::Counter< double >	m_time_without_gps
Time::Counter< double >	m_time_without_dvl
Time::Counter< double >	m_time_without_alt
Time::Counter< double >	m_dvl_sanity_timer
Time::Counter< double >	m_time_without_main_depth
Time::Counter< double >	m_time_without_depth
Time::Counter< double >	m_time_without_euler
double	m_gps_sog
double	m_last_z
bool	m_dead_reckoning
bool	m_aligned
unsigned	m_imu_eid
float	m_lbl_threshold
double	m_heading
bool	m_valid_gv
bool	m_valid_wv
bool	m_lbl_reading
Math::Derivative< double >	m_deriv_heave
Protected Attributes inherited from DUNE::Tasks::Task
Context &	m_ctx
std::vector< Entities::BasicEntity * >	m_entities

Additional Inherited Members

Public Types inherited from DUNE::Concurrency::Runnable

Constructor & Destructor Documentation

DUNE::Navigation::BasicNavigation::BasicNavigation	(	const std::string &	name,
		Tasks::Context &	ctx
	)

Constructor.

Parameters

[in]	name	name.
[in]	ctx	context.

References DUNE::Tasks::Parameter::defaultValue(), DUNE::Tasks::Parameter::description(), DUNE::Tasks::Profiles::isSelected(), m_aligned, DUNE::Tasks::Task::m_ctx, m_dead_reckoning, m_lbl_reading, m_lbl_threshold, m_rpm, DUNE::Tasks::Task::param(), DUNE::Tasks::Context::profiles, DUNE::Tasks::Parameter::units(), DUNE::IMC::GroundVelocity::VAL_VEL_X, DUNE::IMC::WaterVelocity::VAL_VEL_X, DUNE::IMC::GroundVelocity::VAL_VEL_Y, DUNE::IMC::WaterVelocity::VAL_VEL_Y, DUNE::IMC::GroundVelocity::VAL_VEL_Z, DUNE::IMC::WaterVelocity::VAL_VEL_Z, DUNE::Tasks::Parameter::visibility(), and DUNE::Tasks::Parameter::VISIBILITY_USER.

DUNE::Navigation::BasicNavigation::~BasicNavigation ( void  )

virtual

Destructor.

Member Function Documentation

void DUNE::Navigation::BasicNavigation::checkDeclination ( double  lat, double  lon, double  height  )

protected

Routine to check current declination value using WMM.

Parameters

[in]	lat	vehicle current latitude.
[in]	lon	vehicle current longitude.
[in]	height	vehicle current height.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, DUNE::Coordinates::WMM::declination(), DUNE::Tasks::Context::dir_cfg, getEuler(), DUNE::Tasks::Task::m_ctx, DUNE::Math::Angles::normalizeRadian(), and DUNE::Math::Matrix::toDCM().

Referenced by consume(), and updateDepth().

void DUNE::Navigation::BasicNavigation::checkUncertainty ( bool  abort = true )

protected

Routine to check navigation uncertainty.

Parameters

[in]

abort

abort if position uncertainty is exceeded.

References DUNE::Status::CODE_ACTIVE, DUNE::Status::CODE_ALIGNED, DUNE::Status::CODE_NOT_ALIGNED, DUNE::IMC::EntityState::ESTA_ERROR, DUNE::IMC::EntityState::ESTA_NORMAL, DUNE::Navigation::KalmanFilter::getCovariance(), m_aligned, m_dead_reckoning, m_kal, m_time_without_depth, m_time_without_euler, DUNE::Time::Counter< T >::overflow(), DUNE::Tasks::Task::setEntityState(), DUNE::Navigation::SM_STATE_BOOT, DUNE::Navigation::SM_STATE_NORMAL, DUNE::Navigation::SM_STATE_UNSAFE, DUNE::Tasks::Task::spew(), DUNE::Navigation::STATE_X, DUNE::Navigation::STATE_Y, and DUNE::Utils::String::str().

Referenced by Navigation::General::ROV::Task::task(), and updateDepth().

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::Acceleration *

msg

)

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, DUNE::Tasks::Task::err(), DUNE::IMC::Message::getSourceEntity(), DUNE::IMC::Acceleration::x, DUNE::IMC::Acceleration::y, and DUNE::IMC::Acceleration::z.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::AngularVelocity *

msg

)

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, DUNE::IMC::Message::getSourceEntity(), DUNE::Tasks::Task::war(), DUNE::IMC::AngularVelocity::x, DUNE::IMC::AngularVelocity::y, and DUNE::IMC::AngularVelocity::z.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::DataSanity *

msg

)

References DUNE::IMC::DataSanity::DS_NOT_SANE, DUNE::IMC::Message::getSourceEntity(), m_dvl_sanity_timer, DUNE::Time::Counter< T >::reset(), and DUNE::IMC::DataSanity::sane.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::Distance *

msg

)

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::IMC::Distance::DV_INVALID, getEuler(), DUNE::IMC::Message::getSourceEntity(), m_time_without_alt, DUNE::Time::Counter< T >::reset(), DUNE::IMC::Distance::validity, and DUNE::IMC::Distance::value.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::Depth *

msg

)

References DUNE::IMC::Message::getSourceEntity(), m_time_without_depth, m_time_without_main_depth, DUNE::Time::Counter< T >::overflow(), DUNE::Time::Counter< T >::reset(), and DUNE::IMC::Depth::value.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::DepthOffset *

msg

)

References DUNE::IMC::Message::getSourceEntity(), and DUNE::IMC::DepthOffset::value.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::EulerAngles *

msg

)

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, getEuler(), DUNE::IMC::Message::getSourceEntity(), m_time_without_euler, DUNE::Math::Angles::minSignedAngle(), DUNE::IMC::EulerAngles::phi, DUNE::IMC::EulerAngles::psi, DUNE::Time::Counter< T >::reset(), DUNE::IMC::EulerAngles::theta, and DUNE::Tasks::Task::war().

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::EulerAnglesDelta *

msg

)

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, DUNE::IMC::Message::getSourceEntity(), m_imu_eid, DUNE::IMC::EulerAnglesDelta::timestep, DUNE::Tasks::Task::war(), DUNE::IMC::EulerAnglesDelta::x, DUNE::IMC::EulerAnglesDelta::y, and DUNE::IMC::EulerAnglesDelta::z.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::GpsFix *

msg

)

References DUNE::Navigation::AXIS_X, checkDeclination(), DUNE::Navigation::StreamEstimator::consume(), DUNE::IMC::CacheControl::COP_STORE, DUNE::Tasks::Task::dispatch(), DUNE::Coordinates::WGS84::displacement(), getEuler(), DUNE::IMC::Message::getTimeStamp(), DUNE::IMC::GpsFix::GFT_MANUAL_INPUT, DUNE::IMC::GpsFix::GFV_VALID_HACC, DUNE::IMC::GpsFix::GFV_VALID_POS, DUNE::IMC::GpsFix::GFV_VALID_SOG, DUNE::IMC::GpsFix::hacc, DUNE::IMC::GpsFix::hdop, DUNE::IMC::GpsFix::height, DUNE::IMC::EstimatedState::height, DUNE::IMC::GpsFix::lat, DUNE::IMC::EstimatedState::lat, DUNE::IMC::GpsFix::lon, DUNE::IMC::EstimatedState::lon, m_estate, m_gps_rej, m_gps_sog, m_kal, m_last_z, m_ranging, m_stream_filter, DUNE::Math::MovingAverage< T >::mean(), DUNE::IMC::CacheControl::message, DUNE::Math::norm(), DUNE::IMC::CacheControl::op, DUNE::IMC::GpsFixRejection::reason, DUNE::Memory::replace(), DUNE::IMC::GpsFixRejection::RR_ABOVE_MAX_HACC, DUNE::IMC::GpsFixRejection::RR_ABOVE_MAX_HDOP, DUNE::IMC::GpsFixRejection::RR_ABOVE_THRESHOLD, DUNE::IMC::GpsFixRejection::RR_INVALID, runKalmanGPS(), DUNE::Math::MovingAverage< T >::sampleSize(), DUNE::Navigation::KalmanFilter::setState(), DUNE::IMC::Message::setTimeStamp(), DUNE::IMC::GpsFix::sog, DUNE::Tasks::Task::spew(), DUNE::Navigation::STATE_X, DUNE::Navigation::STATE_Y, DUNE::IMC::GpsFix::type, DUNE::Math::MovingAverage< T >::update(), updateKalmanGpsParameters(), DUNE::Navigation::Ranging::updateOrigin(), DUNE::IMC::GpsFix::utc_time, DUNE::IMC::GpsFixRejection::utc_time, and DUNE::IMC::GpsFix::validity.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::GroundVelocity *

msg

)

References DUNE::Navigation::AXIS_Z, DUNE::Tasks::Task::dispatch(), getAngularVelocity(), DUNE::Time::Delta::getDelta(), DUNE::IMC::Message::getTimeStamp(), m_dvl_rej, m_gvel, m_time_without_dvl, m_valid_gv, DUNE::IMC::DvlRejection::reason, DUNE::Time::Counter< T >::reset(), DUNE::IMC::DvlRejection::RR_ABS_THRESHOLD_X, DUNE::IMC::DvlRejection::RR_ABS_THRESHOLD_Y, DUNE::IMC::DvlRejection::RR_INNOV_THRESHOLD_X, DUNE::IMC::DvlRejection::RR_INNOV_THRESHOLD_Y, DUNE::IMC::Message::setTimeStamp(), DUNE::IMC::DvlRejection::timestep, DUNE::IMC::DvlRejection::type, DUNE::IMC::DvlRejection::TYPE_GV, DUNE::IMC::GroundVelocity::validity, DUNE::IMC::DvlRejection::value, DUNE::IMC::GroundVelocity::x, and DUNE::IMC::GroundVelocity::y.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::LblConfig *

msg

)

References DUNE::IMC::CacheControl::COP_STORE, DUNE::Tasks::Task::dispatch(), m_ranging, DUNE::IMC::CacheControl::message, onConsumeLblConfig(), DUNE::IMC::CacheControl::op, DUNE::IMC::LblConfig::op, DUNE::IMC::LblConfig::OP_SET_CFG, and DUNE::Navigation::Ranging::setup().

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::LblRange *

msg

)

References DUNE::IMC::LblRangeAcceptance::acceptance, DUNE::Navigation::AXIS_Z, DUNE::Tasks::Task::dispatch(), DUNE::Navigation::Ranging::exists(), getDepth(), getEuler(), DUNE::Navigation::Ranging::getLocation(), DUNE::Navigation::Ranging::getSize(), DUNE::Navigation::KalmanFilter::getState(), DUNE::IMC::Message::getTimeStamp(), DUNE::IMC::LblRange::id, DUNE::IMC::LblRangeAcceptance::id, m_kal, m_lbl_ac, m_ranging, m_time_without_gps, DUNE::Time::Counter< T >::overflow(), DUNE::IMC::LblRange::range, DUNE::IMC::LblRangeAcceptance::range, rejectLbl(), DUNE::IMC::LblRangeAcceptance::RR_AT_SURFACE, DUNE::IMC::LblRangeAcceptance::RR_NO_INFO, runKalmanLBL(), DUNE::IMC::Message::setTimeStamp(), DUNE::Navigation::STATE_X, and DUNE::Navigation::STATE_Y.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::Rpm *

msg

)

References DUNE::Navigation::StreamEstimator::consume(), m_rpm, m_stream_filter, and DUNE::IMC::Rpm::value.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::UsblFixExtended *

msg

)

References DUNE::Coordinates::WGS84::displacement(), DUNE::Tasks::Task::getSystemName(), DUNE::IMC::GpsFix::lat, DUNE::IMC::UsblFixExtended::lat, DUNE::IMC::GpsFix::lon, DUNE::IMC::UsblFixExtended::lon, runKalmanUSBL(), and DUNE::IMC::UsblFixExtended::target.

void DUNE::Navigation::BasicNavigation::consume

(

const IMC::WaterVelocity *

msg

)

References DUNE::Navigation::AXIS_Z, DUNE::Status::CODE_WAIT_CONVERGE, DUNE::IMC::CacheControl::COP_STORE, DUNE::Tasks::Task::dispatch(), DUNE::IMC::EntityState::ESTA_BOOT, getAngularVelocity(), DUNE::Time::Delta::getDelta(), DUNE::IMC::Message::getTimeStamp(), m_dvl_rej, m_time_without_dvl, m_valid_wv, m_wvel, DUNE::IMC::CacheControl::message, DUNE::IMC::CacheControl::op, DUNE::IMC::DvlRejection::reason, DUNE::Memory::replace(), DUNE::Time::Counter< T >::reset(), DUNE::IMC::DvlRejection::RR_ABS_THRESHOLD_X, DUNE::IMC::DvlRejection::RR_ABS_THRESHOLD_Y, DUNE::IMC::DvlRejection::RR_INNOV_THRESHOLD_X, DUNE::IMC::DvlRejection::RR_INNOV_THRESHOLD_Y, DUNE::Tasks::Task::setEntityState(), DUNE::IMC::Message::setTimeStamp(), setup(), DUNE::Navigation::SM_STATE_BOOT, DUNE::IMC::DvlRejection::timestep, DUNE::IMC::DvlRejection::type, DUNE::IMC::DvlRejection::TYPE_WV, DUNE::IMC::WaterVelocity::validity, DUNE::IMC::DvlRejection::value, DUNE::IMC::WaterVelocity::x, and DUNE::IMC::WaterVelocity::y.

double DUNE::Navigation::BasicNavigation::getAcceleration ( unsigned  axis ) const

inlineprotected

Get the acceleration along a specific axis.

Returns

acceleration value.

Referenced by updateAcceleration().

unsigned DUNE::Navigation::BasicNavigation::getAhrsId ( void  ) const

inlineprotected

Get AHRS Entity Id.

Returns

AHRS entity id.

double DUNE::Navigation::BasicNavigation::getAltitude ( void  )

inlineprotected

Get vehicle altitude.

Negative value denotes invalid estimate.

Returns

altitude value.

References m_dvl_sanity_timer, m_time_without_alt, and DUNE::Time::Counter< T >::overflow().

Referenced by isActive(), and onDispatchNavigation().

double DUNE::Navigation::BasicNavigation::getAngularVelocity ( unsigned  axis ) const

inlineprotected

Get angular velocity value along a specific axis.

Returns

angular velocity value.

Referenced by consume(), getHeadingRate(), isActive(), Navigation::General::ROV::Task::logData(), onDispatchNavigation(), and updateAngularVelocities().

double DUNE::Navigation::BasicNavigation::getDepth ( void  ) const

inlineprotected

Get depth value.

Returns

depth value.

Referenced by consume(), isActive(), onDispatchNavigation(), and updateDepth().

double DUNE::Navigation::BasicNavigation::getEuler ( unsigned  axis ) const

inlineprotected

Get Euler Angle value.

Returns

euler angle value.

Referenced by checkDeclination(), consume(), getHeadingRate(), isActive(), Navigation::General::ROV::Task::logData(), onDispatchNavigation(), Navigation::General::ROV::Task::setTransition(), and updateEuler().

double DUNE::Navigation::BasicNavigation::getEulerDelta ( unsigned  axis ) const

inlineprotected

Get Euler Angles increment value along a specific axis.

Returns

euler angles increment value

Referenced by getHeadingRate(), and updateEulerDelta().

float DUNE::Navigation::BasicNavigation::getEulerDeltaTimestep ( void  ) const

inlineprotected

Get Euler Angles increment value along a specific axis.

Returns

euler angles increment value

Referenced by getHeadingRate().

double DUNE::Navigation::BasicNavigation::getHeadingRate ( void  )

inlineprotected

Get heading rate value.

Returns

heading rate.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, getAngularVelocity(), getEuler(), getEulerDelta(), getEulerDeltaTimestep(), and m_dead_reckoning.

double DUNE::Navigation::BasicNavigation::getLblRejectionValue ( double  exp_range ) const

inlineprotected

Routine to compute LBL rejection value.

Parameters

[in]

exp_range

expected LBL range value.

Returns

LBL rejection value.

Referenced by runKalmanLBL().

virtual unsigned DUNE::Navigation::BasicNavigation::getNumberOutputs ( void  )

protectedpure virtual

Get number of EKF outputs.

Returns

number of outputs.

Implemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

Referenced by runKalmanLBL(), and updateDepth().

virtual void DUNE::Navigation::BasicNavigation::getSpeedOutputStates ( unsigned *  u, unsigned *  v  )

protectedpure virtual

Get EKF output matrix speed indexes.

Parameters

[out]	u	forward speed state index.
[out]	v	transversal speed state index.

Implemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

Referenced by runKalmanDVL(), and updateDepth().

double DUNE::Navigation::BasicNavigation::getTimeStep ( void  )

inlineprotected

Routine to get navigation time step.

Returns

time step.

References DUNE::Time::Delta::getDelta().

Referenced by Navigation::General::ROV::Task::task().

bool DUNE::Navigation::BasicNavigation::gotAccelerationReadings ( void  ) const

inlineprotected

Number of acceleration readings since last cycle plus constant filter gain.

Returns

true is received, false otherwise.

bool DUNE::Navigation::BasicNavigation::gotAngularReadings ( void  ) const

inlineprotected

Number of angular velocity readings since last cycle plus constant filter gain.

Returns

true is received, false otherwise.

Referenced by isActive().

bool DUNE::Navigation::BasicNavigation::gotDepthReadings ( void  ) const

inlineprotected

Number of depth readings since last cycle plus constant filter gain.

Returns

true is received, false otherwise.

bool DUNE::Navigation::BasicNavigation::gotEulerReadings ( void  ) const

inlineprotected

Number of euler angles readings since last cycle plus constant filter gain.

Returns

true is received, false otherwise.

Referenced by isActive().

bool DUNE::Navigation::BasicNavigation::isActive ( void  )

protected

Routine to check if navigation task is active.

Returns

true if active, false otherwise.

References DUNE::IMC::EstimatedState::alt, DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, DUNE::IMC::EstimatedState::depth, DUNE::Tasks::Task::dispatch(), getAltitude(), getAngularVelocity(), getDepth(), getEuler(), gotAngularReadings(), gotEulerReadings(), DUNE::IMC::EstimatedState::height, DUNE::IMC::EstimatedState::lat, DUNE::IMC::EstimatedState::lon, m_estate, m_heading, DUNE::Math::Angles::normalizeRadian(), DUNE::IMC::EstimatedState::p, DUNE::IMC::EstimatedState::phi, DUNE::IMC::EstimatedState::psi, DUNE::IMC::EstimatedState::q, DUNE::IMC::EstimatedState::r, DUNE::IMC::EstimatedState::theta, updateAngularVelocities(), updateDepth(), and updateEuler().

Referenced by updateDepth().

void DUNE::Navigation::BasicNavigation::onConsumeLblConfig ( void  )

protectedvirtual

Method invoked when LblConfig message is received.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

Referenced by consume(), and updateDepth().

void DUNE::Navigation::BasicNavigation::onDispatchNavigation ( void  )

protected

Routine called to assign common dispatch messages.

References DUNE::IMC::EstimatedState::alt, DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::IMC::NavigationData::cyaw, DUNE::IMC::EstimatedState::depth, getAltitude(), getAngularVelocity(), DUNE::Navigation::KalmanFilter::getCovariance(), getDepth(), getEuler(), DUNE::Navigation::KalmanFilter::getState(), m_deriv_heave, m_estate, m_heading, m_kal, m_last_z, m_navdata, m_uncertainty, DUNE::Math::Angles::normalizeRadian(), DUNE::IMC::EstimatedState::p, DUNE::IMC::EstimatedState::phi, DUNE::IMC::EstimatedState::psi, DUNE::IMC::EstimatedState::q, DUNE::Navigation::STATE_X, DUNE::Navigation::STATE_Y, DUNE::IMC::EstimatedState::theta, DUNE::Coordinates::BodyFixedFrame::toInertialFrame(), DUNE::IMC::EstimatedState::u, DUNE::Math::MovingAverage< T >::update(), DUNE::Math::Derivative< Type >::update(), DUNE::IMC::EstimatedState::v, DUNE::IMC::EstimatedState::vx, DUNE::IMC::EstimatedState::vy, DUNE::IMC::EstimatedState::vz, DUNE::IMC::EstimatedState::w, DUNE::IMC::EstimatedState::x, DUNE::IMC::NavigationUncertainty::x, DUNE::IMC::EstimatedState::y, DUNE::IMC::NavigationUncertainty::y, and DUNE::IMC::EstimatedState::z.

Referenced by Navigation::General::ROV::Task::logData(), and updateDepth().

void DUNE::Navigation::BasicNavigation::onEntityResolution ( void  )

virtual

Resolve entities.

Reimplemented from DUNE::Tasks::Task.

Reimplemented in Navigation::AUV::Navigation::Task.

References DUNE::Tasks::Profiles::isSelected(), DUNE::Tasks::Task::m_ctx, DUNE::Tasks::Context::profiles, and DUNE::Tasks::Task::resolveEntity().

void DUNE::Navigation::BasicNavigation::onResourceInitialization ( void  )

virtual

Initialize resources.

Reimplemented from DUNE::Tasks::Task.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

References reset().

void DUNE::Navigation::BasicNavigation::onResourceRelease ( void  )

virtual

Release allocated resources.

Reimplemented from DUNE::Tasks::Task.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

References DUNE::Memory::clear().

void DUNE::Navigation::BasicNavigation::onUpdateParameters ( void  )

virtual

Update internal parameters.

Reimplemented from DUNE::Tasks::Task.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

References DUNE::Tasks::Profiles::isSelected(), DUNE::Tasks::Task::m_ctx, m_dvl_sanity_timer, m_time_without_alt, m_time_without_depth, m_time_without_dvl, m_time_without_euler, m_time_without_gps, m_time_without_main_depth, DUNE::Tasks::Context::profiles, and DUNE::Time::Counter< T >::setTop().

bool DUNE::Navigation::BasicNavigation::rejectLbl ( void  ) const

inlineprotected

This function checks if navigation must reject all lbl.

Returns

true if LBL is to be rejected, false otherwise.

Referenced by consume().

void DUNE::Navigation::BasicNavigation::reportToBus ( void  )

protected

Routine to report navigation messages to the bus.

References DUNE::Tasks::Task::dispatch(), DUNE::Time::Clock::getSinceEpoch(), m_estate, m_navdata, m_uncertainty, and DUNE::IMC::Message::setTimeStamp().

Referenced by Navigation::General::ROV::Task::task(), and updateDepth().

void DUNE::Navigation::BasicNavigation::reset ( void  )

protectedvirtual

Routine to reset navigation.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

References DUNE::Status::CODE_WAIT_GPS_FIX, DUNE::IMC::EntityState::ESTA_BOOT, m_gps_sog, m_heading, m_last_z, m_valid_gv, m_valid_wv, DUNE::Tasks::Task::setEntityState(), and DUNE::Navigation::SM_STATE_IDLE.

Referenced by onResourceInitialization(), setup(), and updateDepth().

void DUNE::Navigation::BasicNavigation::resetAcceleration ( void  )

protected

Routine to reset acceleration buffers.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, and DUNE::Navigation::AXIS_Z.

Referenced by resetEulerAnglesDelta(), and updateDepth().

void DUNE::Navigation::BasicNavigation::resetAngularVelocity ( void  )

protected

Routine to reset angular velocity buffers.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, and DUNE::Navigation::AXIS_Z.

Referenced by resetEulerAnglesDelta(), and updateDepth().

void DUNE::Navigation::BasicNavigation::resetDepth ( void  )

protected

Routine to reset depth buffers.

Referenced by resetEulerAnglesDelta(), and updateDepth().

void DUNE::Navigation::BasicNavigation::resetEulerAngles ( void  )

protected

Routine to reset euler angles buffers.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, and DUNE::Navigation::AXIS_Z.

Referenced by resetEulerAnglesDelta(), and updateDepth().

void DUNE::Navigation::BasicNavigation::resetEulerAnglesDelta ( void  )

protected

Routine to reset euler angles delta buffers.

References DUNE::Navigation::AXIS_X, DUNE::Navigation::AXIS_Y, DUNE::Navigation::AXIS_Z, resetAcceleration(), resetAngularVelocity(), resetDepth(), and resetEulerAngles().

Referenced by updateDepth().

void DUNE::Navigation::BasicNavigation::runKalmanDVL ( void  )

protectedvirtual

Routine to assign EKF filter output variables when a DVL velocity message is received.

References getSpeedOutputStates(), m_gvel, m_kal, m_valid_gv, m_valid_wv, m_wvel, DUNE::Navigation::KalmanFilter::setOutput(), DUNE::IMC::GroundVelocity::x, DUNE::IMC::WaterVelocity::x, DUNE::IMC::GroundVelocity::y, and DUNE::IMC::WaterVelocity::y.

Referenced by Navigation::General::ROV::Task::task(), and updateDepth().

void DUNE::Navigation::BasicNavigation::runKalmanGPS ( double  x, double  y  )

protectedvirtual

Routine to assign EKF filter output variables when a GpsFix message is received.

Parameters

[in]	x	vehicle north displacement (m).
[in]	y	vehicle east displacement (m).

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

Referenced by consume(), and updateDepth().

void DUNE::Navigation::BasicNavigation::runKalmanLBL ( int  beacon, float  range, double  dx, double  dy, double  exp_range  )

protectedvirtual

Routine to assign EKF filter output variables when a GpsFix message is received.

Parameters

[in]	beacon	beacon id.
[in]	range	range to beacon.
[in]	dx	distance along x-axis.
[in]	dy	distance along y-axis.
[in]	exp_range	expected range.

References DUNE::IMC::LblRangeAcceptance::acceptance, DUNE::Tasks::Task::dispatch(), DUNE::Navigation::KalmanFilter::getCovariance(), getLblRejectionValue(), getNumberOutputs(), DUNE::IMC::NavigationData::lbl_rej_level, m_kal, m_lbl_ac, m_lbl_reading, m_lbl_threshold, m_navdata, DUNE::IMC::LblRangeAcceptance::RR_ABOVE_THRESHOLD, DUNE::IMC::LblRangeAcceptance::RR_ACCEPTED, DUNE::Navigation::KalmanFilter::setInnovation(), DUNE::Navigation::KalmanFilter::setObservation(), DUNE::Navigation::KalmanFilter::setOutput(), DUNE::Navigation::STATE_X, and DUNE::Navigation::STATE_Y.

Referenced by consume(), and updateDepth().

void DUNE::Navigation::BasicNavigation::runKalmanUSBL ( double  x, double  y  )

protectedvirtual

Routine to assign EKF filter output variables when a UsblFixExtended message is received.

Parameters

[in]	x	vehicle north displacement (m).
[in]	y	vehicle east displacement (m).

Reimplemented in Navigation::AUV::Navigation::Task.

Referenced by consume(), and updateDepth().

bool DUNE::Navigation::BasicNavigation::setup ( void  )

protectedvirtual

Routine to setup navigation.

Reimplemented in Navigation::AUV::Navigation::Task, and Navigation::General::ROV::Task.

References DUNE::IMC::GpsFix::height, DUNE::IMC::EstimatedState::height, DUNE::IMC::GpsFix::lat, DUNE::IMC::EstimatedState::lat, DUNE::IMC::GpsFix::lon, DUNE::IMC::EstimatedState::lon, m_estate, m_kal, m_ranging, reset(), DUNE::Navigation::KalmanFilter::resetState(), DUNE::Tasks::Task::spew(), and DUNE::Navigation::Ranging::updateOrigin().

Referenced by consume(), and updateDepth().

void DUNE::Navigation::BasicNavigation::updateAcceleration ( float  filter )

inlineprotected

Routine to clear accelerations buffer.

Parameters

[in]

filter

filter value.

References getAcceleration().

Referenced by updateBuffers().

void DUNE::Navigation::BasicNavigation::updateAngularVelocities ( float  filter )

inlineprotected

Routine to clear angular velocities buffer.

Parameters

[in]

filter

filter value.

References getAngularVelocity().

Referenced by isActive(), and updateBuffers().

void DUNE::Navigation::BasicNavigation::updateBuffers ( float  filter )

protected

Routine to update sensor buffers.

Parameters

[in]

filter

sensor filters gain.

References updateAcceleration(), updateAngularVelocities(), updateDepth(), updateEuler(), and updateEulerDelta().

Referenced by Navigation::General::ROV::Task::task(), and updateDepth().

void DUNE::Navigation::BasicNavigation::updateDepth ( float  filter )

inlineprotected

Routine to clear depth buffer.

Parameters

[in]

filter

filter value.

References checkDeclination(), checkUncertainty(), getDepth(), getNumberOutputs(), getSpeedOutputStates(), isActive(), onConsumeLblConfig(), onDispatchNavigation(), reportToBus(), reset(), resetAcceleration(), resetAngularVelocity(), resetDepth(), resetEulerAngles(), resetEulerAnglesDelta(), runKalmanDVL(), runKalmanGPS(), runKalmanLBL(), runKalmanUSBL(), setup(), updateBuffers(), and updateKalmanGpsParameters().

Referenced by isActive(), and updateBuffers().

void DUNE::Navigation::BasicNavigation::updateEuler ( float  filter )

inlineprotected

Routine to clear euler angles buffer.

Parameters

[in]

filter

filter value.

References getEuler().

Referenced by isActive(), and updateBuffers().

void DUNE::Navigation::BasicNavigation::updateEulerDelta ( float  filter )

inlineprotected

Routine to clear euler angles delta buffer.

Parameters

[in]

filter

filter value.

References getEulerDelta().

Referenced by updateBuffers().

void DUNE::Navigation::BasicNavigation::updateKalmanGpsParameters ( double  hacc )

protectedvirtual

Routine to update navigation filter parameters when a GpsFix message is received.

Parameters

[in]

hacc

GPS Sensor horizontal accuracy index.

Reimplemented in Navigation::AUV::Navigation::Task.

Referenced by consume(), and updateDepth().

Member Data Documentation

bool DUNE::Navigation::BasicNavigation::m_aligned

protected

Vehicle is aligned.

Referenced by BasicNavigation(), and checkUncertainty().

bool DUNE::Navigation::BasicNavigation::m_dead_reckoning

protected

Dead reckoning mode.

Referenced by BasicNavigation(), checkUncertainty(), and getHeadingRate().

Math::Derivative<double> DUNE::Navigation::BasicNavigation::m_deriv_heave

protected

Derivative for heave.

Referenced by onDispatchNavigation().

IMC::DvlRejection DUNE::Navigation::BasicNavigation::m_dvl_rej

protected

DVL measurement rejection.

Referenced by consume().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_dvl_sanity_timer

protected

DVL data sanity timeout.

Referenced by consume(), getAltitude(), and onUpdateParameters().

IMC::EstimatedState DUNE::Navigation::BasicNavigation::m_estate

protected

Estimated state message.

Referenced by consume(), isActive(), Navigation::General::ROV::Task::logData(), onDispatchNavigation(), reportToBus(), and setup().

IMC::GpsFixRejection DUNE::Navigation::BasicNavigation::m_gps_rej

protected

GPS fix rejection.

Referenced by consume().

double DUNE::Navigation::BasicNavigation::m_gps_sog

protected

Valid GPS speed over ground.

Referenced by consume(), reset(), and Navigation::General::ROV::Task::task().

IMC::GroundVelocity DUNE::Navigation::BasicNavigation::m_gvel

protected

Current velocity relative to ground message.

Referenced by consume(), and runKalmanDVL().

double DUNE::Navigation::BasicNavigation::m_heading

protected

Heading value (rad).

Referenced by isActive(), onDispatchNavigation(), and reset().

unsigned DUNE::Navigation::BasicNavigation::m_imu_eid

protected

IMU entity id.

Referenced by consume().

Navigation::KalmanFilter DUNE::Navigation::BasicNavigation::m_kal

protected

Kalman Filter matrices.

Referenced by checkUncertainty(), consume(), Navigation::General::ROV::Task::logData(), Navigation::General::ROV::Task::onConsumeLblConfig(), onDispatchNavigation(), Navigation::General::ROV::Task::onUpdateParameters(), Navigation::General::ROV::Task::resetKalman(), runKalmanDVL(), Navigation::General::ROV::Task::runKalmanGPS(), runKalmanLBL(), Navigation::General::ROV::Task::setup(), setup(), Navigation::General::ROV::Task::Task(), and Navigation::General::ROV::Task::task().

double DUNE::Navigation::BasicNavigation::m_last_z

protected

Vertical displacement in the NED frame to the origin height above ellipsoid.

Referenced by consume(), onDispatchNavigation(), and reset().

IMC::LblRangeAcceptance DUNE::Navigation::BasicNavigation::m_lbl_ac

protected

LBL range acceptance.

Referenced by consume(), and runKalmanLBL().

bool DUNE::Navigation::BasicNavigation::m_lbl_reading

protected

Received LBL fix.

Referenced by BasicNavigation(), and runKalmanLBL().

float DUNE::Navigation::BasicNavigation::m_lbl_threshold

protected

LBL threshold.

Referenced by BasicNavigation(), and runKalmanLBL().

std::vector<double> DUNE::Navigation::BasicNavigation::m_measure_noise

protected

Kalman Filter measurement noise covariance matrix parameters.

Referenced by Navigation::General::ROV::Task::onUpdateParameters(), and Navigation::General::ROV::Task::Task().

IMC::NavigationData DUNE::Navigation::BasicNavigation::m_navdata

protected

Navigation Data log.

Referenced by Navigation::General::ROV::Task::logData(), onDispatchNavigation(), reportToBus(), and runKalmanLBL().

std::vector<double> DUNE::Navigation::BasicNavigation::m_process_noise

protected

Kalman Filter process noise covariance matrix parameters.

Referenced by Navigation::General::ROV::Task::onUpdateParameters(), and Navigation::General::ROV::Task::Task().

Navigation::Ranging DUNE::Navigation::BasicNavigation::m_ranging

protected

Ranging data.

Referenced by consume(), Navigation::General::ROV::Task::onConsumeLblConfig(), Navigation::General::ROV::Task::onUpdateParameters(), and setup().

int16_t DUNE::Navigation::BasicNavigation::m_rpm

protected

Propeller speed (RPM)

Referenced by BasicNavigation(), and consume().

std::vector<double> DUNE::Navigation::BasicNavigation::m_state_cov

protected

Kalman Filter state covariance matrix parameters.

Referenced by Navigation::General::ROV::Task::setup(), and Navigation::General::ROV::Task::Task().

Navigation::StreamEstimator DUNE::Navigation::BasicNavigation::m_stream_filter

protected

Stream Estimator.

Referenced by consume().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_alt

protected

Time without altitude readings deadline.

Referenced by consume(), getAltitude(), and onUpdateParameters().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_depth

protected

Time without depth readings.

Referenced by checkUncertainty(), consume(), and onUpdateParameters().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_dvl

protected

Time without DVL sensor readings deadline.

Referenced by consume(), onUpdateParameters(), and Navigation::General::ROV::Task::task().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_euler

protected

Time without euler angles readings.

Referenced by checkUncertainty(), consume(), and onUpdateParameters().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_gps

protected

Time without GPS sensor readings deadline.

Referenced by consume(), onUpdateParameters(), and Navigation::General::ROV::Task::task().

Time::Counter<double> DUNE::Navigation::BasicNavigation::m_time_without_main_depth

protected

Time without main depth provider.

Referenced by consume(), and onUpdateParameters().

IMC::NavigationUncertainty DUNE::Navigation::BasicNavigation::m_uncertainty

protected

Navigation Uncertainty log.

Referenced by Navigation::General::ROV::Task::logData(), onDispatchNavigation(), and reportToBus().

bool DUNE::Navigation::BasicNavigation::m_valid_gv

protected

Received valid ground velocity message.

Referenced by consume(), reset(), runKalmanDVL(), and Navigation::General::ROV::Task::task().

bool DUNE::Navigation::BasicNavigation::m_valid_wv

protected

Received valid water velocity message.

Referenced by consume(), reset(), runKalmanDVL(), and Navigation::General::ROV::Task::task().

IMC::WaterVelocity DUNE::Navigation::BasicNavigation::m_wvel

protected

Current velocity relative to the water message.

Referenced by consume(), and runKalmanDVL().

Collaboration diagram for DUNE::Navigation::BasicNavigation: