fixed bug in mpu6050 accel code

added mpu6050 acc scale factor fixup for es/non es chips (exposed as cli option as well)


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@191 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/cli.c

@@ -127,6 +127,7 @@ const clivalue_t valueTable[] = {
     { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
     { "gyro_lpf", VAR_UINT16, &cfg.gyro_lpf, 0, 256 },
     { "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 },
+    { "mpu6050_scale", VAR_UINT8, &cfg.mpu6050_scale, 0, 1 },
     { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 },
     { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 },
     { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 },

src/config.c

@@ -13,7 +13,7 @@ config_t cfg;
 const char rcChannelLetters[] = "AERT1234";
 
 static uint32_t enabledSensors = 0;
-uint8_t checkNewConf = 24;
+uint8_t checkNewConf = 25;
 
 void parseRcChannels(const char *input)
 {
@@ -137,6 +137,7 @@ void checkFirstTime(bool reset)
     cfg.acc_lpf_factor = 4;
     cfg.gyro_cmpf_factor = 400; // default MWC
     cfg.gyro_lpf = 42;
+    cfg.mpu6050_scale = 1; // fuck invensense
     cfg.gyro_smoothing_factor = 0x00141403;     // default factors of 20, 20, 3 for R/P/Y
     cfg.vbatscale = 110;
     cfg.vbatmaxcellvoltage = 43;

src/drv_mpu6050.c

@@ -143,7 +143,7 @@ int16_t dmpGyroData[3];
 extern uint16_t acc_1G;
 uint8_t mpuProductID = 0;
 
-bool mpu6050Detect(sensor_t * acc, sensor_t * gyro)
+bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t scale)
 {
     bool ack;
     uint8_t sig;
@@ -157,8 +157,11 @@ bool mpu6050Detect(sensor_t * acc, sensor_t * gyro)
     if (sig != MPU6050_ADDRESS)
         return false;
 
-    // get chip revision
+    // get chip revision + fake it if needed
-    i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &mpuProductID);
+    if (scale)
+        mpuProductID = MPU6000_REV_C5; // no, seriously? why don't you make the chip ID list public.
+    else
+        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &mpuProductID);
 
     acc->init = mpu6050AccInit;
     acc->read = mpu6050AccRead;
@@ -176,16 +179,6 @@ bool mpu6050Detect(sensor_t * acc, sensor_t * gyro)
 
 static void mpu6050AccInit(void)
 {
-#ifndef MPU6050_DMP
-    // Product ID detection code from eosBandi (or rather, DIYClones). This doesn't cover product ID for MPU6050 as far as I can tell
-    if ((mpuProductID == MPU6000ES_REV_C4) || (mpuProductID == MPU6000ES_REV_C5) || (mpuProductID == MPU6000_REV_C4)   || (mpuProductID == MPU6000_REV_C5)) {
-        // Accel scale 8g (4096 LSB/g)
-        // Rev C has different scaling than rev D
-        i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1 << 3);
-    } else {
-        i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
-    }
-#endif
     acc_1G = 1023;
 }
 
@@ -224,6 +217,16 @@ static void mpu6050GyroInit(void)
     i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03);      //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, MPU6050_DLPF_CFG);  //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18);      //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
+
+    // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
+    // Product ID detection code from eosBandi (or rather, DIYClones). This doesn't cover product ID for MPU6050 as far as I can tell
+    if ((mpuProductID == MPU6000ES_REV_C4) || (mpuProductID == MPU6000ES_REV_C5) || (mpuProductID == MPU6000_REV_C4) || (mpuProductID == MPU6000_REV_C5)) {
+        // Accel scale 8g (4096 LSB/g)
+        // Rev C has different scaling than rev D
+        i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1 << 3);
+    } else {
+        i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
+    }
 #endif
 }
 

src/drv_mpu6050.h

@@ -1,5 +1,5 @@
 #pragma once
 
-bool mpu6050Detect(sensor_t *acc, sensor_t *gyro);
+bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t scale);
 void mpu6050DmpLoop(void);
 void mpu6050DmpResetFifo(void);

src/mw.h

@@ -119,6 +119,7 @@ typedef struct config_t {
     uint16_t gyro_lpf;                      // mpuX050 LPF setting
     uint16_t gyro_cmpf_factor;              // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
     uint32_t gyro_smoothing_factor;         // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
+    uint8_t mpu6050_scale;                  // seems es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. fucking invenshit won't release chip IDs so I can't autodetect it.
 
     uint16_t activate[CHECKBOXITEMS];       // activate switches
     uint8_t vbatscale;                      // adjust this to match battery voltage to reported value

src/sensors.c

@@ -35,7 +35,7 @@ void sensorsAutodetect(void)
     bool havel3g4200d = false;
 
     // Autodetect gyro hardware. We have MPU3050 or MPU6050.
-    if (mpu6050Detect(&acc, &gyro)) {
+    if (mpu6050Detect(&acc, &gyro, cfg.mpu6050_scale)) {
         // this filled up  acc.* struct with init values
         haveMpu6k = true;
     } else if (l3g4200dDetect(&gyro)) {
@@ -59,7 +59,7 @@ retry:
             ; // fallthrough
        case 2: // MPU6050
             if (haveMpu6k) {
-                mpu6050Detect(&acc, &gyro); // yes, i'm rerunning it again.  re-fill acc struct
+                mpu6050Detect(&acc, &gyro, cfg.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
                 accHardware = ACC_MPU6050;
                 if (cfg.acc_hardware == ACC_MPU6050)
                     break;