void imu6Init(void)
{
if(isInit)
return;
isHmc5883lPresent = false;
isMs5611Present = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C1);
mpu6050Init(I2C1);
if (mpu6050TestConnection() == true)
{
DEBUG_PRINT("MPU6050 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU6050 I2C connection [FAIL].\n");
}
mpu6050Reset();
vTaskDelay(M2T(50));
// Activate MPU6050
mpu6050SetSleepEnabled(false);
// Enable temp sensor
mpu6050SetTempSensorEnabled(true);
// Disable interrupts
mpu6050SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6050SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6050SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6050SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6050SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6050_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6050SetRate(15);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6050SetRate(1);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_HMC5883
hmc5883lInit(I2C1);
if (hmc5883lTestConnection() == true)
{
isHmc5883lPresent = true;
DEBUG_PRINT("HMC5883 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("HMC5883L I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_MS5611
if (ms5611Init(I2C1) == true)
{
isMs5611Present = true;
DEBUG_PRINT("MS5611 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MS5611 I2C connection [FAIL].\n");
}
#endif
imuBiasInit(&gyroBias);
imuBiasInit(&accelBias);
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cos(configblockGetCalibPitch() * M_PI/180);
sinPitch = sin(configblockGetCalibPitch() * M_PI/180);
cosRoll = cos(configblockGetCalibRoll() * M_PI/180);
sinRoll = sin(configblockGetCalibRoll() * M_PI/180);
isInit = true;
}
void dIMUInit(void) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
#ifdef DIMU_HAVE_MPU6000
mpu6000PreInit();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100PreInit();
#endif
#ifdef DIMU_HAVE_EEPROM
eepromPreInit();
#endif
#ifdef DIMU_HAVE_HMC5983
hmc5983PreInit();
#endif
#ifdef DIMU_HAVE_MS5611
ms5611PreInit();
#endif
#ifdef DIMU_HAVE_MPU6000
mpu6000Init();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100Init();
#endif
#ifdef DIMU_HAVE_EEPROM
eepromInit();
// dIMUWriteCalib();
dIMUReadCalib();
#endif
#ifdef DIMU_HAVE_HMC5983
if (hmc5983Init() == 0)
AQ_NOTICE("DIMU: MAG sensor init failed!\n");
#endif
#ifdef DIMU_HAVE_MS5611
if (ms5611Init() == 0)
AQ_NOTICE("DIMU: PRES sensor init failed!\n");
#endif
dIMUTaskStack = aqStackInit(DIMU_STACK_SIZE, "DIMU");
dImuData.flag = CoCreateFlag(1, 0);
dImuData.task = CoCreateTask(dIMUTaskCode, (void *)0, DIMU_PRIORITY, &dIMUTaskStack[DIMU_STACK_SIZE-1], DIMU_STACK_SIZE);
// setup digital IMU timer
DIMU_EN;
// Time base configuration for 1MHz (us)
TIM_TimeBaseStructure.TIM_Period = 0xffff;
TIM_TimeBaseStructure.TIM_Prescaler = (DIMU_CLOCK / 1000000) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(DIMU_TIM, &TIM_TimeBaseStructure);
// reset
TIM_SetCounter(DIMU_TIM, 0);
// Output Compare for alarms
TIM_OCStructInit(&TIM_OCInitStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC1Init(DIMU_TIM, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable);
TIM_OC2Init(DIMU_TIM, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(DIMU_TIM, TIM_OCPreload_Disable);
// Enable the global Interrupt
NVIC_InitStructure.NVIC_IRQChannel = DIMU_IRQ_CH;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// reset
TIM_SetCounter(DIMU_TIM, 0);
dIMUCancelAlarm1();
// go...
TIM_Cmd(DIMU_TIM, ENABLE);
#ifdef DIMU_HAVE_MPU6000
mpu6000Enable();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100Enable();
#endif
#ifdef DIMU_HAVE_HMC5983
hmc5983Enable();
#endif
#ifdef DIMU_HAVE_MS5611
ms5611Enable();
#endif
// setup IMU timestep alarm
dImuData.nextPeriod = DIMU_TIM->CCR2 + DIMU_INNER_PERIOD;
DIMU_TIM->CCR2 = dImuData.nextPeriod;
DIMU_TIM->DIER |= TIM_IT_CC2;
#ifdef DIMU_HAVE_MPU6000
mpu6600InitialBias();
#endif
#ifdef DIMU_HAVE_MAX21100
max21100InitialBias();
#endif
#ifdef DIMU_HAVE_MS5611
ms5611InitialBias();
#endif
}
