Navigation::AUV::SGNavigation::Task Struct Reference

Public Member Functions
	Task (const std::string &name, Tasks::Context &ctx)
void	onUpdateParameters (void)
void	onResourceInitialization (void)
void	onResourceRelease (void)
void	onEntityResolution (void)
void	onReportEntityState (void)
void	consume (const IMC::EntityActivationState *msg)
bool	setup (void)
void	reset (void)
double	getBiasedHeading (void)
void	onConsumeLblConfig (void)
void	updateKalmanParametersGps (double hacc)
void	runKalmanGPS (double x, double y)
void	correctAlignment (double psi)
void	getSpeedOutputStates (unsigned *u, unsigned *v)
unsigned	getNumberOutputs (void)
void	task (void)
void	setTransition (Matrix &A)
void	resetKalman (void)
void	logData (void)
Public Member Functions inherited from DUNE::Navigation::BasicNavigation
	BasicNavigation (const std::string &name, Tasks::Context &ctx)
virtual	~BasicNavigation (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::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
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
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
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)
State	getState (void)
bool	isCreated (void)
bool	isStopping (void)
bool	isRunning (void)
bool	isStarting (void)
bool	isDead (void)

Public Attributes
bool	m_gps_reading
MovingAverage< double > *	m_avg_speed
Arguments	m_args

Additional Inherited Members
Public Types inherited from DUNE::Concurrency::Runnable
enum	State {   StateStarting, StateRunning, StateStopping, StateDead,   StateUnknown }
Protected Member Functions inherited from DUNE::Navigation::BasicNavigation
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	updateKalmanGpsParameters (double hacc)
virtual void	runKalmanLBL (int beacon, float range, double dx, double dy, double exp_range)
virtual void	runKalmanDVL (void)
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
const char *	getEntityLabel (void) const
void	setEntityLabel (const std::string &label)
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)
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 void	onEntityReservation (void)
virtual void	onResourceAcquisition (void)
virtual void	onRequestActivation (void)
virtual void	onRequestDeactivation (void)
virtual void	onActivation (void)
virtual void	onDeactivation (void)
Protected Member Functions inherited from DUNE::Concurrency::Thread
void	startImpl (void)
void	stopImpl (void)
void	joinImpl (void)
void	setPriorityImpl (Scheduler::Policy policy, unsigned priority)
Protected Attributes inherited from DUNE::Navigation::BasicNavigation
Navigation::KalmanFilter	m_kal
Navigation::Ranging	m_ranging
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::EstimatedStreamVelocity	m_ewvel
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_sum_euler_inc
bool	m_aligned
unsigned	m_agvel_eid
unsigned	m_accel_eid
unsigned	m_imu_eid
unsigned	m_alignment_eid
float	m_lbl_threshold
double	m_heading
bool	m_valid_gv
bool	m_valid_wv
Math::Derivative< double >	m_deriv_heave
Protected Attributes inherited from DUNE::Tasks::Task
Context &	m_ctx

Constructor & Destructor Documentation

Navigation::AUV::SGNavigation::Task::Task ( const std::string &  name, Tasks::Context &  ctx  )

inline

References Navigation::AUV::SGNavigation::Arguments::abort, Navigation::AUV::SGNavigation::Arguments::alignment_index, Navigation::AUV::SGNavigation::Arguments::avg_speed_samples, DUNE::Tasks::Parameter::defaultValue(), DUNE::Tasks::Parameter::description(), Navigation::AUV::SGNavigation::Arguments::elabel_imu, Navigation::AUV::SGNavigation::Arguments::gps_noise, Navigation::AUV::SGNavigation::Arguments::increment_euler_delta, Navigation::AUV::SGNavigation::Arguments::initial_rpm_to_speed, Navigation::AUV::SGNavigation::Arguments::lbl_noise_with_imu, m_args, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_measure_noise, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_state_cov, Navigation::AUV::SGNavigation::NUM_OUT, Navigation::AUV::SGNavigation::NUM_STATE, DUNE::Tasks::Task::param(), Navigation::AUV::SGNavigation::Arguments::position_noise_with_imu, DUNE::Navigation::KalmanFilter::reset(), and resetKalman().

Member Function Documentation

void Navigation::AUV::SGNavigation::Task::consume ( const IMC::EntityActivationState *  msg )

inline

References DUNE::Navigation::BasicNavigation::getAhrsId(), DUNE::Navigation::Ranging::getSize(), Navigation::AUV::SGNavigation::Arguments::increment_euler_delta, Navigation::AUV::SGNavigation::Arguments::lbl_noise_with_imu, DUNE::Navigation::BasicNavigation::m_agvel_eid, DUNE::Navigation::BasicNavigation::m_aligned, m_args, DUNE::Navigation::BasicNavigation::m_dead_reckoning, DUNE::Navigation::BasicNavigation::m_imu_eid, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_measure_noise, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_ranging, DUNE::Navigation::BasicNavigation::m_state_cov, DUNE::Navigation::BasicNavigation::m_sum_euler_inc, Navigation::AUV::SGNavigation::MN_LBL, Navigation::AUV::SGNavigation::NUM_OUT, Navigation::AUV::SGNavigation::PN_POSITION, Navigation::AUV::SGNavigation::PN_PSI_BIAS, Navigation::AUV::SGNavigation::Arguments::position_noise_with_imu, DUNE::Navigation::KalmanFilter::resetCovariance(), Navigation::AUV::SGNavigation::SC_BIASES, DUNE::Navigation::KalmanFilter::setCovariance(), DUNE::Navigation::KalmanFilter::setMeasurementNoise(), DUNE::Navigation::KalmanFilter::setProcessNoise(), DUNE::Navigation::KalmanFilter::setState(), DUNE::Tasks::Task::spew(), Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_X, and Navigation::AUV::SGNavigation::STATE_Y.

void Navigation::AUV::SGNavigation::Task::correctAlignment ( double  psi )

inlinevirtual

Routine called when an alignment result is received.

Parameters

[in]

psi

heading angle.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::BasicNavigation::m_heading, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_state_cov, DUNE::Navigation::KalmanFilter::resetCovariance(), Navigation::AUV::SGNavigation::SC_BIASES, DUNE::Navigation::KalmanFilter::setCovariance(), DUNE::Navigation::KalmanFilter::setState(), Navigation::AUV::SGNavigation::STATE_PSI, and Navigation::AUV::SGNavigation::STATE_PSI_BIAS.

double Navigation::AUV::SGNavigation::Task::getBiasedHeading ( void  )

inline

References DUNE::Navigation::KalmanFilter::getState(), DUNE::Navigation::BasicNavigation::m_kal, Navigation::AUV::SGNavigation::STATE_PSI, and Navigation::AUV::SGNavigation::STATE_PSI_BIAS.

Referenced by task().

unsigned Navigation::AUV::SGNavigation::Task::getNumberOutputs ( void  )

inlinevirtual

Get number of EKF outputs.

Returns

number of outputs.

Implements DUNE::Navigation::BasicNavigation.

References Navigation::AUV::SGNavigation::NUM_OUT.

void Navigation::AUV::SGNavigation::Task::getSpeedOutputStates ( unsigned *  u, unsigned *  v  )

inlinevirtual

Get EKF output matrix speed indexes.

Parameters

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

Implements DUNE::Navigation::BasicNavigation.

References Navigation::AUV::SGNavigation::OUT_U, and Navigation::AUV::SGNavigation::OUT_V.

void Navigation::AUV::SGNavigation::Task::logData ( void  )

inline

References DUNE::IMC::NavigationUncertainty::bias_psi, DUNE::IMC::NavigationData::bias_psi, DUNE::IMC::NavigationData::bias_r, DUNE::IMC::NavigationData::cog, DUNE::IMC::NavigationData::custom_x, DUNE::IMC::NavigationData::custom_y, DUNE::IMC::NavigationData::custom_z, DUNE::Navigation::KalmanFilter::getCovariance(), DUNE::Navigation::BasicNavigation::getEuler(), DUNE::Navigation::KalmanFilter::getInnovation(), DUNE::Navigation::KalmanFilter::getState(), m_avg_speed, DUNE::Navigation::BasicNavigation::m_estate, DUNE::Navigation::BasicNavigation::m_ewvel, DUNE::Navigation::BasicNavigation::m_gvel, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_navdata, DUNE::Navigation::BasicNavigation::m_time_without_dvl, DUNE::Navigation::BasicNavigation::m_uncertainty, DUNE::Navigation::BasicNavigation::m_valid_gv, DUNE::Navigation::BasicNavigation::m_valid_wv, DUNE::Navigation::BasicNavigation::m_wvel, DUNE::Navigation::BasicNavigation::onDispatchNavigation(), Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, DUNE::Time::Counter< T >::overflow(), DUNE::IMC::EstimatedState::phi, DUNE::IMC::EstimatedState::psi, DUNE::IMC::NavigationUncertainty::psi, DUNE::IMC::EstimatedState::r, DUNE::IMC::NavigationUncertainty::r, Navigation::AUV::SGNavigation::STATE_K, Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_R_BIAS, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, DUNE::IMC::EstimatedState::theta, DUNE::IMC::EstimatedState::u, DUNE::IMC::NavigationUncertainty::u, DUNE::Math::MovingAverage< T >::update(), DUNE::IMC::EstimatedState::v, DUNE::IMC::NavigationUncertainty::v, DUNE::IMC::EstimatedState::vx, DUNE::IMC::EstimatedState::vy, DUNE::IMC::GroundVelocity::x, DUNE::IMC::WaterVelocity::x, DUNE::IMC::EstimatedStreamVelocity::x, DUNE::IMC::GroundVelocity::y, DUNE::IMC::WaterVelocity::y, DUNE::IMC::EstimatedStreamVelocity::y, DUNE::IMC::GroundVelocity::z, DUNE::IMC::WaterVelocity::z, and DUNE::IMC::EstimatedStreamVelocity::z.

Referenced by task().

void Navigation::AUV::SGNavigation::Task::onConsumeLblConfig ( void  )

inlinevirtual

Method invoked when LblConfig message is received.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::Ranging::getSize(), DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_ranging, Navigation::AUV::SGNavigation::NUM_OUT, onUpdateParameters(), and DUNE::Navigation::KalmanFilter::resize().

void Navigation::AUV::SGNavigation::Task::onEntityResolution ( void  )

inlinevirtual

Resolve entities.

Reimplemented from DUNE::Navigation::BasicNavigation.

References Navigation::AUV::SGNavigation::Arguments::elabel_imu, m_args, DUNE::Navigation::BasicNavigation::m_imu_eid, and DUNE::Tasks::Task::resolveEntity().

void Navigation::AUV::SGNavigation::Task::onReportEntityState ( void  )

inlinevirtual

Called when the task is instructed to report the state of its entities.

Derived classes that need to report the state of entities other than the main entity should override this function to dispatch the EntityState of those entities.

Reimplemented from DUNE::Tasks::Task.

References DUNE::Tasks::Task::dispatch(), DUNE::Navigation::BasicNavigation::m_aligned, and DUNE::Navigation::BasicNavigation::m_imu_eid.

void Navigation::AUV::SGNavigation::Task::onResourceInitialization ( void  )

inlinevirtual

Initialize resources.

Reimplemented from DUNE::Navigation::BasicNavigation.

References Navigation::AUV::SGNavigation::Arguments::avg_speed_samples, m_args, and m_avg_speed.

void Navigation::AUV::SGNavigation::Task::onResourceRelease ( void  )

inlinevirtual

Release allocated resources.

Reimplemented from DUNE::Navigation::BasicNavigation.

References m_avg_speed.

void Navigation::AUV::SGNavigation::Task::onUpdateParameters ( void  )

inlinevirtual

Update internal parameters.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::Ranging::getSize(), Navigation::AUV::SGNavigation::GPS_ACC_GOOD_IDX, Navigation::AUV::SGNavigation::Arguments::gps_noise, Navigation::AUV::SGNavigation::Arguments::lbl_noise_with_imu, m_args, DUNE::Navigation::BasicNavigation::m_dead_reckoning, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_measure_noise, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_ranging, Navigation::AUV::SGNavigation::MN_LBL, Navigation::AUV::SGNavigation::MN_PSI, Navigation::AUV::SGNavigation::MN_U, Navigation::AUV::SGNavigation::MN_V, Navigation::AUV::SGNavigation::MN_YAWRATE, Navigation::AUV::SGNavigation::NUM_OUT, Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, Navigation::AUV::SGNavigation::OUT_PSI, Navigation::AUV::SGNavigation::OUT_R, Navigation::AUV::SGNavigation::OUT_U, Navigation::AUV::SGNavigation::OUT_V, Navigation::AUV::SGNavigation::PN_POSITION, Navigation::AUV::SGNavigation::PN_PSI, Navigation::AUV::SGNavigation::PN_PSI_BIAS, Navigation::AUV::SGNavigation::PN_SPEED, Navigation::AUV::SGNavigation::PN_YAWRATE, Navigation::AUV::SGNavigation::PN_YRATE_BIAS, Navigation::AUV::SGNavigation::Arguments::position_noise_with_imu, DUNE::Navigation::KalmanFilter::setMeasurementNoise(), DUNE::Navigation::KalmanFilter::setProcessNoise(), Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_R_BIAS, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, Navigation::AUV::SGNavigation::STATE_X, and Navigation::AUV::SGNavigation::STATE_Y.

Referenced by onConsumeLblConfig().

void Navigation::AUV::SGNavigation::Task::reset ( void  )

inlinevirtual

Routine to reset navigation.

Reimplemented from DUNE::Navigation::BasicNavigation.

References m_gps_reading.

void Navigation::AUV::SGNavigation::Task::resetKalman ( void  )

inline

References DUNE::Navigation::BasicNavigation::m_kal, Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, Navigation::AUV::SGNavigation::OUT_PSI, Navigation::AUV::SGNavigation::OUT_R, Navigation::AUV::SGNavigation::OUT_U, Navigation::AUV::SGNavigation::OUT_V, DUNE::Navigation::KalmanFilter::resetOutputs(), DUNE::Navigation::KalmanFilter::setObservation(), Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_R_BIAS, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, Navigation::AUV::SGNavigation::STATE_X, and Navigation::AUV::SGNavigation::STATE_Y.

Referenced by Task(), and task().

void Navigation::AUV::SGNavigation::Task::runKalmanGPS ( double  x, double  y  )

inlinevirtual

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 from DUNE::Navigation::BasicNavigation.

References m_gps_reading, DUNE::Navigation::BasicNavigation::m_kal, Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, and DUNE::Navigation::KalmanFilter::setOutput().

void Navigation::AUV::SGNavigation::Task::setTransition ( Matrix &  A )

inline

References DUNE::Math::Matrix::fill(), DUNE::Navigation::BasicNavigation::getEuler(), DUNE::Navigation::KalmanFilter::getState(), DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_rpm, DUNE::Navigation::BasicNavigation::m_time_without_dvl, DUNE::Navigation::BasicNavigation::m_time_without_gps, DUNE::Time::Counter< T >::overflow(), Navigation::AUV::SGNavigation::STATE_K, Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, Navigation::AUV::SGNavigation::STATE_X, and Navigation::AUV::SGNavigation::STATE_Y.

Referenced by task().

bool Navigation::AUV::SGNavigation::Task::setup ( void  )

inlinevirtual

Routine to setup navigation.

Reimplemented from DUNE::Navigation::BasicNavigation.

References Navigation::AUV::SGNavigation::Arguments::initial_rpm_to_speed, m_args, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_state_cov, Navigation::AUV::SGNavigation::SC_ANGLES, Navigation::AUV::SGNavigation::SC_BIASES, Navigation::AUV::SGNavigation::SC_POSITION, Navigation::AUV::SGNavigation::SC_RPM, Navigation::AUV::SGNavigation::SC_SPEED, DUNE::Navigation::KalmanFilter::setCovariance(), DUNE::Navigation::KalmanFilter::setState(), Navigation::AUV::SGNavigation::STATE_K, Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_R_BIAS, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, Navigation::AUV::SGNavigation::STATE_X, and Navigation::AUV::SGNavigation::STATE_Y.

void Navigation::AUV::SGNavigation::Task::task ( void  )

inlinevirtual

The task to be executed on each cycle.

Implements DUNE::Tasks::Periodic.

References Navigation::AUV::SGNavigation::Arguments::abort, Navigation::AUV::SGNavigation::Arguments::alignment_index, DUNE::Navigation::BasicNavigation::checkUncertainty(), getBiasedHeading(), DUNE::Navigation::KalmanFilter::getCovariance(), DUNE::Navigation::BasicNavigation::getEuler(), DUNE::Navigation::BasicNavigation::getHeadingRate(), DUNE::Navigation::KalmanFilter::getOutput(), DUNE::Navigation::KalmanFilter::getState(), DUNE::Navigation::BasicNavigation::getTimeStep(), logData(), DUNE::Navigation::BasicNavigation::m_aligned, m_args, DUNE::Navigation::BasicNavigation::m_dead_reckoning, m_gps_reading, DUNE::Navigation::BasicNavigation::m_gps_sog, DUNE::Navigation::BasicNavigation::m_heading, DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_rpm, DUNE::Navigation::BasicNavigation::m_time_without_dvl, DUNE::Navigation::BasicNavigation::m_time_without_gps, DUNE::Navigation::BasicNavigation::m_valid_gv, DUNE::Navigation::BasicNavigation::m_valid_wv, Navigation::AUV::SGNavigation::NUM_STATE, Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, Navigation::AUV::SGNavigation::OUT_PSI, Navigation::AUV::SGNavigation::OUT_R, Navigation::AUV::SGNavigation::OUT_U, Navigation::AUV::SGNavigation::OUT_V, DUNE::Time::Counter< T >::overflow(), DUNE::Navigation::KalmanFilter::predict(), DUNE::Navigation::BasicNavigation::reportToBus(), DUNE::Time::Counter< T >::reset(), resetKalman(), DUNE::Navigation::BasicNavigation::runKalmanDVL(), DUNE::Navigation::KalmanFilter::setCovarianceTransition(), DUNE::Navigation::KalmanFilter::setInnovation(), DUNE::Navigation::KalmanFilter::setObservation(), DUNE::Navigation::KalmanFilter::setOutput(), DUNE::Navigation::KalmanFilter::setStateTransition(), setTransition(), Navigation::AUV::SGNavigation::STATE_K, Navigation::AUV::SGNavigation::STATE_PSI, Navigation::AUV::SGNavigation::STATE_PSI_BIAS, Navigation::AUV::SGNavigation::STATE_R, Navigation::AUV::SGNavigation::STATE_R_BIAS, Navigation::AUV::SGNavigation::STATE_U, Navigation::AUV::SGNavigation::STATE_V, Navigation::AUV::SGNavigation::STATE_X, Navigation::AUV::SGNavigation::STATE_Y, DUNE::Navigation::KalmanFilter::update(), and DUNE::Navigation::BasicNavigation::updateBuffers().

void Navigation::AUV::SGNavigation::Task::updateKalmanParametersGps ( double  hacc )

inline

References Navigation::AUV::SGNavigation::GPS_ACC_AVG_IDX, Navigation::AUV::SGNavigation::GPS_ACC_AVG_THRESH, Navigation::AUV::SGNavigation::GPS_ACC_BAD_IDX, Navigation::AUV::SGNavigation::GPS_ACC_BAD_THRESH, Navigation::AUV::SGNavigation::GPS_ACC_GOOD_IDX, Navigation::AUV::SGNavigation::GPS_ACC_GOOD_THRESH, Navigation::AUV::SGNavigation::GPS_ACC_PERFECT_IDX, Navigation::AUV::SGNavigation::Arguments::gps_noise, m_args, DUNE::Navigation::BasicNavigation::m_kal, Navigation::AUV::SGNavigation::OUT_GPS_X, Navigation::AUV::SGNavigation::OUT_GPS_Y, and DUNE::Navigation::KalmanFilter::setMeasurementNoise().

Member Data Documentation

Arguments Navigation::AUV::SGNavigation::Task::m_args

Task arguments.

Referenced by consume(), onEntityResolution(), onResourceInitialization(), onUpdateParameters(), setup(), Task(), task(), and updateKalmanParametersGps().

MovingAverage<double>* Navigation::AUV::SGNavigation::Task::m_avg_speed

Moving average for vehicle forward speed.

Referenced by logData(), onResourceInitialization(), and onResourceRelease().

bool Navigation::AUV::SGNavigation::Task::m_gps_reading

Periodic GPS fix reading check.

Referenced by reset(), runKalmanGPS(), and task().

Collaboration diagram for Navigation::AUV::SGNavigation::Task: