DUNE: Uniform Navigational Environment  2.4.2
DUNE::Navigation::BasicNavigation Class Referenceabstract

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::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
 
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)
 

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 correctAlignment (double psi)
 
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 (void)
 
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)
 
unsigned int resolveEntity (const std::string &label) const
 
std::string resolveEntity (unsigned int id) const
 
bool stopping (void)
 
bool isActive (void) const
 
void waitForMessages (double timeout)
 
void consumeMessages (void)
 
template<typename T >
Parameterparam (const std::string &name, T &var)
 
template<typename Y , typename T >
Parameterparam (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 >
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 onReportEntityState (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

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
Contextm_ctx
 

Additional Inherited Members

- Public Types inherited from DUNE::Concurrency::Runnable
enum  State {
  StateStarting, StateRunning, StateStopping, StateDead,
  StateUnknown
}
 

Constructor & Destructor Documentation

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]latvehicle current latitude.
[in]lonvehicle current longitude.
[in]heightvehicle current height.

References DUNE::Coordinates::WMM::declination(), DUNE::Tasks::Context::dir_cfg, and DUNE::Tasks::Task::m_ctx.

Referenced by consume().

void DUNE::Navigation::BasicNavigation::consume ( const IMC::DepthOffset msg)
void DUNE::Navigation::BasicNavigation::consume ( const IMC::GpsFix msg)
void DUNE::Navigation::BasicNavigation::consume ( const IMC::Rpm msg)

References m_rpm, and DUNE::IMC::Rpm::value.

void DUNE::Navigation::BasicNavigation::correctAlignment ( double  psi)
protectedvirtual

Routine called when an alignment result is received.

Parameters
[in]psiheading angle.

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

Referenced by consume().

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.

Referenced by Navigation::AUV::SGNavigation::Task::consume().

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(), Navigation::AUV::Navigation::Task::task(), 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::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
double DUNE::Navigation::BasicNavigation::getLblRejectionValue ( double  exp_range) const
inlineprotected

Routine to compute LBL rejection value.

Parameters
[in]exp_rangeexpected LBL range value.
Returns
LBL rejection value.

Referenced by Navigation::AUV::Navigation::Task::runKalmanLBL(), and runKalmanLBL().

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

Get number of EKF outputs.

Returns
number of outputs.

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

Referenced by runKalmanLBL().

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

Get EKF output matrix speed indexes.

Parameters
[out]uforward speed state index.
[out]vtransversal speed state index.

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

Referenced by runKalmanDVL().

double DUNE::Navigation::BasicNavigation::getTimeStep ( void  )
inlineprotected
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().

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

Method invoked when LblConfig message is received.

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

Referenced by consume().

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

Initialize resources.

Reimplemented from DUNE::Tasks::Task.

Reimplemented in Navigation::AUV::SGNavigation::Task, 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::SGNavigation::Task, Navigation::General::ROV::Task, and Navigation::AUV::Navigation::Task.

References DUNE::Memory::clear().

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
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.

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.

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

Routine to reset depth buffers.

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.

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

Routine to reset euler angles delta buffers.

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

void DUNE::Navigation::BasicNavigation::runKalmanDVL ( void  )
protectedvirtual
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]xvehicle north displacement (m).
[in]yvehicle east displacement (m).

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

Referenced by consume().

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

Routine to clear accelerations buffer.

Parameters
[in]filterfilter value.

References getAcceleration().

Referenced by updateBuffers().

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

Routine to clear angular velocities buffer.

Parameters
[in]filterfilter value.

References getAngularVelocity().

Referenced by isActive(), and updateBuffers().

void DUNE::Navigation::BasicNavigation::updateBuffers ( float  filter)
protected
void DUNE::Navigation::BasicNavigation::updateDepth ( float  filter)
inlineprotected

Routine to clear depth buffer.

Parameters
[in]filterfilter value.

References getDepth().

Referenced by isActive(), and updateBuffers().

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

Routine to clear euler angles buffer.

Parameters
[in]filterfilter 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]filterfilter 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]haccGPS Sensor horizontal accuracy index.

Referenced by consume().

Member Data Documentation

unsigned DUNE::Navigation::BasicNavigation::m_accel_eid
protected

Accelaration message entity id.

Referenced by consume(), and onEntityResolution().

unsigned DUNE::Navigation::BasicNavigation::m_agvel_eid
protected

Angular velocity message entity id.

Referenced by consume(), Navigation::AUV::SGNavigation::Task::consume(), and onEntityResolution().

unsigned DUNE::Navigation::BasicNavigation::m_alignment_eid
protected

Orientation alignment entity id.

Referenced by consume(), and onEntityResolution().

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::EstimatedStreamVelocity DUNE::Navigation::BasicNavigation::m_ewvel
protected
IMC::GpsFixRejection DUNE::Navigation::BasicNavigation::m_gps_rej
protected

GPS fix rejection.

Referenced by consume().

double DUNE::Navigation::BasicNavigation::m_gps_sog
protected
IMC::GroundVelocity DUNE::Navigation::BasicNavigation::m_gvel
protected
Navigation::KalmanFilter DUNE::Navigation::BasicNavigation::m_kal
protected
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
float DUNE::Navigation::BasicNavigation::m_lbl_threshold
protected
int16_t DUNE::Navigation::BasicNavigation::m_rpm
protected
bool DUNE::Navigation::BasicNavigation::m_sum_euler_inc
protected

Sum euler increments to get heading.

Referenced by BasicNavigation(), Navigation::AUV::SGNavigation::Task::consume(), and getHeadingRate().

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_euler
protected

Time without euler angles readings.

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

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

Time without main depth provider.

Referenced by consume(), and onUpdateParameters().

bool DUNE::Navigation::BasicNavigation::m_valid_gv
protected
bool DUNE::Navigation::BasicNavigation::m_valid_wv
protected
IMC::WaterVelocity DUNE::Navigation::BasicNavigation::m_wvel
protected

Current velocity relative to the water message.

Referenced by consume(), Navigation::General::ROV::Task::logData(), Navigation::AUV::SGNavigation::Task::logData(), and runKalmanDVL().

Collaboration diagram for DUNE::Navigation::BasicNavigation:
Collaboration graph