68 f->
dcm[0][0] = 0.0f; f->
dcm[0][1] = 0.0f; f->
dcm[0][2] = 0.0f;
69 f->
dcm[1][0] = 0.0f; f->
dcm[1][1] = 0.0f; f->
dcm[1][2] = 0.0f;
70 f->
dcm[2][0] = 0.0f; f->
dcm[2][1] = 0.0f; f->
dcm[2][2] = 0.0f;
235 float accx, accy, accz;
245 if( ( accValid ==
true ) && IMU_isAccelerationOK( f ) ) {
248 accAngle[0] = asinf( accy );
249 accAngle[1] = -asinf( accx );
258 accAngle[0] =
IMU_PI - asinf( accy );
259 accAngle[1] = -asinf( accx );
260 accAngle[2] =
IMU_PI + dirZ;
270 if( dirValid ==
false ) {
Structure to store the sensor fusion data.
void IMU_fuseGyroUpdate(IMU_SensorFusion *f, float gvec[3])
Updates the fusion calculation with a new gyro data.
void IMU_vectorNormalizeAngle(float v[3])
Normalizes the angle of a vector.
void IMU_dcmReset(float dcm[3][3])
Sets the elements of the DCM matrixto the corresponding elements of the identity matrix.
void IMU_fuseGyroCorrection(IMU_SensorFusion *f, bool accValid, bool dirValid, float dirZ)
Calculates the gyro correction vector.
void IMU_vectorScale(float v[3], float scale)
Scales a vector by a factor.
void IMU_dcmNormalize(float dcm[3][3])
Normalizes the DCM matrix.
#define IMU_MAX_ACCEL_FOR_ANGLE
void IMU_getGyroData(float gvec[3])
Retrieves the raw gyroscope data from the IMU.
void IMU_fuseAccelerometerUpdateFilter(IMU_SensorFusion *f, float avec[3])
The current accelerometer data is added to the accumulator.
void IMU_vectorZero(float v[3])
Sets all elements of a vector to 0.
void IMU_fuseAccelerometerSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the accelerometer sample rate in the IMU_SensorFusion structure.
void IMU_dcmRotate(float dcm[3][3], float ang[3])
Rotates the DCM matrix by a given angle.
void IMU_getAccelerometerData(float avec[3])
Retrieves the raw acceleration data from the IMU.
void IMU_fuseReset(IMU_SensorFusion *f)
Clears the values of the sensor fusion object.
Inertial Measurement Unit DCM matrix related routines.
void IMU_dcmGetAngles(float dcm[3][3], float ang[3])
Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.
void IMU_fuseGyroCorrectionClear(IMU_SensorFusion *f)
Clears the gyro correction vector.
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
void IMU_fuseNew(IMU_SensorFusion *f)
Initializes a new IMU_SensorFusion structure.
void IMU_vectorSubtract(float r[3], float a[3], float b[3])
Subtracts vector b from vector a.
void IMU_fuseUpdate(IMU_SensorFusion *f)
Updates the fusion calculation.
uint32_t aAccumulatorCount
void IMU_fuseGyroSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the gyro sample rate and related values in the IMU_SensorFusion structure.
#define IMU_DEG_TO_RAD_FACTOR
void IMU_vectorScalarMultiplication(float r[3], float v[3], float scale)
Multiplies a vector by a scalar.