DUNE: Uniform Navigational Environment
2019.02.0
|
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::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 |
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 | runKalmanUSBL (double x, double y) |
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 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_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 |
|
inline |
Constructor.
[in] | name | task name. |
[in] | ctx | context. |
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().
|
inlinevirtual |
Get number of EKF outputs.
Implements DUNE::Navigation::BasicNavigation.
References Navigation::General::ROV::NUM_OUT.
|
inlinevirtual |
Get EKF output matrix speed indexes.
[out] | u | forward speed state index. |
[out] | v | transversal speed state index. |
Implements DUNE::Navigation::BasicNavigation.
References Navigation::General::ROV::OUT_U, and Navigation::General::ROV::OUT_V.
|
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_kal, DUNE::Navigation::BasicNavigation::m_navdata, DUNE::Navigation::BasicNavigation::m_uncertainty, DUNE::Navigation::BasicNavigation::onDispatchNavigation(), Navigation::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, DUNE::IMC::EstimatedState::psi, DUNE::IMC::EstimatedState::r, Navigation::General::ROV::STATE_U, Navigation::General::ROV::STATE_V, DUNE::IMC::EstimatedState::u, DUNE::IMC::NavigationUncertainty::u, DUNE::IMC::EstimatedState::v, DUNE::IMC::NavigationUncertainty::v, DUNE::IMC::EstimatedState::vx, and DUNE::IMC::EstimatedState::vy.
Referenced by task().
|
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().
|
inlinevirtual |
Initialize resources.
Reimplemented from DUNE::Navigation::BasicNavigation.
|
inlinevirtual |
Release resources.
Reimplemented from DUNE::Navigation::BasicNavigation.
|
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().
|
inlinevirtual |
Routine to reset navigation.
Reimplemented from DUNE::Navigation::BasicNavigation.
References m_gps_reading.
|
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().
|
inlinevirtual |
Routine to assign EKF filter output variables when a GpsFix message is received.
[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::General::ROV::OUT_GPS_X, Navigation::General::ROV::OUT_GPS_Y, and DUNE::Navigation::KalmanFilter::setOutput().
|
inline |
|
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.
|
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().
bool Navigation::General::ROV::Task::m_gps_reading |
Periodic GPS fix reading check.
Referenced by reset(), runKalmanGPS(), and task().