diff --git a/Makefile b/Makefile index cd5e552b2b..b4a4e13078 100644 --- a/Makefile +++ b/Makefile @@ -201,8 +201,9 @@ STM32F3DISCOVERY_COMMON_SRC = startup_stm32f30x_md_gcc.S \ drivers/timer_common.c STM32F3DISCOVERY_SRC = $(STM32F3DISCOVERY_COMMON_SRC) \ - drivers/accgyro_l3g4200d.c \ drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_mma845x.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_l3g4200d.c \ diff --git a/src/drivers/accgyro_mpu6050.c b/src/drivers/accgyro_mpu6050.c index f8ceefc8a4..7199bd27f3 100644 --- a/src/drivers/accgyro_mpu6050.c +++ b/src/drivers/accgyro_mpu6050.c @@ -132,11 +132,10 @@ typedef enum { static mpu6050Resolution_e mpuAccelTrim; -bool mpu6050Detect(acc_t *acc, gyro_t *gyro, uint16_t lpf) +static bool mpu6050Detect(void) { bool ack; - uint8_t sig, rev; - uint8_t tmp[6]; + uint8_t sig; delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe @@ -152,27 +151,40 @@ bool mpu6050Detect(acc_t *acc, gyro_t *gyro, uint16_t lpf) if (sig != (MPU6050_ADDRESS & 0x7e)) return false; + return true; +} + +bool mpu6050AccDetect(acc_t *acc) +{ + uint8_t readBuffer[6]; + uint8_t revision; + uint8_t productId; + + if (!mpu6050Detect()) { + return false; + } + // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // determine product ID and accel revision - i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp); - rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01); - if (rev) { + i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer); + revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); + if (revision) { /* Congrats, these parts are better. */ - if (rev == 1) { + if (revision == 1) { mpuAccelTrim = MPU_6050_HALF_RESOLUTION; - } else if (rev == 2) { + } else if (revision == 2) { mpuAccelTrim = MPU_6050_FULL_RESOLUTION; } else { failureMode(5); } } else { - i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &sig); - rev = sig & 0x0F; - if (!rev) { + i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId); + revision = productId & 0x0F; + if (!revision) { failureMode(5); - } else if (rev == 4) { + } else if (revision == 4) { mpuAccelTrim = MPU_6050_HALF_RESOLUTION; } else { mpuAccelTrim = MPU_6050_FULL_RESOLUTION; @@ -181,7 +193,17 @@ bool mpu6050Detect(acc_t *acc, gyro_t *gyro, uint16_t lpf) acc->init = mpu6050AccInit; acc->read = mpu6050AccRead; - acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES. + acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. + + return true; +} + +bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) +{ + if (!mpu6050Detect()) { + return false; + } + gyro->init = mpu6050GyroInit; gyro->read = mpu6050GyroRead; diff --git a/src/drivers/accgyro_mpu6050.h b/src/drivers/accgyro_mpu6050.h index a6cdfabca5..f7cddb9721 100644 --- a/src/drivers/accgyro_mpu6050.h +++ b/src/drivers/accgyro_mpu6050.h @@ -1,5 +1,6 @@ #pragma once -bool mpu6050Detect(acc_t * acc, gyro_t * gyro, uint16_t lpf); +bool mpu6050AccDetect(acc_t *acc); +bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf); void mpu6050DmpLoop(void); void mpu6050DmpResetFifo(void); diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index 165e8b7f7a..109d49b0cb 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -40,9 +40,39 @@ #include "sensors_sonar.h" // Use these to help with porting to new boards -//#define USE_FAKE_ACC //#define USE_FAKE_GYRO +#define USE_GYRO_L3G4200D +#define USE_GYRO_L3GD20 +#define USE_GYRO_MPU6050 +#define USE_GYRO_MPU3050 +//#define USE_FAKE_ACC #define USE_ACC_ADXL345 +#define USE_ACC_BMA280 +#define USE_ACC_MMA8452 +#define USE_ACC_LSM303DLHC +#define USE_ACC_MPU6050 + +#ifdef NAZE +#undef USE_ACC_LSM303DLHC +#undef USE_GYRO_L3GD20 +#endif + +#if defined(OLIMEXINO) +#undef USE_ACC_LSM303DLHC +#undef USE_GYRO_L3GD20 +#undef USE_ACC_BMA280 +#undef USE_ACC_MMA8452 +#endif + +#ifdef CHEBUZZF3 +#undef USE_GYRO_L3G4200D +#undef USE_GYRO_MPU6050 +#undef USE_GYRO_MPU3050 +#undef USE_ACC_ADXL345 +#undef USE_ACC_BMA280 +#undef USE_ACC_MPU6050 +#undef USE_ACC_MMA8452 +#endif extern uint16_t batteryWarningVoltage; extern uint8_t batteryCellCount; @@ -79,16 +109,6 @@ bool fakeAccDetect(acc_t *acc) } #endif -#ifdef CHEBUZZF3 -// FIXME ugly hack to support a target that will never use these sensors. There needs to be a better way of compiling in and detecting only the sensors needed. -#define mpu6050Detect(a,b,c) false -#define l3g4200dDetect(a,b) false -#define mpu3050Detect(a,b) false -#define adxl345Detect(a,b) false -#undef USE_ACC_ADXL345 -#endif - - #ifdef FY90Q // FY90Q analog gyro/acc bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) @@ -99,51 +119,68 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t return true; } #else -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) + +bool detectGyro(uint16_t gyroLpf) +{ + gyroAlign = ALIGN_DEFAULT; +#ifdef USE_FAKE_GYRO + if (fakeGyroDetect(&gyro, gyroLpf)) { + return true; +#endif + +#ifdef USE_GYRO_MPU6050 + if (mpu6050GyroDetect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_L3G4200D + if (l3g4200dDetect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_MPU3050 + if (mpu3050Detect(&gyro, gyroLpf)) { +#ifdef NAZE + gyroAlign = CW0_DEG; +#endif + return true; + } +#endif + +#ifdef USE_GYRO_L3GD20 + if (l3gd20Detect(&gyro, gyroLpf)) { + return true; + } +#endif + return false; +} + +static void detectAcc(uint8_t accHardwareToUse) { - int16_t deg, min; #ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif - bool haveMpu6k = false; - memset(&acc, sizeof(acc), 0); - memset(&gyro, sizeof(gyro), 0); - -#ifdef USE_FAKE_GYRO - if (fakeGyroDetect(&gyro, gyroLpf)) { - gyroAlign = ALIGN_DEFAULT; -#else - - // Autodetect gyro hardware. We have MPU3050 or MPU6050. - if (mpu6050Detect(&acc, &gyro, gyroLpf)) { - haveMpu6k = true; - gyroAlign = CW0_DEG; // default NAZE alignment - } else if (l3g4200dDetect(&gyro, gyroLpf)) { - gyroAlign = CW0_DEG; - } else if (mpu3050Detect(&gyro, gyroLpf)) { - gyroAlign = CW0_DEG; -#ifdef STM32F3DISCOVERY - } else if (l3gd20Detect(&gyro, gyroLpf)) { - gyroAlign = ALIGN_DEFAULT; -#endif -#endif - } else { - return false; - } - - // Accelerometer. Fuck it. Let user break shit. retry: + accAlign = ALIGN_DEFAULT; + switch (accHardwareToUse) { #ifdef USE_FAKE_ACC default: if (fakeAccDetect(&acc)) { accHardware = ACC_FAKE; - accAlign = CW0_DEG; // if (accHardwareToUse == ACC_FAKE) break; } -#else +#endif case ACC_NONE: // disable ACC sensorsClear(SENSOR_ACC); break; @@ -154,46 +191,56 @@ retry: acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, &acc)) { accHardware = ACC_ADXL345; - accAlign = CW270_DEG; // default NAZE alignment +#ifdef NAZE + accAlign = CW270_DEG; +#endif } if (accHardwareToUse == ACC_ADXL345) break; ; // fallthrough #endif +#ifdef USE_ACC_MPU6050 case ACC_MPU6050: // MPU6050 - if (haveMpu6k && mpu6050Detect(&acc, &gyro, gyroLpf)) { // FIXME decouple mpu detection from gyro/acc struct filling + if (mpu6050AccDetect(&acc)) { accHardware = ACC_MPU6050; - accAlign = CW0_DEG; // default NAZE alignment +#ifdef NAZE + accAlign = CW0_DEG; +#endif if (accHardwareToUse == ACC_MPU6050) break; } ; // fallthrough -#if !defined(OLIMEXINO) && !defined(STM32F3DISCOVERY) +#endif +#ifdef USE_ACC_MMA8452 case ACC_MMA8452: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; - accAlign = CW90_DEG; // default NAZE alignment +#ifdef NAZE + accAlign = CW90_DEG; +#endif if (accHardwareToUse == ACC_MMA8452) break; } ; // fallthrough +#endif +#ifdef USE_ACC_BMA280 case ACC_BMA280: // BMA280 if (bma280Detect(&acc)) { accHardware = ACC_BMA280; - accAlign = CW0_DEG; // +#ifdef NAZE + accAlign = CW0_DEG; +#endif if (accHardwareToUse == ACC_BMA280) break; } #endif -#ifdef STM32F3DISCOVERY +#ifdef USE_ACC_LSM303DLHC case ACC_LSM303DLHC: if (lsm303dlhcAccDetect(&acc)) { accHardware = ACC_LSM303DLHC; - accAlign = ALIGN_DEFAULT; // if (accHardwareToUse == ACC_LSM303DLHC) break; } -#endif #endif } @@ -204,11 +251,14 @@ retry: accHardwareToUse = ACC_DEFAULT; goto retry; } else { - // We're really screwed + // No ACC was detected sensorsClear(SENSOR_ACC); } } +} +static void detectBaro() +{ #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function if (!ms5611Detect(&baro)) { @@ -219,8 +269,10 @@ retry: } } #endif +} - +void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) +{ if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { gyroAlign = sensorAlignmentConfig->gyro_align; } @@ -230,7 +282,21 @@ retry: if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { magAlign = sensorAlignmentConfig->mag_align; } +} +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig) +{ + int16_t deg, min; + memset(&acc, sizeof(acc), 0); + memset(&gyro, sizeof(gyro), 0); + + if (!detectGyro(gyroLpf)) { + return false; + } + detectAcc(accHardwareToUse); + detectBaro(); + + reconfigureAlignment(sensorAlignmentConfig); // Now time to init things, acc first if (sensors(SENSOR_ACC))