The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow. More...

#include <Task.hpp>

Inheritance diagram for imu_stim300::Task:

Public Member Functions
	Task (std::string const &name="imu_stim300::Task")

	Task (std::string const &name, RTT::ExecutionEngine *engine)

	~Task ()

bool	configureHook ()
	The following lines are template definitions for the various state machine. More...

bool	startHook ()

void	updateHook ()

void	errorHook ()

void	stopHook ()

void	cleanupHook ()

Eigen::Quaternion< double >	deltaHeading (const Eigen::Vector3d &angvelo, Eigen::Matrix4d &oldomega, const double delta_t)

void	outputPortSamples (imu_stim300::Stim300Base *driver, filter::Ikf< double, true, true > &myfilter, base::samples::IMUSensors &imusamples)
	Port out the values. More...

Static Public Member Functions
static double	GravityModel (double latitude, double altitude)
	This computes the theoretical gravity value according to the WGS-84 ellipsoid Earth model. More...

static void	SubtractEarthRotation (Eigen::Vector3d &u, const Eigen::Quaterniond &q, const double latitude)
	Subtract the Earth rotation from the gyroscopes readout. More...

static Eigen::Quaternion< double >	deltaQuaternion (const Eigen::Vector3d &angvelo, const Eigen::Matrix4d &oldomega4, const Eigen::Matrix4d &omega4, const double dt)
	Delta quaternion rotation. Integration of small (given by the current angular velo) variation in attitude. More...

Protected Attributes
bool	initAttitude

unsigned int	initial_alignment_idx

FilterConfiguration	config

InertialNoiseParameters	accnoise

InertialNoiseParameters	gyronoise

InertialNoiseParameters	incnoise

AdaptiveAttitudeConfig	adaptiveconfigAcc

AdaptiveAttitudeConfig	adaptiveconfigInc

LocationConfiguration	location

base::Time	prev_ts

int	correction_numbers

int	correction_idx

double	sampling_frequency

imu_stim300::Stim300Base *	imu_stim300_driver

aggregator::TimestampEstimator *	timestamp_estimator

Eigen::Vector3d	correctionAcc

Eigen::Vector3d	correctionInc

Eigen::Matrix< double, 3, Eigen::Dynamic >	initial_alignment_gyro

Eigen::Matrix< double, 3, Eigen::Dynamic >	initial_alignment_acc

filter::Ikf< double, true, true >	myfilter

Eigen::Quaterniond	deltaquat

Eigen::Quaterniond	deltahead

Eigen::Quaterniond	attitude

Eigen::Matrix4d	oldomega

base::samples::RigidBodyState	orientation_out

Friends
class	TaskBase

Detailed Description

The task context provides and requires services. It uses an ExecutionEngine to perform its functions. Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification. In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow.

Accepted error for the gravity value in [m/s^2]

The name of a TaskContext is primarily defined via:

deployment 'deployment_name'
    task('custom_task_name','imu_stim300::Task')
end

It can be dynamically adapted when the deployment is called with a prefix argument.

Constructor & Destructor Documentation

◆ Task() [1/2]

Task::Task ( std::string const & name = "imu_stim300::Task" )

the output orientation TaskContext constructor for Task

Parameters

name	Name of the task. This name needs to be unique to make it identifiable via nameservices.
initial_state	The initial TaskState of the TaskContext. Default is Stopped state.

◆ Task() [2/2]

Task::Task	(	std::string const &	name,
		RTT::ExecutionEngine *	engine
	)

TaskContext constructor for Task

Parameters

name	Name of the task. This name needs to be unique to make it identifiable for nameservices.
engine	The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task.

◆ ~Task()

Task::~Task ( )

Default deconstructor of Task

Member Function Documentation

◆ cleanupHook()

void Task::cleanupHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to PreOperational, requiring the call to configureHook() before calling start() again.

◆ configureHook()

bool Task::configureHook ( )

The following lines are template definitions for the various state machine.

This hook is called by Orocos when the state machine transitions from PreOperational to Stopped. If it returns false, then the component will stay in PreOperational. Otherwise, it goes into Stopped.

It is meaningful only if the #needs_configuration has been specified in the task context definition with (for example):

task_context "TaskName" do
  needs_configuration
  ...
end

Configuration of the driver

Set the baudrate to the value in the rock property

Set the packageTimeout

Calculate the sampling frequency

Set the frequency

Open the port

Configuration of the attitude filter

Initial vector state

Measurement noise covariance matrix for accelerometers

Measurement noise covariance matrix for gyros

Measurement noise covariance matrix for mag

Measurement noise covariance matrix for inclinometers

Initial covariance matrix

Noise for the gyros bias instability

Noise for the accelerometers bias instability

Noise for the inclinometers bias instability

Read configuration

Configuration of Time estimator

Configuration frequencies

Correction configuration

Noise configuration

Noise depends on frequency bandwidth

It does not have magnetometers

Noise for error in gyros bias instability

Noise for error in accelerometers bias instability

Noise for error in inclinometers bias instability

Initial error covariance

Theoretical Gravity

Initialize the filter, including the adaptive part

Leveling configuration

Set the index to Zero

Set the correction index

Oldomega initial

Initial attitude

Output variable

◆ deltaHeading()

Eigen::Quaternion< double > Task::deltaHeading	(	const Eigen::Vector3d &	angvelo,
		Eigen::Matrix4d &	oldomega,
		const double	delta_t
	)

Performs heading independent integration

◆ deltaQuaternion()

static Eigen::Quaternion<double> imu_stim300::Task::deltaQuaternion	(	const Eigen::Vector3d &	angvelo,
		const Eigen::Matrix4d &	oldomega4,
		const Eigen::Matrix4d &	omega4,
		const double	dt
	)

inlinestatic

Delta quaternion rotation. Integration of small (given by the current angular velo) variation in attitude.

Identity quaternion

Third-order gyroscopes integration accuracy

◆ errorHook()

void Task::errorHook ( )

This hook is called by Orocos when the component is in the RunTimeError state, at each activity step. See the discussion in updateHook() about triggering options.

Call recover() to go back in the Runtime state.

◆ GravityModel()

static double imu_stim300::Task::GravityModel	(	double	latitude,
		double	altitude
	)

inlinestatic

This computes the theoretical gravity value according to the WGS-84 ellipsoid Earth model.

Author: Javier Hidalgo Carrio.

Parameters

[in]	latitude	double the latitude value in radian
[in]	altitude	double with the altitude value in meters

Returns: double. the theoretical value of the local gravity

g magnitude at zero altitude

Nominal Gravity model

Gravity affects by the altitude (aprox the value r = Re

◆ outputPortSamples()

void Task::outputPortSamples	(	imu_stim300::Stim300Base *	driver,
		filter::Ikf< double, true, true > &	myfilter,
		base::samples::IMUSensors &	imusamples
	)

Port out the values.

Temperature sensors

Temperature on the International Units

Raw calibrated inertial sensor

Compensated and calibrated inertial sensor

◆ startHook()

bool Task::startHook ( )

This hook is called by Orocos when the state machine transitions from Stopped to Running. If it returns false, then the component will stay in Stopped. Otherwise, it goes into Running and updateHook() will be called.

◆ stopHook()

void Task::stopHook ( )

This hook is called by Orocos when the state machine transitions from Running to Stopped after stop() has been called.

◆ SubtractEarthRotation()

static void imu_stim300::Task::SubtractEarthRotation	(	Eigen::Vector3d &	u,
		const Eigen::Quaterniond &	q,
		const double	latitude
	)

inlinestatic

Subtract the Earth rotation from the gyroscopes readout.

This function computes the subtraction of the rotation of the Earth (EARTHW) from the gyroscope values. This function uses quaternion of transformation from the geographic to body frame and the latitude in radians.

Author: Javier Hidalgo Carrio.

Parameters

[in,out]	u	angular velocity in body frame
[in]	q	quaternion from body to geographic(world) frame v_body = q_body_2_geo * v_geo
[in]	latitude	location latitude angle in radians

Returns: void

vector of earth rotation components expressed in the geographic frame according to the latitude

Compute the v vector expressed in the body frame

Subtract the earth rotation to the vector of inputs (u = u-v

◆ updateHook()

void Task::updateHook ( )

This hook is called by Orocos when the component is in the Running state, at each activity step. Here, the activity gives the "ticks" when the hook should be called.

The error(), exception() and fatal() calls, when called in this hook, allow to get into the associated RunTimeError, Exception and FatalError states.

In the first case, updateHook() is still called, and recover() allows you to go back into the Running state. In the second case, the errorHook() will be called instead of updateHook(). In Exception, the component is stopped and recover() needs to be called before starting it again. Finally, FatalError cannot be recovered.

Inertial sensor values

Process the current package

Time is current time minus the latency

Package counter incrementation

Checksum is good: Take the sensor values from the driver

Short term solution: the mag carries inclinometers info (FINAL SOLUTION REQUIRES: e.g. to change IMUSensor base/types)

Attitude filter

Calculate the initial alignment to the local geographic frame

Set attitude to identity

Acceleration

Angular velocity

Override the gravity model value with the sensed from the sensors

Compute the local horizontal plane

Set the attitude

The angular velocity in the local horizontal plane

Gyro_ho = Tho_body * gyro_body

Determine the heading or azimuthal orientation

Set the attitude

Compute the Initial Bias

Eliminate Earth rotation

Predict

Accumulate correction measurements

Update/Correction

Timestamp estimator status

Output information

Friends And Related Function Documentation

◆ TaskBase

friend class TaskBase

friend

Member Data Documentation

◆ accnoise

InertialNoiseParameters imu_stim300::Task::accnoise

protected

Accelerometers noise parameters

◆ adaptiveconfigAcc

AdaptiveAttitudeConfig imu_stim300::Task::adaptiveconfigAcc

protected

Adaptive Measurement Configuration

◆ adaptiveconfigInc

AdaptiveAttitudeConfig imu_stim300::Task::adaptiveconfigInc

protected

◆ attitude

Eigen::Quaterniond imu_stim300::Task::attitude

protected

◆ config

FilterConfiguration imu_stim300::Task::config

protected

Filter configuration values

◆ correction_idx

int imu_stim300::Task::correction_idx

protected

◆ correction_numbers

int imu_stim300::Task::correction_numbers

protected

◆ correctionAcc

Eigen::Vector3d imu_stim300::Task::correctionAcc

protected

Correction Variables

◆ correctionInc

Eigen::Vector3d imu_stim300::Task::correctionInc

protected

◆ deltahead

Eigen::Quaterniond imu_stim300::Task::deltahead

protected

◆ deltaquat

Eigen::Quaterniond imu_stim300::Task::deltaquat

protected

The adaptive Indirect Kalman filter

◆ gyronoise

InertialNoiseParameters imu_stim300::Task::gyronoise

protected

Gyroscopes noise parameters

◆ imu_stim300_driver

imu_stim300::Stim300Base* imu_stim300::Task::imu_stim300_driver

protected

Driver variables

◆ incnoise

InertialNoiseParameters imu_stim300::Task::incnoise

protected

Inclinometers noise parameters

◆ initAttitude

bool imu_stim300::Task::initAttitude

protected

Initial Attitude

◆ initial_alignment_acc

Eigen::Matrix<double, 3, Eigen::Dynamic> imu_stim300::Task::initial_alignment_acc

protected

◆ initial_alignment_gyro

Eigen::Matrix<double, 3, Eigen::Dynamic> imu_stim300::Task::initial_alignment_gyro

protected

Initial values of Accelerometers/Inclinometers for Pitch and Roll calculation

◆ initial_alignment_idx

unsigned int imu_stim300::Task::initial_alignment_idx

protected

Index for initializing attitude

◆ location

LocationConfiguration imu_stim300::Task::location

protected

Location configuration variables

◆ myfilter

filter::Ikf<double, true, true> imu_stim300::Task::myfilter

protected

◆ oldomega

Eigen::Matrix4d imu_stim300::Task::oldomega

protected

◆ orientation_out

base::samples::RigidBodyState imu_stim300::Task::orientation_out

protected

Output port variables

◆ prev_ts

base::Time imu_stim300::Task::prev_ts

protected

◆ sampling_frequency

double imu_stim300::Task::sampling_frequency

protected

◆ timestamp_estimator

aggregator::TimestampEstimator* imu_stim300::Task::timestamp_estimator

protected

The documentation for this class was generated from the following files:

/build/rock-master-18.09-drivers-orogen-imu-stim300-0.20160804/tasks/Task.hpp
/build/rock-master-18.09-drivers-orogen-imu-stim300-0.20160804/tasks/Task.cpp

Public Member Functions

Static Public Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ Task() [1/2]

◆ Task() [2/2]

◆ ~Task()

Member Function Documentation

◆ cleanupHook()

◆ configureHook()

◆ deltaHeading()

◆ deltaQuaternion()

◆ errorHook()

◆ GravityModel()

◆ outputPortSamples()

◆ startHook()

◆ stopHook()

◆ SubtractEarthRotation()

◆ updateHook()

Friends And Related Function Documentation

◆ TaskBase

Member Data Documentation

◆ accnoise

◆ adaptiveconfigAcc

◆ adaptiveconfigInc

◆ attitude

◆ config

◆ correction_idx

◆ correction_numbers

◆ correctionAcc

◆ correctionInc

◆ deltahead

◆ deltaquat

◆ gyronoise

◆ imu_stim300_driver

◆ incnoise

◆ initAttitude

◆ initial_alignment_acc

◆ initial_alignment_gyro

◆ initial_alignment_idx

◆ location

◆ myfilter

◆ oldomega

◆ orientation_out

◆ prev_ts

◆ sampling_frequency

◆ timestamp_estimator