rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

cleanup of sensor readings and sensor driver API reorganization part 1 

documented L3G4200D driver why 0x28 read was suddenly turning into 0xA8
removed old wiimotion averaging cruft from computeIMU
NOT FLIGHT TESTED

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@403 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-09-19 11:20:53 +00:00
parent 6d3467c759
commit 14893afb32
10 changed files with 75 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -60,6 +60,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
drv_system.c \
drv_uart.c \
printf.c \
utils.c \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)

15
baseflight.uvproj
View File

@ -462,6 +462,11 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1293,6 +1298,11 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -2068,6 +2078,11 @@
<FileType>1</FileType>
<FilePath>.\src\telemetry.c</FilePath>
</File>
<File>
<FileName>utils.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\utils.c</FilePath>
</File>
</Files>
</Group>
<Group>

28
src/board.h
View File

@ -32,7 +32,6 @@
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// Chip Unique ID on F103
#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
@ -40,11 +39,12 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,
SENSOR_MAG = 1 << 2,
SENSOR_SONAR = 1 << 3,
SENSOR_GPS = 1 << 4,
SENSOR_GYRO = 1 << 0, // always present
SENSOR_ACC = 1 << 1,
SENSOR_BARO = 1 << 2,
SENSOR_MAG = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_GPS = 1 << 5,
} AvailableSensors;
// Type of accelerometer used/detected
@ -98,12 +98,20 @@ typedef enum {
CW270_DEG_FLIP = 8
} sensor_align_e;
enum {
GYRO_UPDATED = 0,
ACC_UPDATED,
MAG_UPDATED,
TEMP_UPDATED
};
typedef struct sensor_data_t
{
int16_t x;
int16_t y;
int16_t z;
int16_t gyro[3];
int16_t acc[3];
int16_t mag[3];
float temperature;
int updated;
} sensor_data_t;
typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype
@ -207,6 +215,8 @@ typedef struct baro_t
#undef SOFT_I2C // enable to test software i2c
#include "utils.h"
#ifdef FY90Q
// FY90Q
#include "drv_adc.h"

11
src/drv_l3g4200d.c
View File

@ -3,6 +3,7 @@
// L3G4200D, Standard address 0x68
#define L3G4200D_ADDRESS 0x68
#define L3G4200D_ID 0xD3
#define L3G4200D_AUTOINCR 0x80
// Registers
#define L3G4200D_WHO_AM_I 0x0F
@ -13,7 +14,7 @@
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_GYRO_OUT 0xA8
#define L3G4200D_GYRO_OUT 0x28
// Bits
#define L3G4200D_POWER_ON 0x0F
@ -87,10 +88,10 @@ static void l3g4200dRead(int16_t *gyroData)
uint8_t buf[6];
int16_t data[3];
i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
data[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}

57
src/imu.c
View File

@ -45,39 +45,18 @@ void imuInit(void)
void computeIMU(void)
{
uint32_t axis;
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
Gyro_getADC();
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave = micros();
annexCode();
if ((micros() - timeInterleave) > 650) {
annex650_overrun_count++;
} else {
while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
}
Gyro_getADC();
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
if (!sensors(SENSOR_ACC))
accADC[axis] = 0;
accADC[X] = 0;
accADC[Y] = 0;
accADC[Z] = 0;
}
annexCode();
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
@ -95,6 +74,9 @@ void computeIMU(void)
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
} else {
for (axis = 0; axis < 3; axis++)
gyroData[axis] = gyroADC[axis];
}
}
@ -118,8 +100,6 @@ void computeIMU(void)
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
typedef struct fp_vector {
float X;
float Y;
@ -303,18 +283,15 @@ static void getEstimatedAttitude(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
// baseflight calculation (no, sorry, tarducopter calculation)
float magX = EstM.A[X];
float magY = EstM.A[Y];
float magZ = EstM.A[Z];
float cr = cosf(anglerad[ROLL]);
float sr = sinf(anglerad[ROLL]);
float cp = cosf(anglerad[PITCH]);
float sp = sinf(anglerad[PITCH]);
float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
float Yh = magY * cr - magZ * sr;
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = -hd;
// baseflight calculation by Luggi09
float cosineRoll = cosf(anglerad[ROLL]);
float sineRoll = sinf(anglerad[ROLL]);
float cosinePitch = cosf(anglerad[PITCH]);
float sinePitch = sinf(anglerad[PITCH]);
float Xh = EstM.A[X] * cosinePitch + EstM.A[Z] * sinePitch;
float Yh = EstM.A[X] * sinePitch * sineRoll + EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll * cosinePitch;
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = hd;
if (heading > 180)
heading = heading - 360;
else if (heading < -180)

3
src/mw.c
View File

@ -11,7 +11,6 @@ uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t headFreeModeHold;
int16_t annex650_overrun_count = 0;
uint8_t vbat; // battery voltage in 0.1V steps
int16_t telemTemperature1; // gyro sensor temperature
@ -841,7 +840,7 @@ void loop(void)
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
rcCommand[THROTTLE]+= throttleAngleCorrection;
rcCommand[THROTTLE] += throttleAngleCorrection;
}
if (sensors(SENSOR_GPS)) {

1
src/mw.h
View File

@ -343,7 +343,6 @@ extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;

7
src/sensors.c
View File

@ -321,7 +321,6 @@ static float devStandardDeviation(stdev_t *dev)
static void GYRO_Common(void)
{
int axis;
static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
static stdev_t var[3];
@ -355,12 +354,8 @@ static void GYRO_Common(void)
}
calibratingG--;
}
for (axis = 0; axis < 3; axis++) {
for (axis = 0; axis < 3; axis++)
gyroADC[axis] -= gyroZero[axis];
//anti gyro glitch, limit the variation between two consecutive readings
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
previousGyroADC[axis] = gyroADC[axis];
}
}
void Gyro_getADC(void)

12
src/utils.c Normal file
View File

@ -0,0 +1,12 @@
#include "board.h"
#include "mw.h"
int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}

3
src/utils.h Normal file
View File

@ -0,0 +1,3 @@
#pragma once
int constrain(int amt, int low, int high);