DUNE: Uniform Navigational Environment  2016.09.0
Navigation::AUV::Navigation::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)
 
void consume (const IMC::VehicleMedium *msg)
 
bool setup (void)
 
void reset (void)
 
double getBiasedHeading (void)
 
void onConsumeLblConfig (void)
 
void updateKalmanParametersGps (double hacc)
 
void runKalmanGPS (double x, double y)
 
void runKalmanUSBL (double x, double y)
 
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::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
 
- 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)
 

Public Attributes

bool m_gps_reading
 
bool m_usbl_reading
 
MovingAverage< double > * m_avg_speed
 
Monitors::MediumHandler m_medium
 
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
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)
 
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 >
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 , 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 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 inherited from DUNE::Navigation::BasicNavigation
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_sum_euler_inc
 
bool m_aligned
 
unsigned m_agvel_eid
 
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
Contextm_ctx
 
std::vector
< Entities::BasicEntity * > 
m_entities
 

Constructor & Destructor Documentation

Member Function Documentation

void Navigation::AUV::Navigation::Task::consume ( const IMC::VehicleMedium *  msg)
inline

References m_medium.

double Navigation::AUV::Navigation::Task::getBiasedHeading ( void  )
inline
unsigned Navigation::AUV::Navigation::Task::getNumberOutputs ( void  )
inlinevirtual

Get number of EKF outputs.

Returns
number of outputs.

Implements DUNE::Navigation::BasicNavigation.

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

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

Get EKF output matrix speed indexes.

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

Implements DUNE::Navigation::BasicNavigation.

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

void Navigation::AUV::Navigation::Task::logData ( void  )
inline
void Navigation::AUV::Navigation::Task::onConsumeLblConfig ( void  )
inlinevirtual
void Navigation::AUV::Navigation::Task::onEntityResolution ( void  )
inlinevirtual
void Navigation::AUV::Navigation::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::Navigation::Task::onResourceInitialization ( void  )
inlinevirtual
void Navigation::AUV::Navigation::Task::onResourceRelease ( void  )
inlinevirtual

Release allocated resources.

Reimplemented from DUNE::Navigation::BasicNavigation.

References m_avg_speed.

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

Update internal parameters.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::Ranging::getSize(), Navigation::AUV::Navigation::GPS_GOOD_IX, Navigation::AUV::Navigation::Arguments::gps_noise, Navigation::AUV::Navigation::Arguments::lbl_noise, 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::Navigation::MN_LBL, Navigation::AUV::Navigation::MN_PSI, Navigation::AUV::Navigation::MN_U, Navigation::AUV::Navigation::MN_V, Navigation::AUV::Navigation::MN_YAWRATE, Navigation::AUV::Navigation::NUM_OUT, Navigation::AUV::Navigation::OUT_GPS_X, Navigation::AUV::Navigation::OUT_GPS_Y, Navigation::AUV::Navigation::OUT_PSI, Navigation::AUV::Navigation::OUT_R, Navigation::AUV::Navigation::OUT_U, Navigation::AUV::Navigation::OUT_V, Navigation::AUV::Navigation::PN_POSITION, Navigation::AUV::Navigation::PN_PSI, Navigation::AUV::Navigation::PN_PSI_BIAS, Navigation::AUV::Navigation::PN_SPEED, Navigation::AUV::Navigation::PN_YAWRATE, Navigation::AUV::Navigation::PN_YRATE_BIAS, Navigation::AUV::Navigation::Arguments::pos_noise, DUNE::Navigation::KalmanFilter::setMeasurementNoise(), DUNE::Navigation::KalmanFilter::setProcessNoise(), Navigation::AUV::Navigation::STATE_PSI, Navigation::AUV::Navigation::STATE_PSI_BIAS, Navigation::AUV::Navigation::STATE_R, Navigation::AUV::Navigation::STATE_R_BIAS, Navigation::AUV::Navigation::STATE_U, Navigation::AUV::Navigation::STATE_V, Navigation::AUV::Navigation::STATE_X, and Navigation::AUV::Navigation::STATE_Y.

Referenced by onConsumeLblConfig().

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

Routine to reset navigation.

Reimplemented from DUNE::Navigation::BasicNavigation.

References m_gps_reading, and m_usbl_reading.

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

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

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

void Navigation::AUV::Navigation::Task::runKalmanUSBL ( double  x,
double  y 
)
inlinevirtual
void Navigation::AUV::Navigation::Task::task ( void  )
inlinevirtual

The task to be executed on each cycle.

Implements DUNE::Tasks::Periodic.

References Navigation::AUV::Navigation::Arguments::abort, Navigation::AUV::Navigation::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_lbl_reading, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_rpm, DUNE::Navigation::BasicNavigation::m_time_without_dvl, DUNE::Navigation::BasicNavigation::m_time_without_gps, m_usbl_reading, DUNE::Navigation::BasicNavigation::m_valid_gv, DUNE::Navigation::BasicNavigation::m_valid_wv, Navigation::AUV::Navigation::NUM_STATE, Navigation::AUV::Navigation::OUT_GPS_X, Navigation::AUV::Navigation::OUT_GPS_Y, Navigation::AUV::Navigation::OUT_PSI, Navigation::AUV::Navigation::OUT_R, Navigation::AUV::Navigation::OUT_U, Navigation::AUV::Navigation::OUT_V, DUNE::Time::Counter< T >::overflow(), Navigation::AUV::Navigation::PN_PSI_BIAS, DUNE::Navigation::KalmanFilter::predict(), DUNE::Navigation::BasicNavigation::reportToBus(), DUNE::Time::Counter< T >::reset(), DUNE::Navigation::KalmanFilter::resetCovariance(), resetKalman(), Navigation::AUV::Navigation::Arguments::rpm_ini, Navigation::AUV::Navigation::Arguments::rpm_max, DUNE::Navigation::BasicNavigation::runKalmanDVL(), DUNE::Navigation::KalmanFilter::setCovariance(), DUNE::Navigation::KalmanFilter::setCovarianceTransition(), DUNE::Navigation::KalmanFilter::setInnovation(), DUNE::Navigation::KalmanFilter::setObservation(), DUNE::Navigation::KalmanFilter::setOutput(), DUNE::Navigation::KalmanFilter::setState(), DUNE::Navigation::KalmanFilter::setStateTransition(), setTransition(), Navigation::AUV::Navigation::STATE_K, Navigation::AUV::Navigation::STATE_PSI, Navigation::AUV::Navigation::STATE_PSI_BIAS, Navigation::AUV::Navigation::STATE_R, Navigation::AUV::Navigation::STATE_R_BIAS, Navigation::AUV::Navigation::STATE_U, Navigation::AUV::Navigation::STATE_V, Navigation::AUV::Navigation::STATE_X, Navigation::AUV::Navigation::STATE_Y, DUNE::Math::trimValue(), DUNE::Navigation::KalmanFilter::update(), and DUNE::Navigation::BasicNavigation::updateBuffers().

Member Data Documentation

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

Moving average for vehicle forward speed.

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

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

Periodic GPS fix reading check.

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

Monitors::MediumHandler Navigation::AUV::Navigation::Task::m_medium

Medium handler.

Referenced by consume().

bool Navigation::AUV::Navigation::Task::m_usbl_reading

USBL fix reading check.

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

Collaboration diagram for Navigation::AUV::Navigation::Task:
Collaboration graph