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