diff --git a/src/config.c b/src/config.c index 691e8a6014..7ca65d4296 100755 --- a/src/config.c +++ b/src/config.c @@ -10,6 +10,7 @@ #include "drivers/system_common.h" #include "statusindicator.h" +#include "sensors_common.h" #include "sensors_acceleration.h" #include "telemetry_common.h" #include "gps_common.h" diff --git a/src/drivers/accgyro_mpu6050.c b/src/drivers/accgyro_mpu6050.c index 170e5b16f5..1325b71ab6 100644 --- a/src/drivers/accgyro_mpu6050.c +++ b/src/drivers/accgyro_mpu6050.c @@ -132,9 +132,14 @@ static void mpu6050AccRead(int16_t *accData); static void mpu6050GyroInit(sensor_align_e align); static void mpu6050GyroRead(int16_t *gyroData); -static uint8_t mpuAccelHalf = 0; +typedef enum { + MPU_6050_HALF_RESOLUTION, + MPU_6050_FULL_RESOLUTION +} mpu6050Resolution_e; -bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) +static mpu6050Resolution_e mpuAccelTrim; + +bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf) { bool ack; uint8_t sig, rev; @@ -150,18 +155,22 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address. // The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already). // But here's the best part: The value of the AD0 pin is not reflected in this register. + if (sig != (MPU6050_ADDRESS & 0x7e)) 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) { /* Congrats, these parts are better. */ if (rev == 1) { - mpuAccelHalf = 1; + mpuAccelTrim = MPU_6050_HALF_RESOLUTION; } else if (rev == 2) { - mpuAccelHalf = 0; + mpuAccelTrim = MPU_6050_FULL_RESOLUTION; } else { failureMode(5); } @@ -171,24 +180,21 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) if (!rev) { failureMode(5); } else if (rev == 4) { - mpuAccelHalf = 1; + mpuAccelTrim = MPU_6050_HALF_RESOLUTION; } else { - mpuAccelHalf = 0; + mpuAccelTrim = MPU_6050_FULL_RESOLUTION; } } 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. gyro->init = mpu6050GyroInit; gyro->read = mpu6050GyroRead; // 16.4 dps/lsb scalefactor gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; - // give halfacc (old revision) back to system - if (scale) - *scale = mpuAccelHalf; - // default lpf is 42Hz switch (lpf) { case 256: @@ -220,10 +226,14 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) static void mpu6050AccInit(sensor_align_e align) { - if (mpuAccelHalf) - acc_1G = 255 * 8; - else - acc_1G = 512 * 8; + switch(mpuAccelTrim) { + case MPU_6050_HALF_RESOLUTION: + acc_1G = 256 * 8; + break; + case MPU_6050_FULL_RESOLUTION: + acc_1G = 512 * 8; + break; + } if (align > 0) accAlign = align; diff --git a/src/drivers/accgyro_mpu6050.h b/src/drivers/accgyro_mpu6050.h index c9501f2716..902bd269d4 100644 --- a/src/drivers/accgyro_mpu6050.h +++ b/src/drivers/accgyro_mpu6050.h @@ -1,5 +1,5 @@ #pragma once -bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale); +bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf); void mpu6050DmpLoop(void); void mpu6050DmpResetFifo(void); diff --git a/src/flight_imu.c b/src/flight_imu.c index e69a2b73e2..7889ba8dac 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -3,6 +3,7 @@ #include "common/maths.h" +#include "sensors_gyro.h" #include "sensors_compass.h" #include "flight_common.h" diff --git a/src/mw.c b/src/mw.c index 266fecc188..5882d73cf4 100755 --- a/src/mw.c +++ b/src/mw.c @@ -4,6 +4,7 @@ #include "common/maths.h" #include "common/typeconversion.h" +#include "sensors_gyro.h" #include "flight_common.h" #include "serial_cli.h" #include "telemetry_common.h" diff --git a/src/mw.h b/src/mw.h index 76ccf01335..690f04bcf5 100755 --- a/src/mw.h +++ b/src/mw.h @@ -90,9 +90,6 @@ extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtit extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) extern flags_t f; -extern sensor_t acc; -extern sensor_t gyro; -extern baro_t baro; // main void setPIDController(int type); diff --git a/src/runtime_config.h b/src/runtime_config.h index d9bfb9483c..6990b86e66 100644 --- a/src/runtime_config.h +++ b/src/runtime_config.h @@ -53,7 +53,6 @@ typedef struct core_t { serialPort_t *gpsport; serialPort_t *telemport; serialPort_t *rcvrport; - uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver. } core_t; typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index e605e8f365..cbff683261 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -4,6 +4,10 @@ #include "mw.h" +#include "sensors_acceleration.h" + +sensor_t acc; // acc access functions +uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h index 323687aa39..2c99c29dfa 100644 --- a/src/sensors_acceleration.h +++ b/src/sensors_acceleration.h @@ -10,5 +10,9 @@ typedef enum AccelSensors { ACC_NONE = 5 } AccelSensors; +extern uint8_t accHardware; +extern sensor_t acc; + void ACC_Common(void); void ACC_getADC(void); + diff --git a/src/sensors_barometer.c b/src/sensors_barometer.c index 54a5d61e63..9a16d9d596 100644 --- a/src/sensors_barometer.c +++ b/src/sensors_barometer.c @@ -1,6 +1,8 @@ #include "board.h" #include "mw.h" +baro_t baro; // barometer access functions + #ifdef BARO void Baro_Common(void) { diff --git a/src/sensors_barometer.h b/src/sensors_barometer.h index 45ae752547..bcbddb503f 100644 --- a/src/sensors_barometer.h +++ b/src/sensors_barometer.h @@ -1,5 +1,7 @@ #pragma once +extern baro_t baro; + #ifdef BARO void Baro_Common(void); int Baro_update(void); diff --git a/src/sensors_common.h b/src/sensors_common.h index aa0557c0cc..138206383c 100644 --- a/src/sensors_common.h +++ b/src/sensors_common.h @@ -31,6 +31,7 @@ typedef struct sensor_t sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr temperature; // read temperature if available float scale; // scalefactor (currently used for gyro only, todo for accel) + char revisionCode; // a revision code for the sensor, if known } sensor_t; extern int16_t heading; diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index 2abf7929a3..452540c157 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -6,6 +6,8 @@ #include "flight_common.h" #include "statusindicator.h" +sensor_t gyro; // gyro access functions + void GYRO_Common(void) { int axis; diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h index 0aa443d2f1..dbfe156801 100644 --- a/src/sensors_gyro.h +++ b/src/sensors_gyro.h @@ -1,4 +1,7 @@ #pragma once +extern sensor_t gyro; + void GYRO_Common(void); void Gyro_getADC(void); + diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index 96a4e1819d..14ad6de61e 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -2,6 +2,10 @@ #include "mw.h" #include "sensors_acceleration.h" +#include "sensors_barometer.h" +#include "sensors_gyro.h" + +#include "sensors_common.h" uint16_t calibratingB = 0; // baro calibration = get new ground pressure value uint16_t calibratingG = 0; @@ -12,15 +16,12 @@ extern uint16_t batteryWarningVoltage; extern uint8_t batteryCellCount; extern float magneticDeclination; -sensor_t acc; // acc access functions -sensor_t gyro; // gyro access functions -baro_t baro; // barometer access functions -uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected - #ifdef FY90Q // FY90Q analog gyro/acc void sensorsAutodetect(void) { + memset(&acc, sizeof(acc), 0); + memset(&gyro, sizeof(gyro), 0); adcSensorInit(&acc, &gyro); } #else @@ -31,8 +32,11 @@ void sensorsAutodetect(void) drv_adxl345_config_t acc_params; bool haveMpu6k = false; + memset(&acc, sizeof(acc), 0); + memset(&gyro, sizeof(gyro), 0); + // Autodetect gyro hardware. We have MPU3050 or MPU6050. - if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) { + if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf)) { // this filled up acc.* struct with init values haveMpu6k = true; } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { @@ -60,7 +64,7 @@ retry: ; // fallthrough 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 + mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; if (mcfg.acc_hardware == ACC_MPU6050) break; diff --git a/src/serial_cli.c b/src/serial_cli.c index 20f69fa4a7..ae68357ddb 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -30,7 +30,6 @@ static void cliVersion(char *cmdline); // from sensors.c extern uint8_t batteryCellCount; -extern uint8_t accHardware; // from config.c RC Channel mapping extern const char rcChannelLetters[]; @@ -996,8 +995,8 @@ static void cliStatus(char *cmdline) } if (sensors(SENSOR_ACC)) { printf("ACCHW: %s", accNames[accHardware]); - if (accHardware == ACC_MPU6050) - printf(".%c", core.mpu6050_scale ? 'o' : 'n'); + if (acc.revisionCode) + printf(".%c", acc.revisionCode); } cliPrint("\r\n");