void imuUpdateGyroAndAttitude(void)
{
/* Calculate dT */
static uint32_t previousIMUUpdateTime;
uint32_t currentTime = micros();
float dT = (currentTime - previousIMUUpdateTime) * 1e-6;
previousIMUUpdateTime = currentTime;
/* Update gyroscope */
gyroUpdate();
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
#ifdef HIL
if (!hilActive) {
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
}
else {
imuHILUpdate();
imuUpdateMeasuredAcceleration();
}
#else
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
void imuUpdateGyroAndAttitude(void)
{
gyroUpdate();
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
gyroUpdate();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
}
