IMU - Inertial Measurement Unit

Detailed Description

Inertial Measurement Unit driver.

Data Structures

struct  _IMU_SensorFusion
 Structure to store the sensor fusion data.
 

Typedefs

typedef struct _IMU_SensorFusion IMU_SensorFusion
 Structure to store the sensor fusion data.
 

Functions

void IMU_accelerationGet (int16_t avec[3])
 Retrieves the processed acceleration data.
 
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.
 
void IMU_config (float sampleRate)
 Configures the IMU.
 
void IMU_dcmGetAngles (float dcm[3][3], float angle[3])
 Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.
 
void IMU_dcmNormalize (float dcm[3][3])
 Normalizes the DCM matrix.
 
void IMU_dcmReset (float dcm[3][3])
 Sets the elements of the DCM matrixto the corresponding elements of the identity matrix.
 
void IMU_dcmResetZ (float dcm[3][3])
 DCM reset, Z direction.
 
void IMU_dcmRotate (float dcm[3][3], float angle[3])
 Rotates the DCM matrix by a given angle.
 
uint32_t IMU_deInit (void)
 De-initializes the IMU chip.
 
void IMU_fuseAccelerometerSetSampleRate (IMU_SensorFusion *f, float rate)
 Sets the accelerometer sample rate in the IMU_SensorFusion structure.
 
void IMU_fuseAccelerometerUpdateFilter (IMU_SensorFusion *f, float avec[3])
 The current accelerometer data is added to the accumulator.
 
void IMU_fuseGyroCorrection (IMU_SensorFusion *f, bool accValid, bool dirValid, float dirZ)
 Calculates the gyro correction vector.
 
void IMU_fuseGyroCorrectionClear (IMU_SensorFusion *f)
 Clears the gyro correction vector.
 
void IMU_fuseGyroSetSampleRate (IMU_SensorFusion *f, float rate)
 Sets the gyro sample rate and related values in the IMU_SensorFusion structure.
 
void IMU_fuseGyroUpdate (IMU_SensorFusion *f, float gvec[3])
 Updates the fusion calculation with a new gyro data.
 
void IMU_fuseNew (IMU_SensorFusion *f)
 Initializes a new IMU_SensorFusion structure.
 
void IMU_fuseReset (IMU_SensorFusion *f)
 Clears the values of the sensor fusion object.
 
void IMU_fuseUpdate (IMU_SensorFusion *f)
 Updates the fusion calculation.
 
void IMU_getAccelerometerData (float avec[3])
 Retrieves the raw acceleration data from the IMU.
 
void IMU_getGyroCorrectionAngles (float acorr[3])
 Retrieves the processed gyroscope correction angles.
 
void IMU_getGyroData (float gvec[3])
 Retrieves the raw gyroscope data from the IMU.
 
uint8_t IMU_getState (void)
 Returns IMU state.
 
uint32_t IMU_gyroCalibrate (void)
 Performs gyroscope calibration to cancel gyro bias.
 
void IMU_gyroGet (int16_t gvec[3])
 Retrieves the processed gyroscope data.
 
uint32_t IMU_init (void)
 Initializes and calibrates the IMU.
 
bool IMU_isDataReady (void)
 Checks if there is new accel/gyro data available for read.
 
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.
 
void IMU_matrixMultiply (float c[3][3], float a[3][3], float b[3][3])
 Multiplies two 3x3 matrices.
 
void IMU_normalizeAngle (float *a)
 Normalizes the angle ( -PI < angle <= PI )
 
void IMU_orientationGet (int16_t ovec[3])
 Retrieves the processed orientation data.
 
void IMU_reset (void)
 Resets the fusion calculation.
 
void IMU_update (void)
 Gets a new set of data from the accel and gyro sensor and updates the fusion calculation.
 
void IMU_vectorAdd (float r[3], float a[3], float b[3])
 Adds two vectors.
 
void IMU_vectorCrossProduct (float r[3], float a[3], float b[3])
 Calculates the cross product of two vectors.
 
float IMU_vectorDotProduct (float a[3], float b[3])
 Calculates the dot product of two vectors.
 
void IMU_vectorNormalizeAngle (float v[3])
 Normalizes the angle of a vector.
 
void IMU_vectorScalarMultiplication (float r[3], float v[3], float scale)
 Multiplies a vector by a scalar.
 
void IMU_vectorScale (float v[3], float scale)
 Scales a vector by a factor.
 
void IMU_vectorSubtract (float r[3], float a[3], float b[3])
 Subtracts vector b from vector a.
 
void IMU_vectorZero (float v[3])
 Sets all elements of a vector to 0.
 

Error Codes

#define IMU_OK   0
 

State Definitions

#define IMU_STATE_DISABLED   0x00
 
#define IMU_STATE_READY   0x01
 
#define IMU_STATE_INITIALIZING   0x02
 
#define IMU_STATE_CALIBRATING   0x03
 

Macro Definition Documentation

#define IMU_OK   0

No errors

Definition at line 52 of file imu.h.

Referenced by IMU_gyroCalibrate().

#define IMU_STATE_CALIBRATING   0x03

The IMU is being calibrated

Definition at line 72 of file imu.h.

Referenced by IMU_init().

#define IMU_STATE_DISABLED   0x00

The IMU is disabled

Definition at line 69 of file imu.h.

Referenced by IMU_deInit(), and IMU_init().

#define IMU_STATE_INITIALIZING   0x02

The IMU is being initialized

Definition at line 71 of file imu.h.

Referenced by IMU_config(), and IMU_init().

#define IMU_STATE_READY   0x01

The IMU is fully configured and ready to take measurements

Definition at line 70 of file imu.h.

Referenced by IMU_config(), IMU_fuseUpdate(), IMU_getAccelerometerData(), IMU_getGyroData(), IMU_isDataReady(), and IMU_isDataReadyFlag().

Function Documentation

void IMU_accelerationGet ( int16_t  avec[3])

Retrieves the processed acceleration data.

Parameters
[out]avecThree dimensonal acceleration vector
Returns
None

Definition at line 244 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 477 of file imu.c.

References BOARD_imuClearIRQ().

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

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

Parameters
[in]dcmDCM matrix
[out]angleAn array containing the Euler angles
Returns
None

Definition at line 186 of file imu_dcm.c.

Referenced by IMU_fuseGyroUpdate().

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

Normalizes the DCM matrix.

Parameters
dcmDCM matrix
Returns
None

Definition at line 141 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
dcmDCM matrix
Returns
None

Definition at line 54 of file imu_dcm.c.

Referenced by IMU_fuseReset().

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

DCM reset, Z direction.

Parameters
dcmDCM matrix
Returns
None

Definition at line 77 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
dcmDCM matrix
angleRotation angle
Returns
None

Definition at line 102 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 146 of file imu.c.

References ICM20648_deInit(), 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]fPointer to the IMU_SensorFusion object
[in]rateSample rate of the accelerometer
Returns
None

Definition at line 130 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]fPointer to the IMU_SensorFusion object
[in]avecAccelerometer vector
Returns
None

Definition at line 150 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]fPointer to the IMU_SensorFusion object
[in]accValidTrue if the acceration value is within the limits
[in]dirValidTrue if the direction value is valid
[in]dirZThe direction of the Z axis
Returns
None

Definition at line 229 of file imu_fuse.c.

References _IMU_SensorFusion::angleCorrection, _IMU_SensorFusion::aVector, _IMU_SensorFusion::gSampleRate, 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]fPointer to the IMU_SensorFusion object
Returns
None

Definition at line 171 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]fPointer to the IMU_SensorFusion object
[in]rateSample rate of the gyroscope
Returns
None

Definition at line 108 of file imu_fuse.c.

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

Referenced by IMU_config().

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

Updates the fusion calculation with a new gyro data.

Parameters
[out]fPointer to the IMU_SensorFusion object
[in]gvecGyroscope 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.

Parameters
[out]fPointer to the IMU_SensorFusion object to be initialized
Returns
None

Definition at line 61 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]fPointer to the IMU_SensorFusion object
Returns
None

Definition at line 314 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]fPointer to the IMU_SensorFusion object
Returns
None

Definition at line 338 of file imu_fuse.c.

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

Referenced by IMU_update().

void IMU_getAccelerometerData ( float  avec[3])

Retrieves the raw acceleration data from the IMU.

Parameters
[out]avecThree dimensonal raw acceleration vector
Returns
None

Definition at line 364 of file imu.c.

References ICM20648_accelDataRead(), and IMU_STATE_READY.

Referenced by IMU_fuseUpdate().

void IMU_getGyroCorrectionAngles ( float  acorr[3])

Retrieves the processed gyroscope correction angles.

Parameters
[out]acorrThree dimensonal gyro correction angle vector
Returns
None

Definition at line 388 of file imu.c.

References _IMU_SensorFusion::angleCorrection.

void IMU_getGyroData ( float  gvec[3])

Retrieves the raw gyroscope data from the IMU.

Parameters
[out]gvecThree dimensonal raw gyro vector
Returns
None

Definition at line 407 of file imu.c.

References ICM20648_gyroDataRead(), 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 163 of file imu.c.

uint32_t IMU_gyroCalibrate ( void  )

Performs gyroscope calibration to cancel gyro bias.

Returns
None

Definition at line 307 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]gvecThree dimensonal gyro vector
Returns
None

Definition at line 291 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 87 of file imu.c.

References GPIOINT_Init(), ICM20648_deInit(), ICM20648_getDeviceID(), ICM20648_gyroCalibrate(), ICM20648_init(), ICM20648_OK, IMU_fuseNew(), 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 428 of file imu.c.

References ICM20648_isDataReady(), 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 456 of file imu.c.

References IMU_STATE_READY.

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

Multiplies two 3x3 matrices.

Parameters
[out]cThe multiplication result, AB
[in]aInput vector A
[in]bInput vector B
Returns
None

Definition at line 80 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
aThe angle to be normalized
Returns
None

Definition at line 51 of file imu_math.c.

Referenced by IMU_vectorNormalizeAngle().

void IMU_orientationGet ( int16_t  ovec[3])

Retrieves the processed orientation data.

Parameters
[out]ovecThree dimensonal orientation vector
Returns
None

Definition at line 272 of file imu.c.

References _IMU_SensorFusion::orientation.

void IMU_reset ( void  )

Resets the fusion calculation.

Returns
None

Definition at line 347 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 333 of file imu.c.

References IMU_fuseUpdate().

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

Adds two vectors.

Parameters
[out]rThe vectorial sum of the vectors, a+b
[in]aThe first vector
[in]bThe second vector
Returns
None

Definition at line 206 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]rThe cross product
[in]aThe first vector
[in]bThe second vector
Returns
None

Definition at line 286 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]aThe first vector
[in]bThe second vector
Returns
The dot product

Definition at line 257 of file imu_math.c.

Referenced by IMU_dcmNormalize().

void IMU_vectorNormalizeAngle ( float  v[3])

Normalizes the angle of a vector.

Parameters
vThe vector, which contains angles to be normalized
Returns
None

Definition at line 107 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]rThe multiplied vector
[in]vThe vector to be multiplied
[in]scaleThe scalar multiplicator value
Returns
None

Definition at line 155 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
vThe vector to be scaled
[in]scaleThe 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]rThe vectorial difference, a-b
[in]aVector a
[in]bVector b
Returns
None

Definition at line 233 of file imu_math.c.

Referenced by IMU_fuseGyroCorrection().

void IMU_vectorZero ( float  v[3])

Sets all elements of a vector to 0.

Parameters
vThe vector to be cleared
Returns
None

Definition at line 128 of file imu_math.c.

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