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.
bool	getDeviceIdentifierString (id_string type, char id[17])
	Read one of the device identifier strings.
	IMU ()
	Constructor.
void	initGyros (double bias_x=0, double bias_y=0, double *bias_z=0)
	Initial gyros.
void	initTime (double fix_off)
	Initialize timing variables.
void	openPort (const char *port_name)
	Open the port.
void	receiveAccelAngrate (uint64_t *time, double accel[3], double angrate[3])
	Read a message of type "ACCEL_ANGRATE".
void	receiveAccelAngrateMag (uint64_t *time, double accel[3], double angrate[3], double mag[3])
	Read a message of type "ACCEL_ANGRATE_MAG".
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".
void	receiveAccelAngrateOrientation (uint64_t *time, double accel[3], double angrate[3], double orientation[9])
	Read a message of type "ACCEL_ANGRATE_ORIENTATION".
void	receiveDelvelDelang (uint64_t *time, double delvel[3], double delang[3])
	Read a message of type "DELVEL_DELANG".
void	receiveEuler (uint64_t time, double roll, double pitch, double yaw)
	Read a message of type "EULER".
void	receiveRawAccelAngrate (uint64_t *time, double accel[3], double angrate[3])
	Read a message of type "CMD_RAW".
bool	setContinuous (cmd command)
	Put the device in continuous mode.
void	setFixedOffset (double fix_off)
	Set the fixed time offset.
void	stopContinuous ()
	Take the device out of continous mode.
	~IMU ()
Static Public Attributes
static const double	G = 9.80665
	Gravity (m/sec^2)
Private Member Functions
uint64_t	extractTime (uint8_t *addr)
	Extract time from a pointer into an imu buffer.
uint64_t	filterTime (uint64_t imu_time, uint64_t sys_time)
	Run the filter on the imu time and system times.
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.
int	send (void *cmd, int cmd_len)
	Send a single packet frmo the IMU.
double	toDouble (uint64_t time)
	Convert the uint64_t time to a double for numerical computations.
uint64_t	toUint64_t (double time)
	Convert the double time back to a uint64_t.
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.
Private Attributes
bool	continuous
	Whether continuous mode is enabled.
unsigned int	counter
	A counter used by the filter.
double	d_offset
uint32_t	diff_ticks
	The different in the number of ticks.
int	fd
	The file descriptor.
double	fixed_offset
	Variables used by the kalman computation.
bool	is_gx3
	Is the IMU a GX3?
uint32_t	last_ticks
	The last number of ticks for computing wraparound.
double	offset
uint32_t	offset_ticks
	The number of ticks the initial offset is off by.
double	P_time_est [2][2]
	The covariances on time offset and driftrate.
unsigned long long	start_time
	The time at which the imu was started.
double	sum_meas
double	time_est [2]
	The estimate of time offset and driftrate.
uint32_t	wraps
	The number of times the imu has wrapped.
Static Private Attributes
static const double	KF_K_1 = 0.00995031
	First KF term.
static const double	KF_K_2 = 0.0000497506
	Second KF term.
static const unsigned int	KF_NUM_SUM = 100
	Number of KF samples to sum over.
static const int	MAX_BYTES_SKIPPED = 1000
	Maximum bytes allowed to be skipped when seeking a message.
static const int	TICKS_PER_SEC_GX2 = 19660800
	IMU internal ticks/second.
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.