DUNE: Uniform Navigational Environment  2.4.2
List of all members | Public Member Functions | Public Attributes
Navigation::General::ROV::Task Struct Reference

Public Member Functions

 Task (const std::string &name, Tasks::Context &ctx)
 
void onUpdateParameters (void)
 
void onResourceInitialization (void)
 
void onResourceRelease (void)
 
void onConsumeLblConfig (void)
 
void runKalmanGPS (double x, double y)
 
unsigned getNumberOutputs (void)
 
void getSpeedOutputStates (unsigned *u, unsigned *v)
 
bool setup (void)
 
void reset (void)
 
void setTransition (Matrix &A)
 
void resetKalman (void)
 
void logData (void)
 
void task (void)
 
- Public Member Functions inherited from DUNE::Navigation::BasicNavigation
 BasicNavigation (const std::string &name, Tasks::Context &ctx)
 
virtual ~BasicNavigation (void)
 
virtual void onEntityResolution (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
 
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
 

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)
 
virtual void correctAlignment (double psi)
 
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 >
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 >
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 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::General::ROV::Task::Task ( const std::string &  name,
Tasks::Context &  ctx 
)
inline

Constructor.

Parameters
[in]nametask name.
[in]ctxcontext.

References DUNE::Tasks::Parameter::defaultValue(), DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_measure_noise, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_state_cov, Navigation::General::ROV::NUM_OUT, Navigation::General::ROV::NUM_STATE, DUNE::Tasks::Task::param(), and DUNE::Navigation::KalmanFilter::reset().

Member Function Documentation

unsigned Navigation::General::ROV::Task::getNumberOutputs ( void  )
inlinevirtual

Get number of EKF outputs.

Returns
number of outputs.

Implements DUNE::Navigation::BasicNavigation.

References Navigation::General::ROV::NUM_OUT.

void Navigation::General::ROV::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::General::ROV::OUT_U, and Navigation::General::ROV::OUT_V.

void Navigation::General::ROV::Task::logData ( void  )
inline

References DUNE::IMC::NavigationData::cog, DUNE::IMC::NavigationData::custom_x, DUNE::Navigation::BasicNavigation::getAngularVelocity(), DUNE::Navigation::KalmanFilter::getCovariance(), DUNE::Navigation::BasicNavigation::getEuler(), DUNE::Navigation::KalmanFilter::getInnovation(), DUNE::Navigation::KalmanFilter::getState(), 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::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, DUNE::Time::Counter< T >::overflow(), DUNE::IMC::EstimatedState::phi, DUNE::IMC::EstimatedState::psi, DUNE::IMC::EstimatedState::r, Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, DUNE::IMC::EstimatedState::theta, DUNE::IMC::EstimatedState::u, DUNE::IMC::NavigationUncertainty::u, 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::General::ROV::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::General::ROV::NUM_OUT, onUpdateParameters(), and DUNE::Navigation::KalmanFilter::resize().

void Navigation::General::ROV::Task::onResourceInitialization ( void  )
inlinevirtual

Initialize resources.

Reimplemented from DUNE::Navigation::BasicNavigation.

void Navigation::General::ROV::Task::onResourceRelease ( void  )
inlinevirtual

Release resources.

Reimplemented from DUNE::Navigation::BasicNavigation.

void Navigation::General::ROV::Task::onUpdateParameters ( void  )
inlinevirtual

Update internal state with new parameter values.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::Ranging::getSize(), DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_measure_noise, DUNE::Navigation::BasicNavigation::m_process_noise, DUNE::Navigation::BasicNavigation::m_ranging, Navigation::General::ROV::MN_GPS, Navigation::General::ROV::MN_LBL, Navigation::General::ROV::MN_U, Navigation::General::ROV::MN_V, Navigation::General::ROV::NUM_OUT, Navigation::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, Navigation::General::ROV::OUT_U, Navigation::General::ROV::OUT_V, Navigation::General::ROV::PN_POSITION, Navigation::General::ROV::PN_SPEED, DUNE::Navigation::KalmanFilter::setMeasurementNoise(), DUNE::Navigation::KalmanFilter::setProcessNoise(), Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, Navigation::General::ROV::STATE_X, and Navigation::General::ROV::STATE_Y.

Referenced by onConsumeLblConfig().

void Navigation::General::ROV::Task::reset ( void  )
inlinevirtual

Routine to reset navigation.

Reimplemented from DUNE::Navigation::BasicNavigation.

References m_gps_reading.

void Navigation::General::ROV::Task::resetKalman ( void  )
inline

References DUNE::Navigation::BasicNavigation::m_kal, Navigation::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, Navigation::General::ROV::OUT_U, Navigation::General::ROV::OUT_V, DUNE::Navigation::KalmanFilter::resetOutputs(), DUNE::Navigation::KalmanFilter::setObservation(), Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, Navigation::General::ROV::STATE_X, and Navigation::General::ROV::STATE_Y.

Referenced by task().

void Navigation::General::ROV::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::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, and DUNE::Navigation::KalmanFilter::setOutput().

void Navigation::General::ROV::Task::setTransition ( Matrix &  A)
inline

References DUNE::Math::Matrix::fill(), DUNE::Navigation::BasicNavigation::getEuler(), Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, Navigation::General::ROV::STATE_X, and Navigation::General::ROV::STATE_Y.

Referenced by task().

bool Navigation::General::ROV::Task::setup ( void  )
inlinevirtual

Routine to setup navigation.

Reimplemented from DUNE::Navigation::BasicNavigation.

References DUNE::Navigation::BasicNavigation::m_kal, DUNE::Navigation::BasicNavigation::m_state_cov, Navigation::General::ROV::SC_POSITION, Navigation::General::ROV::SC_SPEED, DUNE::Navigation::KalmanFilter::setCovariance(), Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, Navigation::General::ROV::STATE_X, and Navigation::General::ROV::STATE_Y.

void Navigation::General::ROV::Task::task ( void  )
inlinevirtual

Main loop.

Implements DUNE::Tasks::Periodic.

References DUNE::Navigation::BasicNavigation::checkUncertainty(), DUNE::Navigation::KalmanFilter::getOutput(), DUNE::Navigation::KalmanFilter::getState(), DUNE::Navigation::BasicNavigation::getTimeStep(), logData(), m_gps_reading, DUNE::Navigation::BasicNavigation::m_gps_sog, DUNE::Navigation::BasicNavigation::m_kal, 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::General::ROV::NUM_STATE, Navigation::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, Navigation::General::ROV::OUT_U, Navigation::General::ROV::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::setStateTransition(), setTransition(), Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, Navigation::General::ROV::STATE_X, Navigation::General::ROV::STATE_Y, DUNE::Navigation::KalmanFilter::update(), and DUNE::Navigation::BasicNavigation::updateBuffers().

Member Data Documentation

bool Navigation::General::ROV::Task::m_gps_reading

Periodic GPS fix reading check.

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

Collaboration diagram for Navigation::General::ROV::Task:
Collaboration graph