void sensorsMpu9250Lps25hSetAccMode(accModes accMode)
{
switch (accMode)
{
case ACC_MODE_PROPTEST:
mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_460);
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&accLpf[i], 1000, 500);
}
break;
case ACC_MODE_FLIGHT:
default:
mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_41);
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ);
}
break;
}
}
static void sensorsDeviceInit(void)
{
isMagnetometerPresent = false;
isBarometerPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < 1000);
i2cdevInit(I2C3_DEV);
mpu6500Init(I2C3_DEV);
if (mpu6500TestConnection() == true)
{
DEBUG_PRINT("MPU9250 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n");
}
mpu6500Reset();
vTaskDelay(M2T(50));
// Activate MPU6500
mpu6500SetSleepEnabled(false);
// Delay until registers are reset
vTaskDelay(M2T(100));
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Delay until clock is set and stable
vTaskDelay(M2T(200));
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the MAG and BARO to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(SENSORS_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(SENSORS_ACCEL_FS_CFG);
// Set accelerometer digital low-pass bandwidth
mpu6500SetAccelDLPF(MPU6500_ACCEL_DLPF_BW_41);
#if SENSORS_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 7) = 1000Hz
mpu6500SetRate(7);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_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 + 0) = 1000Hz
mpu6500SetRate(0);
// Set digital low-pass bandwidth for gyro
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
// Init second order filer for accelerometer
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&gyroLpf[i], 1000, GYRO_LPF_CUTOFF_FREQ);
lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ);
}
#endif
#ifdef SENSORS_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagnetometerPresent = true;
ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
DEBUG_PRINT("AK8963 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
}
#endif
#ifdef SENSORS_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBarometerPresent = true;
DEBUG_PRINT("LPS25H I2C connection [OK].\n");
}
else
{
//TODO: Should sensor test fail hard if no connection
DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
}
#endif
cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI/180);
sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI/180);
cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI/180);
sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI/180);
}