Detailed Description

Inertial Measurement Unit driver and support functions.

DO_NOT_INCLUDE_WITH_DOXYGEN

Functions
void	IMU_accelerationGet (int16_t avec[3])
	Retrieves the processed acceleration data. More...

void	IMU_clearDataReadyFlag (void)
	Clears the IMU data ready flag. In case of ver 0.3.0 and older of the PIC firmware the interrupt register also needs to be cleared. More...

void	IMU_config (float sampleRate)
	Configures the IMU. More...

void	IMU_dcmGetAngles (float dcm[3][3], float angle[3])
	Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix. More...

void	IMU_dcmNormalize (float dcm[3][3])
	Normalizes the DCM matrix. More...

void	IMU_dcmReset (float dcm[3][3])
	Sets the elements of the DCM matrixto the corresponding elements of the identity matrix. More...

void	IMU_dcmResetZ (float dcm[3][3])
	DCM reset, Z direction. More...

void	IMU_dcmRotate (float dcm[3][3], float angle[3])
	Rotates the DCM matrix by a given angle. More...

uint32_t	IMU_deInit (void)
	De-initializes the IMU chip. More...

void	IMU_fuseAccelerometerSetSampleRate (IMU_SensorFusion *f, float rate)
	Sets the accelerometer sample rate in the IMU_SensorFusion structure. More...

void	IMU_fuseAccelerometerUpdateFilter (IMU_SensorFusion *f, float avec[3])
	The current accelerometer data is added to the accumulator. More...

void	IMU_fuseGyroCorrection (IMU_SensorFusion *f, bool accValid, bool dirValid, float dirZ)
	Calculates the gyro correction vector. More...

void	IMU_fuseGyroCorrectionClear (IMU_SensorFusion *f)
	Clears the gyro correction vector. More...

void	IMU_fuseGyroSetSampleRate (IMU_SensorFusion *f, float rate)
	Sets the gyro sample rate and related values in the IMU_SensorFusion structure. More...

void	IMU_fuseGyroUpdate (IMU_SensorFusion *f, float gvec[3])
	Updates the fusion calculation with a new gyro data. More...

void	IMU_fuseNew (IMU_SensorFusion *f)
	Initializes a new IMU_SensorFusion structure. More...

void	IMU_fuseReset (IMU_SensorFusion *f)
	Clears the values of the sensor fusion object. More...

void	IMU_fuseUpdate (IMU_SensorFusion *f)
	Updates the fusion calculation. More...

void	IMU_getAccelerometerData (float avec[3])
	Retrieves the raw acceleration data from the IMU. More...

void	IMU_getGyroCorrectionAngles (float acorr[3])
	Retrieves the processed gyroscope correction angles. More...

void	IMU_getGyroData (float gvec[3])
	Retrieves the raw gyroscope data from the IMU. More...

uint8_t	IMU_getState (void)
	Returns IMU state. More...

uint32_t	IMU_gyroCalibrate (void)
	Performs gyroscope calibration to cancel gyro bias. More...

void	IMU_gyroGet (int16_t gvec[3])
	Retrieves the processed gyroscope data. More...

uint32_t	IMU_init (void)
	Initializes and calibrates the IMU. More...

bool	IMU_isDataReady (void)
	Checks if there is new accel/gyro data available for read. More...

bool	IMU_isDataReadyFlag (void)
	Checks if there is new accel/gyro data available for read. In case of ver 0.3.0 and older of the PIC firmware the state of the PIC_INT_WAKE needs to be read to determinte if the IMU interrupt is valid. More...

void	IMU_matrixMultiply (float c[3][3], float a[3][3], float b[3][3])
	Multiplies two 3x3 matrices. More...

void	IMU_normalizeAngle (float *a)
	Normalizes the angle ( -PI < angle <= PI ) More...

void	IMU_orientationGet (int16_t ovec[3])
	Retrieves the processed orientation data. More...

void	IMU_reset (void)
	Resets the fusion calculation. More...

void	IMU_update (void)
	Gets a new set of data from the accel and gyro sensor and updates the fusion calculation. More...

void	IMU_vectorAdd (float r[3], float a[3], float b[3])
	Adds two vectors. More...

void	IMU_vectorCrossProduct (float r[3], float a[3], float b[3])
	Calculates the cross product of two vectors. More...

float	IMU_vectorDotProduct (float a[3], float b[3])
	Calculates the dot product of two vectors. More...

void	IMU_vectorNormalizeAngle (float v[3])
	Normalizes the angle of a vector. More...

void	IMU_vectorScalarMultiplication (float r[3], float v[3], float scale)
	Multiplies a vector by a scalar. More...

void	IMU_vectorScale (float v[3], float scale)
	Scales a vector by a factor. More...

void	IMU_vectorSubtract (float r[3], float a[3], float b[3])
	Subtracts vector b from vector a. More...

void	IMU_vectorZero (float v[3])
	Sets all elements of a vector to 0. More...

Function Documentation

void IMU_accelerationGet ( int16_t avec[3] )

Retrieves the processed acceleration data.

Parameters

[out] avec Three dimensonal acceleration vector

Returns: None

Definition at line 245 of file imu.c.

References _IMU_SensorFusion::aAccumulator, and _IMU_SensorFusion::aAccumulatorCount.

void IMU_clearDataReadyFlag ( void )

Clears the IMU data ready flag. In case of ver 0.3.0 and older of the PIC firmware the interrupt register also needs to be cleared.

Returns: None

Definition at line 510 of file imu.c.

References BOARD_PIC_REG_INT_CLEAR, BOARD_picIsLegacyIntCtrl(), and BOARD_picWriteReg().

void IMU_config ( float sampleRate )

Configures the IMU.

Parameters

[in] sampleRate The desired sample rate of the acceleration and gyro sensor

Returns: None

IMU general use

Definition at line 177 of file imu.c.

References BOARD_imuEnableIRQ(), BOARD_PIC_INT_WAKE_PIN, BOARD_PIC_REG_INT_CLEAR, BOARD_picWriteReg(), GPIOINT_CallbackRegister(), ICM20648_ACCEL_BW_1210HZ, ICM20648_ACCEL_FULLSCALE_2G, ICM20648_accelBandwidthSet(), ICM20648_accelFullscaleSet(), ICM20648_accelSampleRateSet(), ICM20648_GYRO_BW_51HZ, ICM20648_GYRO_FULLSCALE_250DPS, ICM20648_gyroBandwidthSet(), ICM20648_gyroFullscaleSet(), ICM20648_gyroSampleRateSet(), ICM20648_interruptEnable(), ICM20648_interruptStatusRead(), ICM20648_lowPowerModeEnter(), ICM20648_sensorEnable(), IMU_fuseAccelerometerSetSampleRate(), IMU_fuseGyroSetSampleRate(), IMU_fuseReset(), IMU_state, IMU_STATE_INITIALIZING, IMU_STATE_READY, and UTIL_delay().

Referenced by IMU_gyroCalibrate().

void IMU_dcmGetAngles	(	float	dcm[3][3],
		float	angle[3]
	)

Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.

Parameters

[in]	dcm	DCM matrix
[out]	angle	An array containing the Euler angles

Returns: None

Definition at line 184 of file imu_dcm.c.

Referenced by IMU_fuseGyroUpdate().

void IMU_dcmNormalize ( float dcm[3][3] )

Normalizes the DCM matrix.

Parameters

dcm	DCM matrix

Returns: None

Definition at line 137 of file imu_dcm.c.

References IMU_vectorAdd(), IMU_vectorCrossProduct(), IMU_vectorDotProduct(), and IMU_vectorScalarMultiplication().

Referenced by IMU_fuseGyroUpdate().

void IMU_dcmReset ( float dcm[3][3] )

Sets the elements of the DCM matrixto the corresponding elements of the identity matrix.

Direction Cosine Matrix functions

Parameters

dcm	DCM matrix

Returns: None

Definition at line 44 of file imu_dcm.c.

Referenced by IMU_fuseReset().

void IMU_dcmResetZ ( float dcm[3][3] )

DCM reset, Z direction.

Parameters

dcm	DCM matrix

Returns: None

Definition at line 69 of file imu_dcm.c.

References IMU_vectorCrossProduct(), and IMU_vectorScale().

void IMU_dcmRotate	(	float	dcm[3][3],
		float	angle[3]
	)

Rotates the DCM matrix by a given angle.

Parameters

dcm	DCM matrix
angle	Rotation angle

Returns: None

Definition at line 96 of file imu_dcm.c.

References IMU_matrixMultiply().

Referenced by IMU_fuseGyroUpdate().

uint32_t IMU_deInit ( void )

De-initializes the IMU chip.

Returns: Returns zero on OK, non-zero otherwise

Definition at line 143 of file imu.c.

References ICM20648_deInit(), IMU_state, and IMU_STATE_DISABLED.

Referenced by IMU_gyroCalibrate().

void IMU_fuseAccelerometerSetSampleRate	(	IMU_SensorFusion *	f,
		float	rate
	)

Sets the accelerometer sample rate in the IMU_SensorFusion structure.

Sensor Fusion functions

Parameters

[out]	f	Pointer to the IMU_SensorFusion object
[in]	rate	Sample rate of the accelerometer

Returns: None

Definition at line 124 of file imu_fuse.c.

References _IMU_SensorFusion::gSampleRate.

Referenced by IMU_config().

void IMU_fuseAccelerometerUpdateFilter	(	IMU_SensorFusion *	f,
		float	avec[3]
	)

The current accelerometer data is added to the accumulator.

Parameters

[out]	f	Pointer to the IMU_SensorFusion object
[in]	avec	Accelerometer vector

Returns: None

Definition at line 146 of file imu_fuse.c.

References _IMU_SensorFusion::aAccumulator, and _IMU_SensorFusion::aAccumulatorCount.

Referenced by IMU_fuseUpdate().

void IMU_fuseGyroCorrection	(	IMU_SensorFusion *	f,
		bool	accValid,
		bool	dirValid,
		float	dirZ
	)

Calculates the gyro correction vector.

Parameters

[out]	f	Pointer to the IMU_SensorFusion object
[in]	accValid	True if the acceration value is within the limits
[in]	dirValid	True if the direction value is valid
[in]	dirZ	The direction of the Z axis

Returns: None

Definition at line 231 of file imu_fuse.c.

References _IMU_SensorFusion::angleCorrection, _IMU_SensorFusion::aVector, _IMU_SensorFusion::gSampleRate, IMU_PI, IMU_vectorNormalizeAngle(), IMU_vectorScale(), IMU_vectorSubtract(), IMU_vectorZero(), and _IMU_SensorFusion::orientation.

Referenced by IMU_fuseUpdate().

void IMU_fuseGyroCorrectionClear ( IMU_SensorFusion * f )

Clears the gyro correction vector.

Parameters

[out] f Pointer to the IMU_SensorFusion object

Returns: None

Definition at line 169 of file imu_fuse.c.

References _IMU_SensorFusion::angleCorrection, and IMU_vectorZero().

void IMU_fuseGyroSetSampleRate	(	IMU_SensorFusion *	f,
		float	rate
	)

Sets the gyro sample rate and related values in the IMU_SensorFusion structure.

Parameters

[out]	f	Pointer to the IMU_SensorFusion object
[in]	rate	Sample rate of the gyroscope

Returns: None

Definition at line 100 of file imu_fuse.c.

References _IMU_SensorFusion::gDeltaTime, _IMU_SensorFusion::gDeltaTimeScale, _IMU_SensorFusion::gSampleRate, and IMU_DEG_TO_RAD_FACTOR.

Referenced by IMU_config().

void IMU_fuseGyroUpdate	(	IMU_SensorFusion *	f,
		float	gvec[3]
	)

Updates the fusion calculation with a new gyro data.

Parameters

[out]	f	Pointer to the IMU_SensorFusion object
[in]	gvec	Gyroscope vector

Returns: None

Definition at line 191 of file imu_fuse.c.

References _IMU_SensorFusion::angleCorrection, _IMU_SensorFusion::dcm, _IMU_SensorFusion::gDeltaTimeScale, IMU_dcmGetAngles(), IMU_dcmNormalize(), IMU_dcmRotate(), IMU_vectorAdd(), IMU_vectorScalarMultiplication(), and _IMU_SensorFusion::orientation.

Referenced by IMU_fuseUpdate().

void IMU_fuseNew ( IMU_SensorFusion * f )

Initializes a new IMU_SensorFusion structure.

DO_NOT_INCLUDE_WITH_DOXYGEN

Parameters

[out] f Pointer to the IMU_SensorFusion object to be initialized

Returns: None

Definition at line 51 of file imu_fuse.c.

References _IMU_SensorFusion::aAccumulator, _IMU_SensorFusion::aAccumulatorCount, _IMU_SensorFusion::angleCorrection, _IMU_SensorFusion::aSampleRate, _IMU_SensorFusion::aVector, _IMU_SensorFusion::dcm, _IMU_SensorFusion::gDeltaTime, _IMU_SensorFusion::gSampleRate, _IMU_SensorFusion::gVector, and _IMU_SensorFusion::orientation.

Referenced by IMU_init().

void IMU_fuseReset ( IMU_SensorFusion * f )

Clears the values of the sensor fusion object.

Parameters

[out] f Pointer to the IMU_SensorFusion object

Returns: None

Definition at line 327 of file imu_fuse.c.

References _IMU_SensorFusion::aAccumulator, _IMU_SensorFusion::aAccumulatorCount, _IMU_SensorFusion::angleCorrection, _IMU_SensorFusion::aVector, _IMU_SensorFusion::dcm, _IMU_SensorFusion::gVector, IMU_dcmReset(), IMU_vectorZero(), and _IMU_SensorFusion::orientation.

Referenced by IMU_config(), and IMU_reset().

void IMU_fuseUpdate ( IMU_SensorFusion * f )

Updates the fusion calculation.

Parameters

[out] f Pointer to the IMU_SensorFusion object

Returns: None

Definition at line 353 of file imu_fuse.c.

References _IMU_SensorFusion::aVector, _IMU_SensorFusion::gVector, IMU_fuseAccelerometerUpdateFilter(), IMU_fuseGyroCorrection(), IMU_fuseGyroUpdate(), IMU_getAccelerometerData(), IMU_getGyroData(), IMU_state, and IMU_STATE_READY.

Referenced by IMU_update().

void IMU_getAccelerometerData ( float avec[3] )

Retrieves the raw acceleration data from the IMU.

Parameters

[out] avec Three dimensonal raw acceleration vector

Returns: None

Definition at line 378 of file imu.c.

References ICM20648_accelDataRead(), IMU_state, and IMU_STATE_READY.

Referenced by IMU_fuseUpdate().

void IMU_getGyroCorrectionAngles ( float acorr[3] )

Retrieves the processed gyroscope correction angles.

Parameters

[out] acorr Three dimensonal gyro correction angle vector

Returns: None

Definition at line 404 of file imu.c.

References _IMU_SensorFusion::angleCorrection.

void IMU_getGyroData ( float gvec[3] )

Retrieves the raw gyroscope data from the IMU.

Parameters

[out] gvec Three dimensonal raw gyro vector

Returns: None

Definition at line 425 of file imu.c.

References ICM20648_gyroDataRead(), IMU_state, and IMU_STATE_READY.

Referenced by IMU_fuseUpdate().

uint8_t IMU_getState ( void )

Returns IMU state.

Returns: Returns zero on OK, non-zero otherwise

Definition at line 162 of file imu.c.

References IMU_state.

uint32_t IMU_gyroCalibrate ( void )

Performs gyroscope calibration to cancel gyro bias.

Returns: None

Definition at line 315 of file imu.c.

References ICM20648_interruptEnable(), IMU_config(), IMU_deInit(), IMU_init(), and IMU_OK.

void IMU_gyroGet ( int16_t gvec[3] )

Retrieves the processed gyroscope data.

Parameters

[out] gvec Three dimensonal gyro vector

Returns: None

Definition at line 297 of file imu.c.

References _IMU_SensorFusion::gVector.

uint32_t IMU_init ( void )

Initializes and calibrates the IMU.

Returns: Returns zero on OK, non-zero otherwise

Module functions

Definition at line 82 of file imu.c.

References GPIOINT_Init(), ICM20648_deInit(), ICM20648_getDeviceID(), ICM20648_gyroCalibrate(), ICM20648_init(), ICM20648_OK, IMU_fuseNew(), IMU_state, IMU_STATE_CALIBRATING, IMU_STATE_DISABLED, and IMU_STATE_INITIALIZING.

Referenced by IMU_gyroCalibrate().

bool IMU_isDataReady ( void )

Checks if there is new accel/gyro data available for read.

Returns: True if the measurement data is ready, false otherwise

Definition at line 448 of file imu.c.

References ICM20648_isDataReady(), IMU_state, and IMU_STATE_READY.

bool IMU_isDataReadyFlag ( void )

Checks if there is new accel/gyro data available for read. In case of ver 0.3.0 and older of the PIC firmware the state of the PIC_INT_WAKE needs to be read to determinte if the IMU interrupt is valid.

Returns: True if data ready flag is set, false otherwise

Definition at line 478 of file imu.c.

References BOARD_PIC_INT_WAKE_PIN, BOARD_PIC_INT_WAKE_PORT, BOARD_picIsLegacyIntCtrl(), GPIO_PinInGet(), IMU_state, and IMU_STATE_READY.

void IMU_matrixMultiply	(	float	c[3][3],
		float	a[3][3],
		float	b[3][3]
	)

Multiplies two 3x3 matrices.

Parameters

[out]	c	The multiplication result, AB
[in]	a	Input vector A
[in]	b	Input vector B

Returns: None

Definition at line 72 of file imu_math.c.

Referenced by IMU_dcmRotate().

void IMU_normalizeAngle ( float * a )

Normalizes the angle ( -PI < angle <= PI )

Vector and Matrix math

Parameters

a	The angle to be normalized

Returns: None

Definition at line 41 of file imu_math.c.

References IMU_PI.

Referenced by IMU_vectorNormalizeAngle().

void IMU_orientationGet ( int16_t ovec[3] )

Retrieves the processed orientation data.

Parameters

[out] ovec Three dimensonal orientation vector

Returns: None

Definition at line 276 of file imu.c.

References IMU_RAD_TO_DEG_FACTOR, and _IMU_SensorFusion::orientation.

void IMU_reset ( void )

Resets the fusion calculation.

Returns: None

Definition at line 359 of file imu.c.

References IMU_fuseReset().

void IMU_update ( void )

Gets a new set of data from the accel and gyro sensor and updates the fusion calculation.

Returns: None

Definition at line 343 of file imu.c.

References IMU_fuseUpdate().

void IMU_vectorAdd	(	float	r[3],
		float	a[3],
		float	b[3]
	)

Adds two vectors.

Parameters

[out]	r	The vectorial sum of the vectors, a+b
[in]	a	The first vector
[in]	b	The second vector

Returns: None

Definition at line 208 of file imu_math.c.

Referenced by IMU_dcmNormalize(), and IMU_fuseGyroUpdate().

void IMU_vectorCrossProduct	(	float	r[3],
		float	a[3],
		float	b[3]
	)

Calculates the cross product of two vectors.

Parameters

[out]	r	The cross product
[in]	a	The first vector
[in]	b	The second vector

Returns: None

Definition at line 293 of file imu_math.c.

Referenced by IMU_dcmNormalize(), and IMU_dcmResetZ().

float IMU_vectorDotProduct	(	float	a[3],
		float	b[3]
	)

Calculates the dot product of two vectors.

Parameters

[in]	a	The first vector
[in]	b	The second vector

Returns: The dot product

Definition at line 262 of file imu_math.c.

Referenced by IMU_dcmNormalize().

void IMU_vectorNormalizeAngle ( float v[3] )

Normalizes the angle of a vector.

Parameters

v	The vector, which contains angles to be normalized

Returns: None

Definition at line 102 of file imu_math.c.

References IMU_normalizeAngle().

Referenced by IMU_fuseGyroCorrection().

void IMU_vectorScalarMultiplication	(	float	r[3],
		float	v[3],
		float	scale
	)

Multiplies a vector by a scalar.

Parameters

[out]	r	The multiplied vector
[in]	v	The vector to be multiplied
[in]	scale	The scalar multiplicator value

Returns: None

Definition at line 153 of file imu_math.c.

Referenced by IMU_dcmNormalize(), and IMU_fuseGyroUpdate().

void IMU_vectorScale	(	float	v[3],
		float	scale
	)

Scales a vector by a factor.

Parameters

	v	The vector to be scaled
[in]	scale	The scale factor

Returns: None

Definition at line 179 of file imu_math.c.

Referenced by IMU_dcmResetZ(), and IMU_fuseGyroCorrection().

void IMU_vectorSubtract	(	float	r[3],
		float	a[3],
		float	b[3]
	)

Subtracts vector b from vector a.

Parameters

[out]	r	The vectorial difference, a-b
[in]	a	Vector a
[in]	b	Vector b

Returns: None

Definition at line 237 of file imu_math.c.

Referenced by IMU_fuseGyroCorrection().

void IMU_vectorZero ( float v[3] )

Sets all elements of a vector to 0.

Parameters

v	The vector to be cleared

Returns: None

Definition at line 125 of file imu_math.c.

Referenced by IMU_fuseGyroCorrection(), IMU_fuseGyroCorrectionClear(), and IMU_fuseReset().

Detailed Description

Functions

Function Documentation