fixed mma driver ID;

fixed a boner #ifdef typo in imu.c
go back to gyro glitch stuff in sensors.c
do gyrocal during acc cal as well

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

src/drv_mma845x.c

@@ -5,6 +5,7 @@
 #define MMA8452_ADDRESS     0x1C
 
 #define MMA8452_DEVICE_SIGNATURE    0x2A
+#define MMA8451_DEVICE_SIGNATURE    0x1A
 
 #define MMA8452_STATUS              0x00
 #define MMA8452_OUT_X_MSB           0x01
@@ -46,6 +47,7 @@
 #define MMA8452_CTRL_REG1_ACTIVE        0x01
 
 extern uint16_t acc_1G;
+static uint8_t device_id;
 
 static void mma8452Init(void);
 static void mma8452Read(int16_t *accelData);
@@ -57,12 +59,13 @@ bool mma8452Detect(sensor_t *acc)
     uint8_t sig = 0;
 
     ack = i2cRead(MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
-    if (!ack || sig != MMA8452_DEVICE_SIGNATURE)
+    if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
         return false;
 
     acc->init = mma8452Init;
     acc->read = mma8452Read;
     acc->align = mma8452Align;
+    device_id = sig;
     return true;
 }
 

src/imu.c

@@ -56,9 +56,9 @@ void computeIMU(void)
         gyroADCp[axis] = gyroADC[axis];
 #else
         gyroData[axis] = gyroADC[axis];
+#endif
         if (!sensors(SENSOR_ACC))
             accADC[axis] = 0;
-#endif
     timeInterleave = micros();
     annexCode();
 #ifdef GYRO_INTERLEAVE

src/sensors.c

@@ -272,6 +272,7 @@ void Baro_update(void)
 
 static void GYRO_Common(void)
 {
+    static int16_t previousGyroADC[3] = { 0, 0, 0 };
     static int32_t g[3];
     uint8_t axis;
 
@@ -293,6 +294,12 @@ static void GYRO_Common(void)
         }
         calibratingG--;
     }
+    for (axis = 0; axis < 3; axis++) {
+        gyroADC[axis] -= gyroZero[axis];
+        //anti gyro glitch, limit the variation between two consecutive readings
+        gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
+        previousGyroADC[axis] = gyroADC[axis];
+    }
 }
 
 void Gyro_getADC(void)

src/serial.c

@@ -318,6 +318,7 @@ void serialCom(void)
                         break;
                     case MSP_ACC_CALIBRATION:
                         calibratingA = 400;
+                        calibratingG = 1000; // hit gyro too
                         break;
                     case MSP_MAG_CALIBRATION:
                         calibratingM = 1;