IMU - Inertial Measurement UnitBSP > Thunderboard Sense BSP
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 |
#define IMU_STATE_CALIBRATING 0x03 |
#define IMU_STATE_DISABLED 0x00 |
The IMU is disabled
Definition at line 56
of file imu.h
.
Referenced by IMU_deInit(), and IMU_init().
#define IMU_STATE_INITIALIZING 0x02 |
The IMU is being initialized
Definition at line 58
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 57
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] avec
Three dimensonal acceleration vector
- Returns
- None
Definition at line 231
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 464
of file imu.c
.
References BOARD_imuClearIRQ().
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 165
of file imu.c
.
References BOARD_imuClearIRQ(), BOARD_imuEnableIRQ(), BOARD_imuSetIRQCallback(), 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_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 173
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 128
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 41
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 64
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 89
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 133
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] f
Pointer to the IMU_SensorFusion object [in] rate
Sample rate of the accelerometer
- Returns
- None
Definition at line 117
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 137
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 216
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] f
Pointer to the IMU_SensorFusion object
- Returns
- None
Definition at line 158
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 95
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] f
Pointer to the IMU_SensorFusion object [in] gvec
Gyroscope vector
- Returns
- None
Definition at line 178
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] f
Pointer to the IMU_SensorFusion object to be initialized
- Returns
- None
Definition at line 48
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 301
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 325
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] avec
Three dimensonal raw acceleration vector
- Returns
- None
Definition at line 351
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] acorr
Three dimensonal gyro correction angle vector
- Returns
- None
Definition at line 375
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 394
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 150
of file imu.c
.
uint32_t IMU_gyroCalibrate | ( | void |
| ) |
Performs gyroscope calibration to cancel gyro bias.
- Returns
- None
Definition at line 294
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 278
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 74
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 415
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 443
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] c
The multiplication result, AB [in] a
Input vector A [in] b
Input vector B
- Returns
- None
Definition at line 67
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 38
of file imu_math.c
.
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 259
of file imu.c
.
References _IMU_SensorFusion::orientation.
void IMU_reset | ( | void |
| ) |
Resets the fusion calculation.
- Returns
- None
Definition at line 334
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 320
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 193
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 273
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 244
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 94
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 142
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 166
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 220
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 115
of file imu_math.c
.
Referenced by IMU_fuseGyroCorrection(), IMU_fuseGyroCorrectionClear(), and IMU_fuseReset().