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

Remove mpu6050_scale from core_t by providing a sensor independent way

Browse source 
of determining a revision code.  Previously there was mpu6050 specific
code in cli.c (the status command).

Finally this commit has removed all non-serial port configuration
settings from core_t so that a future commit can refactor core_t to
reduce dependencies on serial port code.

In doing this I also noted from other source code that the MPU6050
accelerometer trim for some revisions appeared to be incorrectly set to
255 * 8 instead of 256 * 8.
This commit is contained in:
Dominic Clifton 2014-04-18 23:30:48 +01:00
parent 94c0f87c45
commit 2baf385b99
16 changed files with 59 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/config.c
View file

@ -10,6 +10,7 @@
#include "drivers/system_common.h"
#include "statusindicator.h"
#include "sensors_common.h"
#include "sensors_acceleration.h"
#include "telemetry_common.h"
#include "gps_common.h"

36
src/drivers/accgyro_mpu6050.c
View file

@ -132,9 +132,14 @@ static void mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(sensor_align_e align);
static void mpu6050GyroRead(int16_t *gyroData);
static uint8_t mpuAccelHalf = 0;
typedef enum {
MPU_6050_HALF_RESOLUTION,
MPU_6050_FULL_RESOLUTION
} mpu6050Resolution_e;
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
static mpu6050Resolution_e mpuAccelTrim;
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf)
{
bool ack;
uint8_t sig, rev;
@ -150,18 +155,22 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address.
// The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig != (MPU6050_ADDRESS & 0x7e))
return false;
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp);
rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01);
if (rev) {
/* Congrats, these parts are better. */
if (rev == 1) {
mpuAccelHalf = 1;
mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
} else if (rev == 2) {
mpuAccelHalf = 0;
mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
} else {
failureMode(5);
}
@ -171,24 +180,21 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
if (!rev) {
failureMode(5);
} else if (rev == 4) {
mpuAccelHalf = 1;
mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
} else {
mpuAccelHalf = 0;
mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
}
}
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES.
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
// give halfacc (old revision) back to system
if (scale)
*scale = mpuAccelHalf;
// default lpf is 42Hz
switch (lpf) {
case 256:
@ -220,10 +226,14 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
static void mpu6050AccInit(sensor_align_e align)
{
if (mpuAccelHalf)
acc_1G = 255 * 8;
else
switch(mpuAccelTrim) {
case MPU_6050_HALF_RESOLUTION:
acc_1G = 256 * 8;
break;
case MPU_6050_FULL_RESOLUTION:
acc_1G = 512 * 8;
break;
}
if (align > 0)
accAlign = align;

2
src/drivers/accgyro_mpu6050.h
View file

@ -1,5 +1,5 @@
#pragma once
bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale);
bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf);
void mpu6050DmpLoop(void);
void mpu6050DmpResetFifo(void);

1
src/flight_imu.c
View file

@ -3,6 +3,7 @@
#include "common/maths.h"
#include "sensors_gyro.h"
#include "sensors_compass.h"
#include "flight_common.h"

1
src/mw.c
View file

@ -4,6 +4,7 @@
#include "common/maths.h"
#include "common/typeconversion.h"
#include "sensors_gyro.h"
#include "flight_common.h"
#include "serial_cli.h"
#include "telemetry_common.h"

3
src/mw.h
View file

@ -90,9 +90,6 @@ extern uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtit
extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength)
extern flags_t f;
extern sensor_t acc;
extern sensor_t gyro;
extern baro_t baro;
// main
void setPIDController(int type);

1
src/runtime_config.h
View file

@ -53,7 +53,6 @@ typedef struct core_t {
serialPort_t *gpsport;
serialPort_t *telemport;
serialPort_t *rcvrport;
uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver.
} core_t;
typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype

4
src/sensors_acceleration.c
View file

@ -4,6 +4,10 @@
#include "mw.h"
#include "sensors_acceleration.h"
sensor_t acc; // acc access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.

4
src/sensors_acceleration.h
View file

@ -10,5 +10,9 @@ typedef enum AccelSensors {
ACC_NONE = 5
} AccelSensors;
extern uint8_t accHardware;
extern sensor_t acc;
void ACC_Common(void);
void ACC_getADC(void);

2
src/sensors_barometer.c
View file

@ -1,6 +1,8 @@
#include "board.h"
#include "mw.h"
baro_t baro; // barometer access functions
#ifdef BARO
void Baro_Common(void)
{

2
src/sensors_barometer.h
View file

@ -1,5 +1,7 @@
#pragma once
extern baro_t baro;
#ifdef BARO
void Baro_Common(void);
int Baro_update(void);

1
src/sensors_common.h
View file

@ -31,6 +31,7 @@ typedef struct sensor_t
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor (currently used for gyro only, todo for accel)
char revisionCode; // a revision code for the sensor, if known
} sensor_t;
extern int16_t heading;

2
src/sensors_gyro.c
View file

@ -6,6 +6,8 @@
#include "flight_common.h"
#include "statusindicator.h"
sensor_t gyro; // gyro access functions
void GYRO_Common(void)
{
int axis;

3
src/sensors_gyro.h
View file

@ -1,4 +1,7 @@
#pragma once
extern sensor_t gyro;
void GYRO_Common(void);
void Gyro_getADC(void);

18
src/sensors_initialisation.c
View file

@ -2,6 +2,10 @@
#include "mw.h"
#include "sensors_acceleration.h"
#include "sensors_barometer.h"
#include "sensors_gyro.h"
#include "sensors_common.h"
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
uint16_t calibratingG = 0;
@ -12,15 +16,12 @@ extern uint16_t batteryWarningVoltage;
extern uint8_t batteryCellCount;
extern float magneticDeclination;
sensor_t acc; // acc access functions
sensor_t gyro; // gyro access functions
baro_t baro; // barometer access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
#ifdef FY90Q
// FY90Q analog gyro/acc
void sensorsAutodetect(void)
{
memset(&acc, sizeof(acc), 0);
memset(&gyro, sizeof(gyro), 0);
adcSensorInit(&acc, &gyro);
}
#else
@ -31,8 +32,11 @@ void sensorsAutodetect(void)
drv_adxl345_config_t acc_params;
bool haveMpu6k = false;
memset(&acc, sizeof(acc), 0);
memset(&gyro, sizeof(gyro), 0);
// Autodetect gyro hardware. We have MPU3050 or MPU6050.
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) {
if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf)) {
// this filled up acc.* struct with init values
haveMpu6k = true;
} else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
@ -60,7 +64,7 @@ retry:
; // fallthrough
case ACC_MPU6050: // MPU6050
if (haveMpu6k) {
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again. re-fill acc struct
accHardware = ACC_MPU6050;
if (mcfg.acc_hardware == ACC_MPU6050)
break;

5
src/serial_cli.c
View file

@ -30,7 +30,6 @@ static void cliVersion(char *cmdline);
// from sensors.c
extern uint8_t batteryCellCount;
extern uint8_t accHardware;
// from config.c RC Channel mapping
extern const char rcChannelLetters[];
@ -996,8 +995,8 @@ static void cliStatus(char *cmdline)
}
if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]);
if (accHardware == ACC_MPU6050)
printf(".%c", core.mpu6050_scale ? 'o' : 'n');
if (acc.revisionCode)
printf(".%c", acc.revisionCode);
}
cliPrint("\r\n");