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; }