bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
{
if (!mpu6500Detect()) {
return false;
}
gyro->init = mpu6500GyroInit;
gyro->read = mpu6500GyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
// default lpf is 42Hz
if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else
mpuLowPassFilter = INV_FILTER_5HZ;
return true;
}
bool mpu6500SpiAccDetect(acc_t *acc)
{
if (!mpu6500Detect()) {
return false;
}
acc->init = mpu6500AccInit;
acc->read = mpu6500AccRead;
return true;
}
bool sensorsAutodetect(void)
{
int16_t deg, min;
#ifndef CJMCU
drv_adxl345_config_t acc_params;
bool haveMpu65 = false;
#endif
bool haveMpu6k = false;
// Autodetect gyro hardware. We have MPU3050 or MPU6050 or MPU6500 on SPI
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
// this filled up acc.* struct with init values
haveMpu6k = true;
} else
#ifndef CJMCU
if (hw_revision == NAZE32_SP && mpu6500Detect(&acc, &gyro, mcfg.gyro_lpf))
haveMpu65 = true;
else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
// well, we found our gyro
;
} else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf))
#endif
{
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
return false;
}
// Accelerometer. F**k it. Let user break shit.
retry:
switch (mcfg.acc_hardware) {
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
#ifndef CJMCU
case ACC_ADXL345: // ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
if (adxl345Detect(&acc_params, &acc))
accHardware = ACC_ADXL345;
if (mcfg.acc_hardware == ACC_ADXL345)
break;
; // fallthrough
#endif
case ACC_MPU6050: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050)
break;
}
; // fallthrough
#ifdef NAZE
case ACC_MPU6500: // MPU6500
if (haveMpu65) {
mpu6500Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6500;
if (mcfg.acc_hardware == ACC_MPU6500)
break;
}
; // fallthrough
case ACC_MMA8452: // MMA8452
if (mma8452Detect(&acc)) {
accHardware = ACC_MMA8452;
if (mcfg.acc_hardware == ACC_MMA8452)
break;
}
; // fallthrough
case ACC_BMA280: // BMA280
if (bma280Detect(&acc)) {
accHardware = ACC_BMA280;
if (mcfg.acc_hardware == ACC_BMA280)
break;
}
#endif
}
// Found anything? Check if user f****d up or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (mcfg.acc_hardware > ACC_DEFAULT && mcfg.acc_hardware < ACC_NONE) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.acc_hardware = ACC_DEFAULT;
goto retry;
} else {
// We're really screwed
sensorsClear(SENSOR_ACC);
}
}
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
if (!bmp085Detect(&baro)) {
// ms5611 disables BMP085, and tries to initialize + check PROM crc.
// moved 5611 init here because there have been some reports that calibration data in BMP180
// has been "passing" ms5611 PROM crc check
if (!ms5611Detect(&baro)) {
// if both failed, we don't have anything
sensorsClear(SENSOR_BARO);
}
}
#endif
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init(mcfg.acc_align);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(mcfg.gyro_align);
#ifdef MAG
retryMag:
switch (mcfg.mag_hardware) {
case MAG_NONE: // disable MAG
sensorsClear(SENSOR_MAG);
break;
case MAG_DEFAULT: // autodetect
case MAG_HMC5883L:
if (hmc5883lDetect(&mag)) {
magHardware = MAG_HMC5883L;
if (mcfg.mag_hardware == MAG_HMC5883L)
break;
}
; // fallthrough
#ifdef NAZE
case MAG_AK8975:
if (ak8975detect(&mag)) {
magHardware = MAG_AK8975;
if (mcfg.mag_hardware == MAG_AK8975)
break;
}
#endif
}
// Found anything? Check if user f****d up or mag is really missing.
if (magHardware == MAG_DEFAULT) {
if (mcfg.mag_hardware > MAG_DEFAULT && mcfg.mag_hardware < MAG_NONE) {
// Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present.
mcfg.mag_hardware = MAG_DEFAULT;
goto retryMag;
} else {
// No mag present
sensorsClear(SENSOR_MAG);
}
}
#endif
// calculate magnetic declination
deg = cfg.mag_declination / 100;
min = cfg.mag_declination % 100;
if (sensors(SENSOR_MAG))
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
else
magneticDeclination = 0.0f;
return true;
}