rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleanup sensor auto detection code. 

Decoupled mpu6050 acc and gyro detection code.

This commit cleans up the #ifdef/#ifndef code which, due to the amount
of targets and variants, was starting to get a bit out of hand.

Now each acc & gyro is assumed to be used unless the respective
USE_ACC_* and USE_GYRO_* is #undef'd at the top of the file by a
particular target.

This commit also cleans up the mess of re-running all the mpu6050
detection code twice.  Now the acc specific communication code is only
run once and only code that is actually needed to be run twice is run
twice, and the rest of the code is now only used once.  This will
improve board startup time, albeit negligibly.

Sensor alignment is only known for the NAZE target so now the alignment
will be ALIGN_DEFAULT for all targets other than NAZE unless they are
known.

Finally, in the Makefile a file was included twice in the source list
for the STM32F3DISCOVERY target and now additional sensors are supported
for users that may want to try them on that board.
This commit is contained in:
Dominic Clifton 2014-05-07 23:14:20 +01:00
parent a3d23de2ca
commit 90d36b6869
4 changed files with 158 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -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 \

48
src/drivers/accgyro_mpu6050.c
View File

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

3
src/drivers/accgyro_mpu6050.h
View File

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

172
src/sensors_initialisation.c
View File

@ -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))