Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00
Code Issues Wiki Activity

fixed bug in mpu6050 accel code

Browse source 
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
This commit is contained in:
timecop 2012-08-07 08:00:16 +00:00
parent ecda218e8f
commit e894dba995
7 changed files with 1702 additions and 1693 deletions
Download patch file Download diff file Expand all files Collapse all files

3355
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

1
src/cli.c
View file

@ -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 },

3
src/config.c
View file

@ -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;

29
src/drv_mpu6050.c
View file

@ -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
i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &mpuProductID);
// get chip revision + fake it if needed
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
}

2
src/drv_mpu6050.h
View file

@ -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);

1
src/mw.h
View file

@ -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

4
src/sensors.c
View file

@ -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;