/** * 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++; }