Detailed Description

Design Philosophy

Originally Swadges were planned to use a LSM6DSL and a QMC6308, however, because the batteries are so close to the magnetometer, the quality of the data was low enough we decided to proceed with with a LSM6DSL-only IMU.

Unlike the accelerometer process, the IMU fuses the gyroscope and accelerometer data from the LMS6DSL. By fusing both sensors, we are able to produce a quaternion to represent the rotation of the swadge. The idea is that we run the IMU at 208 Hz, and we use the hardware FIFO built into the LSM6DSL to queue up events. Then, every frame, we empty out the FIFO.

Usage

The core system will call initAccelerometer() and deInitAccelerometer() appropriately. And you can at any point call any of the proper IMU / accel functions.

accelSetRegistersAndReset() must be called when entering a Swadge mode that uses the IMU.

accelIntegrate() should be called periodically. This can be done either in the mode's main loop or in the background draw callback. If this is called from the background draw callback, make sure to only call it once per frame, since multiple callbacks are called per-frame.

The functions you can use to get acceleration data are:

accelGetAccelVecRaw()
accelGetQuaternion()

Example

// Declare variables to receive rotation
float q[4];
 
// Get the current rotation
if(ESP_OK == accelGetQuaternion( q ))
{
    // Print data to debug logs
    printf( "%f %f %f %f\n", q[0], q[1], q[2], q[3] );
}

Go to the source code of this file.

Data Structures
struct	LSM6DSLData

Functions
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	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	accelIntegrate (void)
	Read all pending samples in IMU and perform a sensor fusion pass.

esp_err_t	accelPerformCal (void)
	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 (void)
	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

Data Structure Documentation

◆ LSM6DSLData

struct LSM6DSLData

Data Fields
int32_t	temp
uint32_t	computetime
uint32_t	performCal
float	fqQuatLast[4]
float	fqQuat[4]
float	fvLastAccelRaw[3]
float	fvBias[3]
float	fvDeviation[3]
float	fvAverage[3]
uint32_t	sampCount
int	lastreadr
int32_t	gyroaccum[3]
int16_t	gyrolast[3]
int16_t	accellast[3]
float	fCorrectLast[3]

Function Documentation

◆ 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

◆ 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

◆ 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.

◆ 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

extern