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

baseflight.uvproj

@@ -61,6 +61,8 @@
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
+            <nStopU1X>0</nStopU1X>
+            <nStopU2X>0</nStopU2X>
           </BeforeCompile>
           <BeforeMake>
             <RunUserProg1>0</RunUserProg1>
@@ -342,6 +344,7 @@
             <Rwpi>0</Rwpi>
             <wLevel>2</wLevel>
             <uThumb>0</uThumb>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define>STM32F10X_MD,USE_STDPERIPH_DRIVER</Define>
@@ -357,6 +360,7 @@
             <SplitLS>0</SplitLS>
             <SwStkChk>0</SwStkChk>
             <NoWarn>0</NoWarn>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define></Define>
@@ -550,6 +554,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -594,6 +599,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -619,6 +625,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\drv_i2c_soft.c</FilePath>
             </File>
+            <File>
+              <FileName>drv_l3g4200d.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\drv_l3g4200d.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -653,6 +664,7 @@
                 <Rwpi>2</Rwpi>
                 <wLevel>0</wLevel>
                 <uThumb>2</uThumb>
+                <uSurpInc>2</uSurpInc>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -668,6 +680,7 @@
                 <SplitLS>2</SplitLS>
                 <SwStkChk>2</SwStkChk>
                 <NoWarn>2</NoWarn>
+                <uSurpInc>2</uSurpInc>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -772,6 +785,7 @@
                     <SplitLS>2</SplitLS>
                     <SwStkChk>2</SwStkChk>
                     <NoWarn>2</NoWarn>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -841,6 +855,8 @@
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
+            <nStopU1X>0</nStopU1X>
+            <nStopU2X>0</nStopU2X>
           </BeforeCompile>
           <BeforeMake>
             <RunUserProg1>0</RunUserProg1>
@@ -1122,6 +1138,7 @@
             <Rwpi>0</Rwpi>
             <wLevel>2</wLevel>
             <uThumb>0</uThumb>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define>STM32F10X_MD,USE_STDPERIPH_DRIVER</Define>
@@ -1137,6 +1154,7 @@
             <SplitLS>0</SplitLS>
             <SwStkChk>0</SwStkChk>
             <NoWarn>0</NoWarn>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define></Define>
@@ -1330,6 +1348,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -1374,6 +1393,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -1399,6 +1419,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\drv_i2c_soft.c</FilePath>
             </File>
+            <File>
+              <FileName>drv_l3g4200d.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\drv_l3g4200d.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1498,6 +1523,7 @@
                     <SplitLS>2</SplitLS>
                     <SwStkChk>2</SwStkChk>
                     <NoWarn>2</NoWarn>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -1567,6 +1593,8 @@
             <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
+            <nStopU1X>0</nStopU1X>
+            <nStopU2X>0</nStopU2X>
           </BeforeCompile>
           <BeforeMake>
             <RunUserProg1>0</RunUserProg1>
@@ -1848,6 +1876,7 @@
             <Rwpi>0</Rwpi>
             <wLevel>0</wLevel>
             <uThumb>0</uThumb>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define>STM32F10X_MD,USE_STDPERIPH_DRIVER,FY90Q</Define>
@@ -1863,6 +1892,7 @@
             <SplitLS>0</SplitLS>
             <SwStkChk>0</SwStkChk>
             <NoWarn>0</NoWarn>
+            <uSurpInc>0</uSurpInc>
             <VariousControls>
               <MiscControls></MiscControls>
               <Define></Define>
@@ -2001,6 +2031,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2045,6 +2076,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2089,6 +2121,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2133,6 +2166,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2182,6 +2216,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2226,6 +2261,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2280,6 +2316,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2324,6 +2361,7 @@
                     <Rwpi>2</Rwpi>
                     <wLevel>0</wLevel>
                     <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>
@@ -2359,6 +2397,11 @@
               <FileType>1</FileType>
               <FilePath>.\src\drv_i2c_soft.c</FilePath>
             </File>
+            <File>
+              <FileName>drv_l3g4200d.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>.\src\drv_l3g4200d.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -2393,6 +2436,7 @@
                 <Rwpi>2</Rwpi>
                 <wLevel>0</wLevel>
                 <uThumb>2</uThumb>
+                <uSurpInc>2</uSurpInc>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -2408,6 +2452,7 @@
                 <SplitLS>2</SplitLS>
                 <SwStkChk>2</SwStkChk>
                 <NoWarn>2</NoWarn>
+                <uSurpInc>2</uSurpInc>
                 <VariousControls>
                   <MiscControls></MiscControls>
                   <Define></Define>
@@ -2507,6 +2552,7 @@
                     <SplitLS>2</SplitLS>
                     <SwStkChk>2</SwStkChk>
                     <NoWarn>2</NoWarn>
+                    <uSurpInc>2</uSurpInc>
                     <VariousControls>
                       <MiscControls></MiscControls>
                       <Define></Define>

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 (havel3g4200d) {
+        l3g4200dConfig(cfg.gyro_lpf);
+    } else {
         if (!haveMpu6k)
             mpu3050Config(cfg.gyro_lpf);
+    }
 
     // calculate magnetic declination
     deg = cfg.mag_declination / 100;