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
This commit is contained in:
parent
75adda0597
commit
ecda218e8f
|
@ -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>
|
||||
|
|
4815
obj/baseflight.hex
4815
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
@ -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"
|
||||
|
|
|
@ -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];
|
||||
}
|
|
@ -0,0 +1,4 @@
|
|||
#pragma once
|
||||
|
||||
bool l3g4200dDetect(sensor_t * gyro);
|
||||
void l3g4200dConfig(uint16_t lpf);
|
|
@ -138,3 +138,10 @@ int main(void)
|
|||
loop();
|
||||
}
|
||||
}
|
||||
|
||||
void HardFault_Handler(void)
|
||||
{
|
||||
// fall out of the sky
|
||||
writeAllMotors(cfg.mincommand);
|
||||
while (1);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue