hdw-imu.c File Reference

Macros
#define	DSCL_OUTPUT
#define	DSCL_INPUT
#define	DSDA_OUTPUT
#define	DSDA_INPUT
#define	READ_DSDA   ((GPIO.in >> 3) & 1)
#define	DELAY1   i2c_delay(1);
#define	DELAY2   i2c_delay(2);
#define	LSM6DSL_ADDRESS   0x6a
#define	QMC6308_ADDRESS   0x2c

Enumerations
enum	lsm6dslReg_t {   LSM6DSL_FUNC_CFG_ACCESS = 0x01 , LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04 , LSM6DSL_FIFO_CTRL1 = 0x06 , LSM6DSL_FIFO_CTRL2 = 0x07 ,   LSM6DSL_FIFO_CTRL3 = 0x08 , LSM6DSL_FIFO_CTRL4 = 0x09 , LSM6DSL_FIFO_CTRL5 = 0x0a , LSM6DSL_ORIENT_CFG_G = 0x0b ,   LSM6DSL_INT1_CTRL = 0x0d , LSM6DSL_INT2_CTRL = 0x0e , LMS6DS3_WHO_AM_I = 0x0f , LSM6DSL_CTRL1_XL = 0x10 ,   LSM6DSL_CTRL2_G = 0x11 , LSM6DSL_CTRL3_C = 0x12 , LSM6DSL_CTRL4_C = 0x13 , LSM6DSL_CTRL5_C = 0x14 ,   LSM6DSL_CTRL6_C = 0x15 , LSM6DSL_CTRL7_G = 0x16 , LSM6DSL_CTRL8_XL = 0x17 , LSM6DSL_CTRL9_XL = 0x18 ,   LSM6DSL_CTRL10_C = 0x19 , LSM6DSL_MASTER_CONFIG = 0x1a , LSM6DSL_WAKE_UP_SRC = 0x1b , LSM6DSL_TAP_SRC = 0x1c ,   LSM6DSL_D6D_SRC = 0x1d , LSM6DSL_STATUS_REG = 0x1e , LSM6DSL_OUT_TEMP_L = 0x20 , LSM6DSL_OUT_TEMP_H = 0x21 ,   LSM6DSL_FIFO_STATUS1 = 0x3A }

Functions
void	i2c_delay (int x)
float	rsqrtf (float x)
	Perform a fast, approximate reciprocal square root.
float	mathsqrtf (float x)
	Perform a fast, approximate square root.
void	mathEulerToQuat (float *q, const float *euler)
	convert euler angles (in radians) to a quaternion.
void	mathQuatApply (float *qout, const float *q1, const float *q2)
	Rotate one quaternion by another (and do not normalize)
void	mathQuatNormalize (float *qout, const float *qin)
	Normalize a quaternion.
void	mathCrossProduct (float *p, const float *a, const float *b)
	Perform a 3D cross product.
void	mathRotateVectorByInverseOfQuaternion (float *pout, const float *q, const float *p)
	Rotate a 3D vector by the inverse of a quaternion.
void	mathRotateVectorByQuaternion (float *pout, const float *q, const float *p)
	Rotate a 3D vector by a quaternion.
esp_err_t	initAccelerometer (gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup)
	Initialize the IMU.
esp_err_t	deInitAccelerometer (void)
	Deinit the accelerometer (nothing to do)
esp_err_t	accelIntegrate ()
	Read all pending samples in IMU and perform a sensor fusion pass.
esp_err_t	accelGetAccelVecRaw (int16_t *x, int16_t *y, int16_t *z)
	Get the RAW accelerometer "up" vector from device point of view.
esp_err_t	accelGetOrientVec (int16_t *x, int16_t *y, int16_t *z)
	Get the current orientation "up" vector from device point of view.
esp_err_t	accelGetQuaternion (float *q)
	Get the current estimated quaternion of the IMU.
esp_err_t	accelPerformCal ()
	Initialize calibration of IMU.
esp_err_t	accelGetSteeringAngleDegrees (int16_t *xcomp, int16_t *ycomp)
	Get a steering wheel style input. Use getAtan2( xcomp, ycomp ) to get angle. 0..360 with 180 being up.
float	accelGetStdDevInCal ()
	Get current divergence as the cal algorithm attempts to converge on a good cal.
void	accelSetRegistersAndReset (void)
	Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU.

Variables
LSM6DSLData	LSM6DSL

Macro Definition Documentation

DSCL_OUTPUT

#define DSCL_OUTPUT

Value:

    {                                           \
        GPIO.enable1_w1ts.val = 1 << (41 - 32); \
    }

DSCL_INPUT

#define DSCL_INPUT

Value:

    {                                           \
        GPIO.enable1_w1tc.val = 1 << (41 - 32); \
    }

DSDA_OUTPUT

#define DSDA_OUTPUT

Value:

    {                                \
        GPIO.enable_w1ts = 1 << (3); \
    }

DSDA_INPUT

#define DSDA_INPUT

Value:

    {                                \
        GPIO.enable_w1tc = 1 << (3); \
    }

READ_DSDA

#define READ_DSDA   ((GPIO.in >> 3) & 1)

DELAY1

#define DELAY1   i2c_delay(1);

DELAY2

#define DELAY2   i2c_delay(2);

LSM6DSL_ADDRESS

#define LSM6DSL_ADDRESS   0x6a

QMC6308_ADDRESS

#define QMC6308_ADDRESS   0x2c

Enumeration Type Documentation

lsm6dslReg_t

enum lsm6dslReg_t

Enumerator
LSM6DSL_FUNC_CFG_ACCESS
LSM6DSL_SENSOR_SYNC_TIME_FRAME
LSM6DSL_FIFO_CTRL1
LSM6DSL_FIFO_CTRL2
LSM6DSL_FIFO_CTRL3
LSM6DSL_FIFO_CTRL4
LSM6DSL_FIFO_CTRL5
LSM6DSL_ORIENT_CFG_G
LSM6DSL_INT1_CTRL
LSM6DSL_INT2_CTRL
LMS6DS3_WHO_AM_I
LSM6DSL_CTRL1_XL
LSM6DSL_CTRL2_G
LSM6DSL_CTRL3_C
LSM6DSL_CTRL4_C
LSM6DSL_CTRL5_C
LSM6DSL_CTRL6_C
LSM6DSL_CTRL7_G
LSM6DSL_CTRL8_XL
LSM6DSL_CTRL9_XL
LSM6DSL_CTRL10_C
LSM6DSL_MASTER_CONFIG
LSM6DSL_WAKE_UP_SRC
LSM6DSL_TAP_SRC
LSM6DSL_D6D_SRC
LSM6DSL_STATUS_REG
LSM6DSL_OUT_TEMP_L
LSM6DSL_OUT_TEMP_H
LSM6DSL_FIFO_STATUS1

Function Documentation

i2c_delay()

void i2c_delay

(

int

x

)

rsqrtf()

float rsqrtf

(

float

x

)

Perform a fast, approximate reciprocal square root.

Parameters

x	The number to take a recriprocal square root of.

Returns

approximately 1/sqrt(x)

mathsqrtf()

float mathsqrtf

(

float

x

)

Perform a fast, approximate square root.

Parameters

x	The number to take a square root of.

Returns

approximately sqrt(x) (but is much faster)

mathEulerToQuat()

void mathEulerToQuat	(	float *	q,
		const float *	euler )

convert euler angles (in radians) to a quaternion.

Parameters

q	Pointer to the wxyz quat (float[4]) to be written.
euler	Pointer to a float[3] of euler angles.

mathQuatApply()

void mathQuatApply	(	float *	qout,
		const float *	q1,
		const float *	q2 )

Rotate one quaternion by another (and do not normalize)

Parameters

qout	Pointer to the wxyz quat (float[4]) to be written.
q1	First quaternion to be rotated.
q2	Quaternion to rotate q1 by.

mathQuatNormalize()

void mathQuatNormalize	(	float *	qout,
		const float *	qin )

Normalize a quaternion.

Parameters

qout	Pointer to the wxyz quat (float[4]) to be written.
qin	Pointer to the quaterion to normalize.

mathCrossProduct()

void mathCrossProduct	(	float *	p,
		const float *	a,
		const float *	b )

Perform a 3D cross product.

Parameters

p	Pointer to the float[3] output of the cross product (p = a x b)
a	Pointer to the float[3] of the cross product a vector.
b	Pointer to the float[3] of the cross product b vector.

mathRotateVectorByInverseOfQuaternion()

void mathRotateVectorByInverseOfQuaternion	(	float *	pout,
		const float *	q,
		const float *	p )

Rotate a 3D vector by the inverse of a quaternion.

Parameters

pout	Pointer to the float[3] output of the antirotation.
q	Pointer to the wxyz quaternion (float[4]) opposite of the rotation.
p	Pointer to the float[3] of the vector to antirotates.

mathRotateVectorByQuaternion()

void mathRotateVectorByQuaternion	(	float *	pout,
		const float *	q,
		const float *	p )

Rotate a 3D vector by a quaternion.

Parameters

pout	Pointer to the float[3] output of the rotation
q	Pointer to the wxyz quaternion (float[4]) of the rotation.
p	Pointer to the float[3] of the vector to rotates.

initAccelerometer()

esp_err_t initAccelerometer	(	gpio_num_t	sda,
		gpio_num_t	scl,
		gpio_pullup_t	pullup )

Initialize the IMU.

Parameters

sda	The GPIO for the Serial DAta line
scl	The GPIO for the Serial CLock line
pullup	Either GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or GPIO_PULLUP_ENABLE if internal pull-ups should be used

Returns

ESP_OK if the accelerometer initialized, or a non-zero value if it did not

deInitAccelerometer()

esp_err_t deInitAccelerometer

(

void

)

Deinit the accelerometer (nothing to do)

Returns

ESP_OK

accelIntegrate()

esp_err_t accelIntegrate

(

void

)

Read all pending samples in IMU and perform a sensor fusion pass.

Returns

ESP_OK if successful, or nonzero if error.

accelGetAccelVecRaw()

esp_err_t accelGetAccelVecRaw	(	int16_t *	x,
		int16_t *	y,
		int16_t *	z )

Get the RAW accelerometer "up" vector from device point of view.

Returns

ESP_OK

accelGetOrientVec()

esp_err_t accelGetOrientVec	(	int16_t *	x,
		int16_t *	y,
		int16_t *	z )

Get the current orientation "up" vector from device point of view.

Returns

ESP_OK

accelGetQuaternion()

esp_err_t accelGetQuaternion

(

float *

q

)

Get the current estimated quaternion of the IMU.

Returns

ESP_OK

accelPerformCal()

esp_err_t accelPerformCal

(

void

)

Initialize calibration of IMU.

Returns

ESP_OK If cal start was OK.

accelGetSteeringAngleDegrees()

esp_err_t accelGetSteeringAngleDegrees	(	int16_t *	xcomp,
		int16_t *	ycomp )

Get a steering wheel style input. Use getAtan2( xcomp, ycomp ) to get angle. 0..360 with 180 being up.

int16_t xcomp;
int16_t ycomp;
accelGetSteeringAngleDegrees( &xcomp, &ycomp );
ESP_LOGI( "x", "%d %d %d\n",  getAtan2( xcomp, ycomp ), xcomp, ycomp );

Parameters

xcomp	is a pointer to receive the x component of the steering wheel (-4096-4096) +x is "right"
ycomp	is a pointer to receive the y component of the steering wheel (-4096-4096) +y is "down"

Returns

Return ESP_OK if all is ok.

accelGetStdDevInCal()

float accelGetStdDevInCal

(

void

)

Get current divergence as the cal algorithm attempts to converge on a good cal.

Returns

If in cal mode, will return current motion deviation. If out of cal mode, will return 0.0f

accelSetRegistersAndReset()

void accelSetRegistersAndReset

(

void

)

Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU.

Variable Documentation

LSM6DSL

LSM6DSLData LSM6DSL