A class for interfacing to the microstrain 3dmgx2 and inertialink IMUs. More...

#include <3dmgx2.h>

Public Types
enum	cmd { CMD_RAW = 0xC1, CMD_ACCEL_ANGRATE = 0xC2, CMD_DELVEL_DELANG = 0xC3, CMD_CONTINUOUS = 0xC4, CMD_ORIENT = 0xC5, CMD_ATT_UPDATE = 0xC6, CMD_MAG_VEC = 0xC7, CMD_ACCEL_ANGRATE_ORIENT = 0xC8, CMD_WRITE_ACCEL_BIAS = 0xC9, CMD_WRITE_GYRO_BIAS = 0xCA, CMD_ACCEL_ANGRATE_MAG = 0xCB, CMD_ACCEL_ANGRATE_MAG_ORIENT = 0xCC, CMD_CAPTURE_GYRO_BIAS = 0xCD, CMD_EULER = 0xCE, CMD_EULER_ANGRATE = 0xCF, CMD_TEMPERATURES = 0xD1, CMD_GYROSTAB_ANGRATE_MAG = 0xD2, CMD_DELVEL_DELANG_MAG = 0xD3, CMD_DEV_ID_STR = 0xEA, CMD_STOP_CONTINUOUS = 0xFA }
	Enumeration of possible IMU commands. More...

enum	id_string { ID_MODEL_NUMBER = 0, ID_SERIAL_NUMBER = 1, ID_DEVICE_NAME = 2, ID_DEVICE_OPTIONS = 3 }
	Enumeration of possible identifier strings for the getDeviceIdentifierString command. More...

Public Member Functions
void	closePort ()
	Close the port. More...

bool	getDeviceIdentifierString (id_string type, char id[17])
	Read one of the device identifier strings. More...

	IMU ()
	Constructor. More...

void	initGyros (double bias_x=0, double bias_y=0, double *bias_z=0)
	Initial gyros. More...

void	initTime (double fix_off)
	Initialize timing variables. More...

void	openPort (const char *port_name)
	Open the port. More...

void	receiveAccelAngrate (uint64_t *time, double accel[3], double angrate[3])
	Read a message of type "ACCEL_ANGRATE". More...

void	receiveAccelAngrateMag (uint64_t *time, double accel[3], double angrate[3], double mag[3])
	Read a message of type "ACCEL_ANGRATE_MAG". More...

void	receiveAccelAngrateMagOrientation (uint64_t *time, double accel[3], double angrate[3], double mag[3], double orientation[9])
	Read a message of type "ACCEL_ANGRATE_MAG_ORIENT". More...

void	receiveAccelAngrateOrientation (uint64_t *time, double accel[3], double angrate[3], double orientation[9])
	Read a message of type "ACCEL_ANGRATE_ORIENTATION". More...

void	receiveDelvelDelang (uint64_t *time, double delvel[3], double delang[3])
	Read a message of type "DELVEL_DELANG". More...

void	receiveEuler (uint64_t time, double roll, double pitch, double yaw)
	Read a message of type "EULER". More...

void	receiveRawAccelAngrate (uint64_t *time, double accel[3], double angrate[3])
	Read a message of type "CMD_RAW". More...

bool	setContinuous (cmd command)
	Put the device in continuous mode. More...

void	setFixedOffset (double fix_off)
	Set the fixed time offset. More...

void	stopContinuous ()
	Take the device out of continous mode. More...

	~IMU ()

Static Public Attributes
static const double	G = 9.80665
	Gravity (m/sec^2) More...

Private Member Functions
uint64_t	extractTime (uint8_t *addr)
	Extract time from a pointer into an imu buffer. More...

uint64_t	filterTime (uint64_t imu_time, uint64_t sys_time)
	Run the filter on the imu time and system times. More...

int	receive (uint8_t command, void rep, int rep_len, int timeout=0, uint64_t sys_time=NULL)
	Receive a particular message from the IMU. More...

int	send (void *cmd, int cmd_len)
	Send a single packet frmo the IMU. More...

double	toDouble (uint64_t time)
	Convert the uint64_t time to a double for numerical computations. More...

uint64_t	toUint64_t (double time)
	Convert the double time back to a uint64_t. More...

int	transact (void cmd, int cmd_len, void rep, int rep_len, int timeout=0)
	Send a command to the IMU and wait for a reply. More...

Private Attributes
bool	continuous
	Whether continuous mode is enabled. More...

unsigned int	counter
	A counter used by the filter. More...

double	d_offset

uint32_t	diff_ticks
	The different in the number of ticks. More...

int	fd
	The file descriptor. More...

double	fixed_offset
	Variables used by the kalman computation. More...

bool	is_gx3
	Is the IMU a GX3? More...

uint32_t	last_ticks
	The last number of ticks for computing wraparound. More...

double	offset

uint32_t	offset_ticks
	The number of ticks the initial offset is off by. More...

double	P_time_est [2][2]
	The covariances on time offset and driftrate. More...

unsigned long long	start_time
	The time at which the imu was started. More...

double	sum_meas

double	time_est [2]
	The estimate of time offset and driftrate. More...

uint32_t	wraps
	The number of times the imu has wrapped. More...

Static Private Attributes
static const double	KF_K_1 = 0.00995031
	First KF term. More...

static const double	KF_K_2 = 0.0000497506
	Second KF term. More...

static const unsigned int	KF_NUM_SUM = 100
	Number of KF samples to sum over. More...

static const int	MAX_BYTES_SKIPPED = 1000
	Maximum bytes allowed to be skipped when seeking a message. More...

static const int	TICKS_PER_SEC_GX2 = 19660800
	IMU internal ticks/second. More...

static const int	TICKS_PER_SEC_GX3 = 62500

Detailed Description

A class for interfacing to the microstrain 3dmgx2 and inertialink IMUs.

Note: This class is unreviewed and unsupported. It may change at any time without notice.

Many of the methods within this class may throw an microstrain_3dmgx2_imu::exception, timeout_exception, or corrupted_data_exception.

Before using the IMU, it must be opened via the open_port method. When finished using, it should be closed via the close_port method. Alternatively, close port will get called at destruction.

The library is primarily designed to be used in continuous mode, which is enabled with the set_continuous method, and then serviced with one of the receive methods.

Implementation of specific polled message transactions can be done with the transact method.

Because the timing related to the USB stack is fairly non-deterministic, but the IMU is internally known to be clocked to a 100hz clock, we have wrapped a Kalman filter around calls to get system time, and the internal imu time. This is only known to be reliable when operating in continuous mode, and if init_time is called shortly prior to beginning to get readings.

Example code:

microstrain_3dmgx2_imu::IMU imu;
imu.open_port("/dev/ttyUSB0");
imu.init_time();
imu.init_gyros();
imu.set_continuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_ORIENT);
while (int i = 0 ; i < 100; i++)
{
  double accel[3];
  double angrate[3];
  double orientation[9];
  imu.receive_accel_angrate_orientation(&time, accel, angrate, orientation);
}
imu.close_port();

Definition at line 91 of file 3dmgx2.h.

Member Enumeration Documentation

enum microstrain_3dmgx2_imu::IMU::cmd

Enumeration of possible IMU commands.

Enumerator
CMD_RAW
CMD_ACCEL_ANGRATE
CMD_DELVEL_DELANG
CMD_CONTINUOUS
CMD_ORIENT
CMD_ATT_UPDATE
CMD_MAG_VEC
CMD_ACCEL_ANGRATE_ORIENT
CMD_WRITE_ACCEL_BIAS
CMD_WRITE_GYRO_BIAS
CMD_ACCEL_ANGRATE_MAG
CMD_ACCEL_ANGRATE_MAG_ORIENT
CMD_CAPTURE_GYRO_BIAS
CMD_EULER
CMD_EULER_ANGRATE
CMD_TEMPERATURES
CMD_GYROSTAB_ANGRATE_MAG
CMD_DELVEL_DELANG_MAG
CMD_DEV_ID_STR
CMD_STOP_CONTINUOUS

Definition at line 118 of file 3dmgx2.h.

enum microstrain_3dmgx2_imu::IMU::id_string

Enumeration of possible identifier strings for the getDeviceIdentifierString command.

Enumerator
ID_MODEL_NUMBER
ID_SERIAL_NUMBER
ID_DEVICE_NAME
ID_DEVICE_OPTIONS

Definition at line 143 of file 3dmgx2.h.

Constructor & Destructor Documentation

microstrain_3dmgx2_imu::IMU::IMU ( )

Constructor.

Definition at line 98 of file 3dmgx2.cc.

microstrain_3dmgx2_imu::IMU::~IMU ( )

Definition at line 104 of file 3dmgx2.cc.

Member Function Documentation

void microstrain_3dmgx2_imu::IMU::closePort ( )

Close the port.

Definition at line 170 of file 3dmgx2.cc.

uint64_t microstrain_3dmgx2_imu::IMU::extractTime ( uint8_t * addr )

private

Extract time from a pointer into an imu buffer.

Definition at line 587 of file 3dmgx2.cc.

uint64_t microstrain_3dmgx2_imu::IMU::filterTime	(	uint64_t	imu_time,
		uint64_t	sys_time
	)

private

Run the filter on the imu time and system times.

Definition at line 724 of file 3dmgx2.cc.

bool microstrain_3dmgx2_imu::IMU::getDeviceIdentifierString	(	id_string	type,
		char	id[17]
	)

Read one of the device identifier strings.

Parameters

type	Indicates which identifier string to read
id	Array that gets filled with the identifier string

Returns: True if successful

Definition at line 476 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::initGyros	(	double *	bias_x = `0`,
		double *	bias_y = `0`,
		double *	bias_z = `0`
	)

Initial gyros.

This call will prompt the IMU to run its gyro initialization routine.

NOTE: THE IMU MUST BE STATIONARY WHEN THIS ROUTINE IS CALLED

Parameters

bias_x	Pointer to double where x bias will be placed.
bias_y	Pointer to double where y bias will be placed.
bias_z	Pointer to double where z bias will be placed.

Definition at line 224 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::initTime ( double fix_off )

Initialize timing variables.

This call determines the initial offset of the imu relative to system clock time, and resets the kalman filter state.

Parameters

fix_off this fixed offset will be added to the timestamp of the imu

Definition at line 196 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::openPort ( const char * port_name )

Open the port.

This must be done before the imu can be used.

Parameters

port_name A character array containing the name of the port

Todo:: tcsetattr returns true if at least one attribute was set. Hence, we might not have set everything on success.

Definition at line 113 of file 3dmgx2.cc.

int microstrain_3dmgx2_imu::IMU::receive	(	uint8_t	command,
		void *	rep,
		int	rep_len,
		int	timeout = `0`,
		uint64_t *	sys_time = `NULL`
	)

private

Receive a particular message from the IMU.

Definition at line 671 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveAccelAngrate	(	uint64_t *	time,
		double	accel[3],
		double	angrate[3]
	)

Read a message of type "ACCEL_ANGRATE".

Parameters

time	Pointer to uint64_t which will receive time
accel	array of accelerations which will be filled
angrate	array of angular rates which will be filled

Definition at line 389 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveAccelAngrateMag	(	uint64_t *	time,
		double	accel[3],
		double	angrate[3],
		double	mag[3]
	)

Read a message of type "ACCEL_ANGRATE_MAG".

Parameters

time	Pointer to uint64_t which will receive time
accel	array of accelerations which will be filled
angrate	array of angular rates which will be filled
mag	array of magnetometer orientations which will be filled

Definition at line 304 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveAccelAngrateMagOrientation	(	uint64_t *	time,
		double	accel[3],
		double	angrate[3],
		double	mag[3],
		double	orientation[9]
	)

Read a message of type "ACCEL_ANGRATE_MAG_ORIENT".

Parameters

time	Pointer to uint64_t which will receive time
accel	array of accelerations which will be filled
angrate	array of angular rates which will be filled
mag	array of magnetometer orientations which will be filled
orientation	orientation matrix which will be filled

Definition at line 505 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveAccelAngrateOrientation	(	uint64_t *	time,
		double	accel[3],
		double	angrate[3],
		double	orientation[9]
	)

Read a message of type "ACCEL_ANGRATE_ORIENTATION".

Parameters

time	Pointer to uint64_t which will receive time
accel	array of accelerations which will be filled
angrate	array of angular rates which will be filled
orientation	orientation matrix which will be filled

Definition at line 346 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveDelvelDelang	(	uint64_t *	time,
		double	delvel[3],
		double	delang[3]
	)

Read a message of type "DELVEL_DELANG".

Parameters

time	Pointer to uint64_t which will receive time
delvel	array of accelerations which will be filled
delang	array of angular rates which will be filled

Definition at line 422 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveEuler	(	uint64_t *	time,
		double *	roll,
		double *	pitch,
		double *	yaw
	)

Read a message of type "EULER".

Parameters

time	Pointer to uint64_t which will receive time
roll	Pointer to roll value which will be filled
pitch	Pointer to pitch value which will be filled
yaw	Pointer to yaw value which will be filled

Definition at line 456 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::receiveRawAccelAngrate	(	uint64_t *	time,
		double	accel[3],
		double	angrate[3]
	)

Read a message of type "CMD_RAW".

Parameters

time	Pointer to uint64_t which will receive time
accel	array of accelerations which will be filled
angrate	array of angular rates which will be filled

Definition at line 553 of file 3dmgx2.cc.

int microstrain_3dmgx2_imu::IMU::send	(	void *	cmd,
		int	cmd_len
	)

private

Send a single packet frmo the IMU.

Definition at line 620 of file 3dmgx2.cc.

bool microstrain_3dmgx2_imu::IMU::setContinuous ( cmd command )

Put the device in continuous mode.

This call puts the IMU into a mode where it is continuously outputting a particular message.

Parameters

command The type of message to be output.

Returns: Whether or not continuous mode was enabled successfully.

Definition at line 252 of file 3dmgx2.cc.

void microstrain_3dmgx2_imu::IMU::setFixedOffset ( double fix_off )

inline

Set the fixed time offset.

Parameters

fix_off Fixed time offset in seconds

Definition at line 269 of file 3dmgx2.h.

void microstrain_3dmgx2_imu::IMU::stopContinuous ( )

Take the device out of continous mode.

Definition at line 277 of file 3dmgx2.cc.

double microstrain_3dmgx2_imu::IMU::toDouble ( uint64_t time )

private

Convert the uint64_t time to a double for numerical computations.

Definition at line 750 of file 3dmgx2.cc.

uint64_t microstrain_3dmgx2_imu::IMU::toUint64_t ( double time )

private

Convert the double time back to a uint64_t.

Definition at line 760 of file 3dmgx2.cc.

int microstrain_3dmgx2_imu::IMU::transact	(	void *	cmd,
		int	cmd_len,
		void *	rep,
		int	rep_len,
		int	timeout = `0`
	)

private

Send a command to the IMU and wait for a reply.

Definition at line 608 of file 3dmgx2.cc.

Member Data Documentation

bool microstrain_3dmgx2_imu::IMU::continuous

private

Whether continuous mode is enabled.

Definition at line 326 of file 3dmgx2.h.

unsigned int microstrain_3dmgx2_imu::IMU::counter

private

A counter used by the filter.

Definition at line 329 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::d_offset

private

Definition at line 332 of file 3dmgx2.h.

uint32_t microstrain_3dmgx2_imu::IMU::diff_ticks

private

The different in the number of ticks.

Definition at line 314 of file 3dmgx2.h.

int microstrain_3dmgx2_imu::IMU::fd

private

The file descriptor.

Definition at line 302 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::fixed_offset

private

Variables used by the kalman computation.

Definition at line 332 of file 3dmgx2.h.

const double microstrain_3dmgx2_imu::IMU::G = 9.80665

static

Gravity (m/sec^2)

Definition at line 112 of file 3dmgx2.h.

bool microstrain_3dmgx2_imu::IMU::is_gx3

private

Is the IMU a GX3?

Definition at line 335 of file 3dmgx2.h.

const double microstrain_3dmgx2_imu::IMU::KF_K_1 = 0.00995031

staticprivate

First KF term.

Definition at line 105 of file 3dmgx2.h.

const double microstrain_3dmgx2_imu::IMU::KF_K_2 = 0.0000497506

staticprivate

Second KF term.

Definition at line 107 of file 3dmgx2.h.

const unsigned int microstrain_3dmgx2_imu::IMU::KF_NUM_SUM = 100

staticprivate

Number of KF samples to sum over.

Definition at line 103 of file 3dmgx2.h.

uint32_t microstrain_3dmgx2_imu::IMU::last_ticks

private

The last number of ticks for computing wraparound.

Definition at line 311 of file 3dmgx2.h.

const int microstrain_3dmgx2_imu::IMU::MAX_BYTES_SKIPPED = 1000

staticprivate

Maximum bytes allowed to be skipped when seeking a message.

Definition at line 101 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::offset

private

Definition at line 332 of file 3dmgx2.h.

uint32_t microstrain_3dmgx2_imu::IMU::offset_ticks

private

The number of ticks the initial offset is off by.

Definition at line 308 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::P_time_est[2][2]

private

The covariances on time offset and driftrate.

Definition at line 323 of file 3dmgx2.h.

unsigned long long microstrain_3dmgx2_imu::IMU::start_time

private

The time at which the imu was started.

Definition at line 317 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::sum_meas

private

Definition at line 332 of file 3dmgx2.h.

const int microstrain_3dmgx2_imu::IMU::TICKS_PER_SEC_GX2 = 19660800

staticprivate

IMU internal ticks/second.

Definition at line 98 of file 3dmgx2.h.

const int microstrain_3dmgx2_imu::IMU::TICKS_PER_SEC_GX3 = 62500

staticprivate

Definition at line 99 of file 3dmgx2.h.

double microstrain_3dmgx2_imu::IMU::time_est[2]

private

The estimate of time offset and driftrate.

Definition at line 320 of file 3dmgx2.h.

uint32_t microstrain_3dmgx2_imu::IMU::wraps

private

The number of times the imu has wrapped.

Definition at line 305 of file 3dmgx2.h.

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

Public Types

Public Member Functions

Static Public Attributes

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation