void IMU_Init(void) {
// Check which IMU is installed by trying to read ID registers
unsigned char id_adxl = I2C_ReadRegister(ADXL_ADDRESS, 0x00);
unsigned char id_itg = I2C_ReadRegister(ITG_ADDRESS, 0x00);
unsigned char id_hmc[3];
I2C_ReadMultipleRegisters(HMC_ADDRESS, 0x0A, &id_hmc[0], 3);
if ((id_adxl == 0xe5) &&
((id_itg & 0x7e) == ITG_ADDRESS) &&
(id_hmc[0] == 'H' && id_hmc[1] == '4' && id_hmc[2] == '3')) {
dbgu_printf("Found digital IMU!\r\n");
IMU_accel_init();
IMU_gyro_init();
IMU_magnet_init();
TC_DelayMs(100);
double temp = IMU_get_temp();
dbgu_printf("Current temperature: %f °C\r\n", temp);
imu_digital = 1;
} else {
imu_digital = 0;
}
// Todo: load values from eeprom
IMU_Update();
IMU_Calibrate_Accel();
#ifdef USE_KALMAN_FILTER
init_Gyro1DKalman(&filter_roll, Q_ACCEL, Q_GYRO, R_ACCEL);
init_Gyro1DKalman(&filter_pitch, Q_ACCEL, Q_GYRO, R_ACCEL);
float accX = ((float)(imu_AccelZ_raw - 512)) / 128.0f;
float accY = ((float)(imu_AccelY_raw - 512)) / 128.0f;
float accZ = ((float)(imu_AccelX_raw - 512)) / 128.0f;
pitch = atan2(accX, sqrt(accY * accY + accZ * accZ));
roll = atan2(accY, -accZ);
filter_roll.x_angle = roll;
filter_pitch.x_angle = pitch;
#endif
}
void IMU_Step(VehicleSense & vs, IMU_Euler & result, float SlewScale)
{
lGx=vs.Gx;
lGz=vs.Gz;
lGy=vs.Gy;
lAx=vs.Ax;
lAy=vs.Ay;
lAz=vs.Az;
lMx=vs.Mx;
lMy=vs.My;
lMz=vs.Mz;
IMU_Update(0.005,SlewScale);
float pitch, heading,bank;
fromInertialToObjectQuaternion(g_gyroFrame,pitch, heading, bank );
result.roll=bank;//*DEGREES_PER_RADIAN;
result.pitch=pitch;//*DEGREES_PER_RADIAN;
result.yaw=heading;//*DEGREES_PER_RADIAN;
}
