added support for l3g4200d i2c gyro, autodetected

added motors off in hardfault handler, so we drop to the ground on hardfault.


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

src/board.h

@@ -139,6 +139,7 @@ typedef struct sensor_t
 #include "drv_mma845x.h"
 #include "drv_mpu3050.h"
 #include "drv_mpu6050.h"
+#include "drv_l3g4200d.h"
 #include "drv_pwm.h"
 #include "drv_uart.h"
 #include "drv_hcsr04.h"

src/drv_l3g4200d.c

@@ -0,0 +1,98 @@
+#include "board.h"
+
+// L3G4200D, Standard address 0x68
+#define L3G4200D_ADDRESS         0x68
+#define L3G4200D_ID              0xD3
+
+// Registers
+#define L3G4200D_WHO_AM_I        0x0F
+#define L3G4200D_CTRL_REG1       0x20
+#define L3G4200D_CTRL_REG2       0x21
+#define L3G4200D_CTRL_REG3       0x22
+#define L3G4200D_CTRL_REG4       0x23
+#define L3G4200D_CTRL_REG5       0x24
+#define L3G4200D_REFERENCE       0x25
+#define L3G4200D_STATUS_REG      0x27
+#define L3G4200D_GYRO_OUT        0xA8
+
+// Bits
+#define L3G4200D_POWER_ON        0x0F
+#define L3G4200D_FS_SEL_2000DPS  0xF0
+#define L3G4200D_DLPF_32HZ       0x00
+#define L3G4200D_DLPF_54HZ       0x40
+#define L3G4200D_DLPF_78HZ       0x80
+#define L3G4200D_DLPF_93HZ       0xC0
+
+static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
+
+static void l3g4200dInit(void);
+static void l3g4200dRead(int16_t *gyroData);
+static void l3g4200dAlign(int16_t *gyroData);
+
+bool l3g4200dDetect(sensor_t *gyro)
+{
+    uint8_t deviceid;
+
+    delay(25);
+
+    i2cRead(L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
+    if (deviceid != L3G4200D_ID)
+        return false;
+
+    gyro->init = l3g4200dInit;
+    gyro->read = l3g4200dRead;
+    gyro->align = l3g4200dAlign;
+
+    return true;
+}
+
+void l3g4200dConfig(uint16_t lpf)
+{
+    switch (lpf) {
+    case 32:
+        mpuLowPassFilter = L3G4200D_DLPF_32HZ;
+        break;
+    case 54:
+        mpuLowPassFilter = L3G4200D_DLPF_54HZ;
+        break;
+    case 78:
+        mpuLowPassFilter = L3G4200D_DLPF_78HZ;
+        break;
+    case 93:
+        mpuLowPassFilter = L3G4200D_DLPF_93HZ;
+        break;
+    }
+
+    i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
+}
+
+static void l3g4200dInit(void)
+{
+    bool ack;
+
+    delay(100);
+
+    ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
+    if (!ack)
+        failureMode(3);
+
+    delay(5);
+    i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
+}
+
+static void l3g4200dAlign(int16_t *gyroData)
+{
+    gyroData[0] = -gyroData[0] / 4;
+    gyroData[1] = gyroData[1] / 4;
+    gyroData[2] = -gyroData[2] / 4;
+}
+
+// Read 3 gyro values into user-provided buffer. No overrun checking is done.
+static void l3g4200dRead(int16_t *gyroData)
+{
+    uint8_t buf[6];
+    i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
+    gyroData[1] = (buf[0] << 8) | buf[1];
+    gyroData[0] = (buf[2] << 8) | buf[3];
+    gyroData[2] = (buf[4] << 8) | buf[5];
+}

src/drv_l3g4200d.h

@@ -0,0 +1,4 @@
+#pragma once
+
+bool l3g4200dDetect(sensor_t * gyro);
+void l3g4200dConfig(uint16_t lpf);

src/main.c

@@ -138,3 +138,10 @@ int main(void)
         loop();
     }
 }
+
+void HardFault_Handler(void)
+{
+    // fall out of the sky
+    writeAllMotors(cfg.mincommand);
+    while (1);
+}

src/sensors.c

@@ -32,11 +32,14 @@ void sensorsAutodetect(void)
     int16_t deg, min;
     drv_adxl345_config_t acc_params;
     bool haveMpu6k = false;
+    bool havel3g4200d = false;
 
     // Autodetect gyro hardware. We have MPU3050 or MPU6050.
     if (mpu6050Detect(&acc, &gyro)) {
         // this filled up  acc.* struct with init values
         haveMpu6k = true;
+    } else if (l3g4200dDetect(&gyro)) {
+        havel3g4200d = true;
     } else if (!mpu3050Detect(&gyro)) {
         // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
         failureMode(3);
@@ -93,11 +96,16 @@ retry:
         acc.init();
     if (sensors(SENSOR_BARO))
         bmp085Init();
-    // this is safe because either mpu6050 or mpu3050 sets it, and in case of fail, none do.
+    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
     gyro.init();
+
     // todo: this is driver specific :(
-    if (!haveMpu6k)
-        mpu3050Config(cfg.gyro_lpf);
+    if (havel3g4200d) {
+        l3g4200dConfig(cfg.gyro_lpf);
+    } else {
+        if (!haveMpu6k)
+            mpu3050Config(cfg.gyro_lpf);
+    }
 
     // calculate magnetic declination
     deg = cfg.mag_declination / 100;