/**
* Update IMU topic
* Function is called at main loop rate
*/
static void updateIMUTopic(void)
{
static float calibratedGravityCMSS = GRAVITY_CMSS;
if (!isImuReady()) {
posEstimator.imu.accelNEU.V.X = 0;
posEstimator.imu.accelNEU.V.Y = 0;
posEstimator.imu.accelNEU.V.Z = 0;
}
else {
t_fp_vector accelBF;
/* Read acceleration data in body frame */
accelBF.V.X = imuAccelInBodyFrame.V.X;
accelBF.V.Y = imuAccelInBodyFrame.V.Y;
accelBF.V.Z = imuAccelInBodyFrame.V.Z;
/* Correct accelerometer bias */
accelBF.V.X -= posEstimator.imu.accelBias.V.X;
accelBF.V.Y -= posEstimator.imu.accelBias.V.Y;
accelBF.V.Z -= posEstimator.imu.accelBias.V.Z;
/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
imuTransformVectorBodyToEarth(&accelBF);
/* Read acceleration data in NEU frame from IMU */
posEstimator.imu.accelNEU.V.X = accelBF.V.X;
posEstimator.imu.accelNEU.V.Y = accelBF.V.Y;
posEstimator.imu.accelNEU.V.Z = accelBF.V.Z;
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
//if (!ARMING_FLAG(ARMED) && imuRuntimeConfig->acc_unarmedcal) {
if (!ARMING_FLAG(ARMED) && posControl.navConfig->inav.accz_unarmed_cal) {
// Slowly converge on calibrated gravity while level
calibratedGravityCMSS += (posEstimator.imu.accelNEU.V.Z - calibratedGravityCMSS) * 0.0025f;
}
posEstimator.imu.accelNEU.V.Z -= calibratedGravityCMSS;
}
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
imuTransformVectorBodyToEarth(&accel_ned);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
