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;
}
static void sensorsDeviceInit(void)
{
if (isInit)
return;
bstdr_ret_t rslt;
isBarometerPresent = false;
// Wait for sensors to startup
vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
/* BMI160 */
// assign bus read function
bmi160Dev.read = (bmi160_com_fptr_t)bstdr_burst_read;
// assign bus write function
bmi160Dev.write = (bmi160_com_fptr_t)bstdr_burst_write;
// assign delay function
bmi160Dev.delay_ms = (bmi160_delay_fptr_t)bstdr_ms_delay;
bmi160Dev.id = BMI160_I2C_ADDR+1; // I2C device address
rslt = bmi160_init(&bmi160Dev); // initialize the device
if (rslt == BSTDR_OK)
{
DEBUG_PRINT("BMI160 I2C connection [OK].\n");
/* Select the Output data rate, range of Gyroscope sensor
* ~92Hz BW by OSR4 @ODR=800Hz */
bmi160Dev.gyro_cfg.odr = BMI160_GYRO_ODR_800HZ;
bmi160Dev.gyro_cfg.range = SENSORS_BMI160_GYRO_FS_CFG;
bmi160Dev.gyro_cfg.bw = BMI160_GYRO_BW_OSR4_MODE;
/* Select the power mode of Gyroscope sensor */
bmi160Dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
/* Select the Output data rate, range of accelerometer sensor
* ~92Hz BW by OSR4 @ODR=800Hz */
bmi160Dev.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ;
bmi160Dev.accel_cfg.range = SENSORS_BMI160_ACCEL_FS_CFG;
bmi160Dev.accel_cfg.bw = BMI160_ACCEL_BW_OSR4_AVG1;
/* Select the power mode of accelerometer sensor */
bmi160Dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
/* Set the sensor configuration */
rslt |= bmi160_set_sens_conf(&bmi160Dev);
bmi160Dev.delay_ms(50);
/* read sensor */
struct bmi160_sensor_data gyr;
rslt |= bmi160_get_sensor_data(BMI160_GYRO_ONLY, NULL, &gyr,
&bmi160Dev);
struct bmi160_sensor_data acc;
rslt |= bmi160_get_sensor_data(BMI160_ACCEL_ONLY, &acc, NULL,
&bmi160Dev);
}
else
{
DEBUG_PRINT("BMI160 I2C connection [FAIL].\n");
}
/* BMI055 */
bmi055Dev.accel_id = BMI055_ACCEL_I2C_ADDR;
bmi055Dev.gyro_id = BMI055_GYRO_I2C_ADDR;
bmi055Dev.interface = BMI055_I2C_INTF;
bmi055Dev.read = (bmi055_com_fptr_t)bstdr_burst_read;
bmi055Dev.write = (bmi055_com_fptr_t)bstdr_burst_write;
bmi055Dev.delay_ms = (bmi055_delay_fptr_t)bstdr_ms_delay;
/* BMI055 GYRO */
rslt = bmi055_gyro_init(&bmi055Dev); // initialize the device
if (rslt == BSTDR_OK)
{
DEBUG_PRINT("BMI055 Gyro I2C connection [OK].\n");
/* set power mode of gyro */
bmi055Dev.gyro_cfg.power = BMI055_GYRO_PM_NORMAL;
rslt |= bmi055_set_gyro_power_mode(&bmi055Dev);
/* set bandwidth and range of gyro */
bmi055Dev.gyro_cfg.bw = BMI055_GYRO_BW_116_HZ;
bmi055Dev.gyro_cfg.range = SENSORS_BMI055_GYRO_FS_CFG;
rslt |= bmi055_set_gyro_sensor_config(CONFIG_ALL, &bmi055Dev);
bmi055Dev.delay_ms(50);
struct bmi055_sensor_data gyr;
rslt |= bmi055_get_gyro_data(&gyr, &bmi055Dev);
}
else
{
DEBUG_PRINT("BMI055 Gyro I2C connection [FAIL].\n");
}
/* BMI055 ACCEL */
rslt = bmi055_accel_init(&bmi055Dev); // initialize the device
if (rslt == BSTDR_OK)
{
DEBUG_PRINT("BMI055 Accel I2C connection [OK].\n");
/* set power mode of accel */
bmi055Dev.accel_cfg.power = BMI055_ACCEL_PM_NORMAL;
rslt |= bmi055_set_accel_power_mode(&bmi055Dev);
/* set bandwidth and range of accel */
bmi055Dev.accel_cfg.bw = BMI055_ACCEL_BW_125_HZ;
bmi055Dev.accel_cfg.range = SENSORS_BMI055_ACCEL_FS_CFG;
rslt |= bmi055_set_accel_sensor_config(CONFIG_ALL, &bmi055Dev);
bmi055Dev.delay_ms(10);
struct bmi055_sensor_data acc;
rslt |= bmi055_get_accel_data(&acc, &bmi055Dev);
}
else
{
DEBUG_PRINT("BMI055 Accel I2C connection [FAIL].\n");
}
/* BMM150 */
rslt = BSTDR_E_GEN_ERROR;
/* Sensor interface over I2C */
bmm150Dev.id = BMM150_DEFAULT_I2C_ADDRESS;
bmm150Dev.interface = BMM150_I2C_INTF;
bmm150Dev.read = (bmm150_com_fptr_t)bstdr_burst_read;
bmm150Dev.write = (bmm150_com_fptr_t)bstdr_burst_write;
bmm150Dev.delay_ms = bstdr_ms_delay;
rslt = bmm150_init(&bmm150Dev);
if (rslt == BMM150_OK){
bmm150Dev.settings.pwr_mode = BMM150_NORMAL_MODE;
rslt |= bmm150_set_op_mode(&bmm150Dev);
bmm150Dev.settings.preset_mode = BMM150_PRESETMODE_HIGHACCURACY;
rslt |= bmm150_set_presetmode(&bmm150Dev);
DEBUG_PRINT("BMM150 I2C connection [OK].\n");
isMagnetometerPresent = true;
}
/* BMP280 */
rslt = BSTDR_E_GEN_ERROR;
bmp280Dev.bus_read = bstdr_burst_read;
bmp280Dev.bus_write = bstdr_burst_write;
bmp280Dev.delay_ms = bstdr_ms_delay;
bmp280Dev.dev_addr = BMP280_I2C_ADDRESS1;
rslt = bmp280_init(&bmp280Dev);
if (rslt == BSTDR_OK)
{
isBarometerPresent = true;
DEBUG_PRINT("BMP280 I2C connection [OK].\n");
bmp280_set_filter(BMP280_FILTER_COEFF_OFF);
bmp280_set_oversamp_temperature(BMP280_OVERSAMP_2X);
bmp280_set_oversamp_pressure(BMP280_OVERSAMP_8X);
bmp280_set_power_mode(BMP280_NORMAL_MODE);
bmp280Dev.delay_ms(20); // wait before first read out
// read out data
int32_t v_temp_s32;
uint32_t v_pres_u32;
bmp280_read_pressure_temperature(&v_pres_u32, &v_temp_s32);
baroMeasDelayMin = SENSORS_DELAY_BARO;
}
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
sensorsAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
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);
isInit = true;
}
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);
}
void imu6Init(void)
{
if(isInit)
return;
isMagPresent = false;
isBaroPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
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);
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6500SetRate(15);
// 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 + 1) = 500Hz
mpu6500SetRate(1);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagPresent = 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 IMU_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBaroPresent = 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
imuBiasInit(&gyroBias);
#ifdef IMU_TAKE_ACCEL_BIAS
imuBiasInit(&accelBias);
#endif
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
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);
isInit = true;
}
static void sensorsDeviceInit(void)
{
if (isInit)
return;
bstdr_ret_t rslt;
isBarometerPresent = false;
// Wait for sensors to startup
vTaskDelay(M2T(SENSORS_STARTUP_TIME_MS));
/* BMI088 */
bmi088Dev.accel_id = BMI088_ACCEL_I2C_ADDR_PRIMARY;
bmi088Dev.gyro_id = BMI088_GYRO_I2C_ADDR_SECONDARY;
bmi088Dev.interface = BMI088_I2C_INTF;
bmi088Dev.read = bmi088_burst_read;
bmi088Dev.write = bmi088_burst_write;
bmi088Dev.delay_ms = bmi088_ms_delay;
/* BMI088 GYRO */
rslt = bmi088_gyro_init(&bmi088Dev); // initialize the device
if (rslt == BSTDR_OK)
{
struct bmi088_int_cfg intConfig;
DEBUG_PRINT("BMI088 Gyro I2C connection [OK].\n");
/* set power mode of gyro */
bmi088Dev.gyro_cfg.power = BMI088_GYRO_PM_NORMAL;
rslt |= bmi088_set_gyro_power_mode(&bmi088Dev);
/* set bandwidth and range of gyro */
bmi088Dev.gyro_cfg.bw = BMI088_GYRO_BW_116_ODR_1000_HZ;
bmi088Dev.gyro_cfg.range = SENSORS_BMI088_GYRO_FS_CFG;
bmi088Dev.gyro_cfg.odr = BMI088_GYRO_BW_116_ODR_1000_HZ;
rslt |= bmi088_set_gyro_meas_conf(&bmi088Dev);
intConfig.gyro_int_channel = BMI088_INT_CHANNEL_3;
intConfig.gyro_int_type = BMI088_GYRO_DATA_RDY_INT;
intConfig.gyro_int_pin_3_cfg.enable_int_pin = 1;
intConfig.gyro_int_pin_3_cfg.lvl = 1;
intConfig.gyro_int_pin_3_cfg.output_mode = 0;
/* Setting the interrupt configuration */
rslt = bmi088_set_gyro_int_config(&intConfig, &bmi088Dev);
bmi088Dev.delay_ms(50);
struct bmi088_sensor_data gyr;
rslt |= bmi088_get_gyro_data(&gyr, &bmi088Dev);
}
else
{
#ifndef SENSORS_IGNORE_IMU_FAIL
DEBUG_PRINT("BMI088 Gyro I2C connection [FAIL]\n");
isInit = false;
#endif
}
/* BMI088 ACCEL */
rslt |= bmi088_accel_switch_control(&bmi088Dev, BMI088_ACCEL_POWER_ENABLE);
bmi088Dev.delay_ms(5);
rslt = bmi088_accel_init(&bmi088Dev); // initialize the device
if (rslt == BSTDR_OK)
{
DEBUG_PRINT("BMI088 Accel I2C connection [OK]\n");
/* set power mode of accel */
bmi088Dev.accel_cfg.power = BMI088_ACCEL_PM_ACTIVE;
rslt |= bmi088_set_accel_power_mode(&bmi088Dev);
bmi088Dev.delay_ms(10);
/* set bandwidth and range of accel */
bmi088Dev.accel_cfg.bw = BMI088_ACCEL_BW_OSR4;
bmi088Dev.accel_cfg.range = SENSORS_BMI088_ACCEL_FS_CFG;
bmi088Dev.accel_cfg.odr = BMI088_ACCEL_ODR_1600_HZ;
rslt |= bmi088_set_accel_meas_conf(&bmi088Dev);
struct bmi088_sensor_data acc;
rslt |= bmi088_get_accel_data(&acc, &bmi088Dev);
}
else
{
#ifndef SENSORS_IGNORE_IMU_FAIL
DEBUG_PRINT("BMI088 Accel I2C connection [FAIL]\n");
isInit = false;
#endif
}
/* BMP388 */
bmp388Dev.dev_id = BMP3_I2C_ADDR_SEC;
bmp388Dev.intf = BMP3_I2C_INTF;
bmp388Dev.read = bmi088_burst_read;
bmp388Dev.write = bmi088_burst_write;
bmp388Dev.delay_ms = bmi088_ms_delay;
int i = 3;
do {
bmp388Dev.delay_ms(1);
// For some reason it often doesn't work first time
rslt = bmp3_init(&bmp388Dev);
} while (rslt != BMP3_OK && i-- > 0);
if (rslt == BMP3_OK)
{
isBarometerPresent = true;
DEBUG_PRINT("BMP388 I2C connection [OK]\n");
/* Used to select the settings user needs to change */
uint16_t settings_sel;
/* Select the pressure and temperature sensor to be enabled */
bmp388Dev.settings.press_en = BMP3_ENABLE;
bmp388Dev.settings.temp_en = BMP3_ENABLE;
/* Select the output data rate and oversampling settings for pressure and temperature */
bmp388Dev.settings.odr_filter.press_os = BMP3_OVERSAMPLING_8X;
bmp388Dev.settings.odr_filter.temp_os = BMP3_NO_OVERSAMPLING;
bmp388Dev.settings.odr_filter.odr = BMP3_ODR_50_HZ;
bmp388Dev.settings.odr_filter.iir_filter = BMP3_IIR_FILTER_COEFF_3;
/* Assign the settings which needs to be set in the sensor */
settings_sel = BMP3_PRESS_EN_SEL | BMP3_TEMP_EN_SEL | BMP3_PRESS_OS_SEL | BMP3_TEMP_OS_SEL | BMP3_ODR_SEL | BMP3_IIR_FILTER_SEL;
rslt = bmp3_set_sensor_settings(settings_sel, &bmp388Dev);
/* Set the power mode to normal mode */
bmp388Dev.settings.op_mode = BMP3_NORMAL_MODE;
rslt = bmp3_set_op_mode(&bmp388Dev);
bmp388Dev.delay_ms(20); // wait before first read out
// read out data
/* Variable used to select the sensor component */
uint8_t sensor_comp;
/* Variable used to store the compensated data */
struct bmp3_data data;
/* Sensor component selection */
sensor_comp = BMP3_PRESS | BMP3_TEMP;
/* Temperature and Pressure data are read and stored in the bmp3_data instance */
rslt = bmp3_get_sensor_data(sensor_comp, &data, &bmp388Dev);
/* Print the temperature and pressure data */
// DEBUG_PRINT("BMP388 T:%0.2f P:%0.2f\n",data.temperature, data.pressure/100.0f);
baroMeasDelayMin = SENSORS_DELAY_BARO;
}
else
{
#ifndef SENSORS_IGNORE_BAROMETER_FAIL
DEBUG_PRINT("BMP388 I2C connection [FAIL]\n");
isInit = false;
return;
#endif
}
// Init second order filer for accelerometer and gyro
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&gyroLpf[i], 1000, GYRO_LPF_CUTOFF_FREQ);
lpf2pInit(&accLpf[i], 1000, ACCEL_LPF_CUTOFF_FREQ);
}
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);
isInit = true;
}
