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;
}
}
void sensorsBmi088Bmp388SetAccMode(accModes accMode)
{
switch (accMode)
{
case ACC_MODE_PROPTEST:
// bmi088_accel_soft_reset(&bmi088Dev);
/* set bandwidth and range of accel (280Hz cut-off according to datasheet) */
bmi088Dev.accel_cfg.bw = BMI088_ACCEL_BW_NORMAL;
bmi088Dev.accel_cfg.range = SENSORS_BMI088_ACCEL_FS_CFG;
bmi088Dev.accel_cfg.odr = BMI088_ACCEL_ODR_1600_HZ;
if (bmi088_set_accel_meas_conf(&bmi088Dev) != BMI088_OK)
{
DEBUG_PRINT("ACC config [FAIL]\n");
}
for (uint8_t i = 0; i < 3; i++)
{
lpf2pInit(&accLpf[i], 1000, 500);
}
break;
case ACC_MODE_FLIGHT:
default:
/* set bandwidth and range of accel (145Hz cut-off according to datasheet) */
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;
if (bmi088_set_accel_meas_conf(&bmi088Dev) != BMI088_OK)
{
DEBUG_PRINT("ACC config [FAIL]\n");
}
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);
}
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;
}