beginnings of the great sensor orientation unfucking. WORK IN PROGRESS DO NOT FLY.
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@397 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com
parent
edb0ef01b7
commit
1cc306493b
39
src/board.h
39
src/board.h
|
|
@ -53,6 +53,7 @@ typedef enum AccelSensors {
|
|||
ACC_ADXL345 = 1,
|
||||
ACC_MPU6050 = 2,
|
||||
ACC_MMA8452 = 3,
|
||||
ACC_NONE = 4
|
||||
} AccelSensors;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -79,8 +80,35 @@ typedef enum {
|
|||
GPS_MTK,
|
||||
} GPSHardware;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
|
||||
typedef enum {
|
||||
X = 0,
|
||||
Y,
|
||||
Z
|
||||
} sensor_axis_e;
|
||||
|
||||
typedef enum {
|
||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||
CW0_DEG = 1,
|
||||
CW90_DEG = 2,
|
||||
CW180_DEG = 3,
|
||||
CW270_DEG = 4,
|
||||
CW0_DEG_FLIP = 5,
|
||||
CW90_DEG_FLIP = 6,
|
||||
CW180_DEG_FLIP = 7,
|
||||
CW270_DEG_FLIP = 8
|
||||
} sensor_align_e;
|
||||
|
||||
typedef struct sensor_data_t
|
||||
{
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
float temperature;
|
||||
} sensor_data_t;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype
|
||||
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||
typedef void (* baroOpFuncPtr)(void); // baro start operation
|
||||
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
|
||||
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
|
||||
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
|
||||
|
|
@ -90,7 +118,6 @@ typedef struct sensor_t
|
|||
{
|
||||
sensorInitFuncPtr init; // initialize function
|
||||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
sensorReadFuncPtr align; // sensor align
|
||||
sensorReadFuncPtr temperature; // read temperature if available
|
||||
float scale; // scalefactor (currently used for gyro only, todo for accel)
|
||||
} sensor_t;
|
||||
|
|
@ -99,10 +126,10 @@ typedef struct baro_t
|
|||
{
|
||||
uint16_t ut_delay;
|
||||
uint16_t up_delay;
|
||||
sensorInitFuncPtr start_ut;
|
||||
sensorInitFuncPtr get_ut;
|
||||
sensorInitFuncPtr start_up;
|
||||
sensorInitFuncPtr get_up;
|
||||
baroOpFuncPtr start_ut;
|
||||
baroOpFuncPtr get_ut;
|
||||
baroOpFuncPtr start_up;
|
||||
baroOpFuncPtr get_up;
|
||||
baroCalculateFuncPtr calculate;
|
||||
} baro_t;
|
||||
|
||||
|
|
|
|||
14
src/cli.c
14
src/cli.c
|
|
@ -121,16 +121,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
|
||||
{ "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
|
||||
{ "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },
|
||||
{ "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 },
|
||||
{ "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 },
|
||||
{ "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 },
|
||||
{ "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 },
|
||||
{ "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 },
|
||||
{ "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 },
|
||||
{ "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 },
|
||||
{ "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 },
|
||||
{ "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 },
|
||||
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 },
|
||||
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
|
||||
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
|
||||
{ "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
|
||||
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 4 },
|
||||
{ "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
|
||||
{ "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
|
||||
{ "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
|
||||
|
|
|
|||
|
|
@ -159,7 +159,6 @@ void checkFirstTime(bool reset)
|
|||
static void resetConf(void)
|
||||
{
|
||||
int i;
|
||||
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
|
||||
|
||||
// Clear all configuration
|
||||
memset(&mcfg, 0, sizeof(master_t));
|
||||
|
|
@ -178,7 +177,9 @@ static void resetConf(void)
|
|||
mcfg.accZero[0] = 0;
|
||||
mcfg.accZero[1] = 0;
|
||||
mcfg.accZero[2] = 0;
|
||||
memcpy(&mcfg.align, default_align, sizeof(mcfg.align));
|
||||
mcfg.gyro_align = ALIGN_DEFAULT;
|
||||
mcfg.acc_align = ALIGN_DEFAULT;
|
||||
mcfg.mag_align = ALIGN_DEFAULT;
|
||||
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
mcfg.moron_threshold = 32;
|
||||
mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
|
||||
|
|
|
|||
|
|
@ -31,11 +31,11 @@
|
|||
|
||||
extern uint16_t acc_1G;
|
||||
|
||||
static void adxl345Init(void);
|
||||
static void adxl345Init(sensor_align_e align);
|
||||
static void adxl345Read(int16_t *accelData);
|
||||
static void adxl345Align(int16_t *accelData);
|
||||
|
||||
static bool useFifo = false;
|
||||
static sensor_align_e accAlign = CW0_DEG;
|
||||
|
||||
bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
|
||||
{
|
||||
|
|
@ -55,11 +55,10 @@ bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
|
|||
|
||||
acc->init = adxl345Init;
|
||||
acc->read = adxl345Read;
|
||||
acc->align = adxl345Align;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void adxl345Init(void)
|
||||
static void adxl345Init(sensor_align_e align)
|
||||
{
|
||||
if (useFifo) {
|
||||
uint8_t fifoDepth = 16;
|
||||
|
|
@ -73,6 +72,9 @@ static void adxl345Init(void)
|
|||
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
|
||||
}
|
||||
acc_1G = 265; // 3.3V operation
|
||||
|
||||
if (align > 0)
|
||||
accAlign = align;
|
||||
}
|
||||
|
||||
uint8_t acc_samples = 0;
|
||||
|
|
@ -80,6 +82,7 @@ uint8_t acc_samples = 0;
|
|||
static void adxl345Read(int16_t *accelData)
|
||||
{
|
||||
uint8_t buf[8];
|
||||
int16_t data[3];
|
||||
|
||||
if (useFifo) {
|
||||
int32_t x = 0;
|
||||
|
|
@ -96,19 +99,16 @@ static void adxl345Read(int16_t *accelData)
|
|||
z += (int16_t)(buf[4] + (buf[5] << 8));
|
||||
samples_remaining = buf[7] & 0x7F;
|
||||
} while ((i < 32) && (samples_remaining > 0));
|
||||
accelData[0] = x / i;
|
||||
accelData[1] = y / i;
|
||||
accelData[2] = z / i;
|
||||
data[0] = x / i;
|
||||
data[1] = y / i;
|
||||
data[2] = z / i;
|
||||
acc_samples = i;
|
||||
} else {
|
||||
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
|
||||
accelData[0] = buf[0] + (buf[1] << 8);
|
||||
accelData[1] = buf[2] + (buf[3] << 8);
|
||||
accelData[2] = buf[4] + (buf[5] << 8);
|
||||
data[0] = buf[0] + (buf[1] << 8);
|
||||
data[1] = buf[2] + (buf[3] << 8);
|
||||
data[2] = buf[4] + (buf[5] << 8);
|
||||
}
|
||||
}
|
||||
|
||||
static void adxl345Align(int16_t *accData)
|
||||
{
|
||||
// official direction is RPY, nothing to change here.
|
||||
alignSensors(data, accelData, accAlign);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -73,9 +73,9 @@
|
|||
#define HMC_POS_BIAS 1
|
||||
#define HMC_NEG_BIAS 2
|
||||
|
||||
static int8_t sensor_align[3];
|
||||
static sensor_align_e magAlign = CW180_DEG;
|
||||
|
||||
bool hmc5883lDetect(int8_t *align)
|
||||
bool hmc5883lDetect(sensor_align_e align)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
|
@ -84,7 +84,8 @@ bool hmc5883lDetect(int8_t *align)
|
|||
if (!ack || sig != 'H')
|
||||
return false;
|
||||
|
||||
memcpy(sensor_align, align, 3);
|
||||
if (align > 0)
|
||||
magAlign = align;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -184,18 +185,11 @@ void hmc5883lRead(int16_t *magData)
|
|||
{
|
||||
uint8_t buf[6];
|
||||
int16_t mag[3];
|
||||
int i;
|
||||
|
||||
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
|
||||
mag[0] = buf[0] << 8 | buf[1];
|
||||
mag[1] = buf[2] << 8 | buf[3];
|
||||
mag[2] = buf[4] << 8 | buf[5];
|
||||
mag[X] = buf[0] << 8 | buf[1];
|
||||
mag[Z] = buf[2] << 8 | buf[3];
|
||||
mag[Y] = buf[4] << 8 | buf[5];
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
int8_t axis = sensor_align[i];
|
||||
if (axis > 0)
|
||||
magData[axis - 1] = mag[i];
|
||||
else
|
||||
magData[-axis - 1] = -mag[i];
|
||||
}
|
||||
alignSensors(mag, magData, magAlign);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
#pragma once
|
||||
|
||||
bool hmc5883lDetect(int8_t *align);
|
||||
bool hmc5883lDetect(sensor_align_e align);
|
||||
void hmc5883lInit(float *calibrationGain);
|
||||
void hmc5883lRead(int16_t *magData);
|
||||
|
|
|
|||
|
|
@ -24,10 +24,10 @@
|
|||
#define L3G4200D_DLPF_93HZ 0xC0
|
||||
|
||||
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
|
||||
static sensor_align_e gyroAlign = CW0_DEG;
|
||||
|
||||
static void l3g4200dInit(void);
|
||||
static void l3g4200dInit(sensor_align_e align);
|
||||
static void l3g4200dRead(int16_t *gyroData);
|
||||
static void l3g4200dAlign(int16_t *gyroData);
|
||||
|
||||
bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
|
||||
{
|
||||
|
|
@ -41,7 +41,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
|
|||
|
||||
gyro->init = l3g4200dInit;
|
||||
gyro->read = l3g4200dRead;
|
||||
gyro->align = l3g4200dAlign;
|
||||
|
||||
// 14.2857dps/lsb scalefactor
|
||||
gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
|
||||
|
||||
|
|
@ -65,7 +65,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void l3g4200dInit(void)
|
||||
static void l3g4200dInit(sensor_align_e align)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
|
|
@ -77,21 +77,20 @@ static void l3g4200dInit(void)
|
|||
|
||||
delay(5);
|
||||
i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
|
||||
}
|
||||
|
||||
static void l3g4200dAlign(int16_t *gyroData)
|
||||
{
|
||||
gyroData[0] = -gyroData[0];
|
||||
gyroData[1] = gyroData[1];
|
||||
gyroData[2] = -gyroData[2];
|
||||
if (align > 0)
|
||||
gyroAlign = align;
|
||||
}
|
||||
|
||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||
static void l3g4200dRead(int16_t *gyroData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
int16_t data[3];
|
||||
|
||||
i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
|
||||
gyroData[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
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;
|
||||
|
||||
alignSensors(data, gyroData, gyroAlign);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,10 +49,10 @@
|
|||
|
||||
extern uint16_t acc_1G;
|
||||
static uint8_t device_id;
|
||||
static sensor_align_e accAlign = CW180_DEG;
|
||||
|
||||
static void mma8452Init(void);
|
||||
static void mma8452Init(sensor_align_e align);
|
||||
static void mma8452Read(int16_t *accelData);
|
||||
static void mma8452Align(int16_t *accelData);
|
||||
|
||||
bool mma8452Detect(sensor_t *acc)
|
||||
{
|
||||
|
|
@ -69,12 +69,11 @@ bool mma8452Detect(sensor_t *acc)
|
|||
|
||||
acc->init = mma8452Init;
|
||||
acc->read = mma8452Read;
|
||||
acc->align = mma8452Align;
|
||||
device_id = sig;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mma8452Init(void)
|
||||
static void mma8452Init(sensor_align_e align)
|
||||
{
|
||||
gpio_config_t gpio;
|
||||
|
||||
|
|
@ -95,21 +94,20 @@ static void mma8452Init(void)
|
|||
i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
|
||||
|
||||
acc_1G = 256;
|
||||
|
||||
if (align > 0)
|
||||
accAlign = align;
|
||||
}
|
||||
|
||||
static void mma8452Read(int16_t *accelData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
int16_t data[3];
|
||||
|
||||
i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
|
||||
accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
|
||||
accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
|
||||
accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
|
||||
}
|
||||
data[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
|
||||
data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
|
||||
data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
|
||||
|
||||
static void mma8452Align(int16_t *accelData)
|
||||
{
|
||||
accelData[0] = -accelData[0];
|
||||
accelData[1] = -accelData[1];
|
||||
accelData[2] = accelData[2];
|
||||
alignSensors(data, accelData, accAlign);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,10 +25,10 @@
|
|||
#define MPU3050_CLK_SEL_PLL_GX 0x01
|
||||
|
||||
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
|
||||
static sensor_align_e gyroAlign = CW0_DEG;
|
||||
|
||||
static void mpu3050Init(void);
|
||||
static void mpu3050Init(sensor_align_e align);
|
||||
static void mpu3050Read(int16_t *gyroData);
|
||||
static void mpu3050Align(int16_t *gyroData);
|
||||
static void mpu3050ReadTemp(int16_t *tempData);
|
||||
|
||||
bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
|
||||
|
|
@ -43,7 +43,6 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
|
|||
|
||||
gyro->init = mpu3050Init;
|
||||
gyro->read = mpu3050Read;
|
||||
gyro->align = mpu3050Align;
|
||||
gyro->temperature = mpu3050ReadTemp;
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
|
||||
|
|
@ -74,7 +73,7 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void mpu3050Init(void)
|
||||
static void mpu3050Init(sensor_align_e align)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
|
|
@ -88,24 +87,23 @@ static void mpu3050Init(void)
|
|||
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
}
|
||||
|
||||
static void mpu3050Align(int16_t *gyroData)
|
||||
{
|
||||
// official direction is RPY
|
||||
gyroData[0] = gyroData[0];
|
||||
gyroData[1] = gyroData[1];
|
||||
gyroData[2] = -gyroData[2];
|
||||
if (align > 0)
|
||||
gyroAlign = align;
|
||||
}
|
||||
|
||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||
static void mpu3050Read(int16_t *gyroData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
int16_t data[3];
|
||||
|
||||
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
|
||||
alignSensors(data, gyroData, gyroAlign);
|
||||
}
|
||||
|
||||
static void mpu3050ReadTemp(int16_t *tempData)
|
||||
|
|
|
|||
|
|
@ -4,26 +4,9 @@
|
|||
// MPU_INT on PB13 on rev4 hardware
|
||||
#define MPU6050_ADDRESS 0x68
|
||||
|
||||
// Experimental DMP support
|
||||
// #define MPU6050_DMP
|
||||
|
||||
#define DMP_MEM_START_ADDR 0x6E
|
||||
#define DMP_MEM_R_W 0x6F
|
||||
|
||||
#define INV_MAX_NUM_ACCEL_SAMPLES (8)
|
||||
#define DMP_REF_QUATERNION (0)
|
||||
#define DMP_REF_GYROS (DMP_REF_QUATERNION + 4) // 4
|
||||
#define DMP_REF_CONTROL (DMP_REF_GYROS + 3) // 7
|
||||
#define DMP_REF_RAW (DMP_REF_CONTROL + 4) // 11
|
||||
#define DMP_REF_RAW_EXTERNAL (DMP_REF_RAW + 8) // 19
|
||||
#define DMP_REF_ACCEL (DMP_REF_RAW_EXTERNAL + 6) // 25
|
||||
#define DMP_REF_QUANT_ACCEL (DMP_REF_ACCEL + 3) // 28
|
||||
#define DMP_REF_QUATERNION_6AXIS (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) // 36
|
||||
#define DMP_REF_EIS (DMP_REF_QUATERNION_6AXIS + 4) // 40
|
||||
#define DMP_REF_DMP_PACKET (DMP_REF_EIS + 3) // 43
|
||||
#define DMP_REF_GARBAGE (DMP_REF_DMP_PACKET + 1) // 44
|
||||
#define DMP_REF_LAST (DMP_REF_GARBAGE + 1) // 45
|
||||
|
||||
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
|
||||
|
|
@ -113,7 +96,7 @@
|
|||
#define MPU_RA_FIFO_R_W 0x74
|
||||
#define MPU_RA_WHO_AM_I 0x75
|
||||
|
||||
#define MPU6050_SMPLRT_DIV 0 //8000Hz
|
||||
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
|
||||
|
||||
#define MPU6050_LPF_256HZ 0
|
||||
#define MPU6050_LPF_188HZ 1
|
||||
|
|
@ -124,19 +107,13 @@
|
|||
#define MPU6050_LPF_5HZ 6
|
||||
|
||||
static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
|
||||
static sensor_align_e gyroAlign = CW0_DEG;
|
||||
static sensor_align_e accAlign = CW0_DEG;
|
||||
|
||||
static void mpu6050AccInit(void);
|
||||
static void mpu6050AccInit(sensor_align_e align);
|
||||
static void mpu6050AccRead(int16_t *accData);
|
||||
static void mpu6050AccAlign(int16_t *accData);
|
||||
static void mpu6050GyroInit(void);
|
||||
static void mpu6050GyroInit(sensor_align_e align);
|
||||
static void mpu6050GyroRead(int16_t *gyroData);
|
||||
static void mpu6050GyroAlign(int16_t *gyroData);
|
||||
|
||||
#ifdef MPU6050_DMP
|
||||
static void mpu6050DmpInit(void);
|
||||
float dmpdata[2];
|
||||
int16_t dmpGyroData[3];
|
||||
#endif
|
||||
|
||||
extern uint16_t acc_1G;
|
||||
static uint8_t mpuAccelHalf = 0;
|
||||
|
|
@ -186,12 +163,11 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
|
|||
|
||||
acc->init = mpu6050AccInit;
|
||||
acc->read = mpu6050AccRead;
|
||||
acc->align = mpu6050AccAlign;
|
||||
gyro->init = mpu6050GyroInit;
|
||||
gyro->read = mpu6050GyroRead;
|
||||
gyro->align = mpu6050GyroAlign;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
|
||||
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
|
||||
|
||||
// give halfacc (old revision) back to system
|
||||
if (scale)
|
||||
|
|
@ -223,48 +199,34 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
|
|||
break;
|
||||
}
|
||||
|
||||
#ifdef MPU6050_DMP
|
||||
mpu6050DmpInit();
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mpu6050AccInit(void)
|
||||
static void mpu6050AccInit(sensor_align_e align)
|
||||
{
|
||||
if (mpuAccelHalf)
|
||||
acc_1G = 255 * 8;
|
||||
else
|
||||
acc_1G = 512 * 8;
|
||||
|
||||
if (align > 0)
|
||||
accAlign = align;
|
||||
}
|
||||
|
||||
static void mpu6050AccRead(int16_t *accData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
int16_t data[3];
|
||||
|
||||
#ifndef MPU6050_DMP
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
|
||||
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
#else
|
||||
accData[0] = accData[1] = accData[2] = 0;
|
||||
#endif
|
||||
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
|
||||
alignSensors(data, accData, accAlign);
|
||||
}
|
||||
|
||||
static void mpu6050AccAlign(int16_t *accData)
|
||||
{
|
||||
int16_t temp[2];
|
||||
temp[0] = accData[0];
|
||||
temp[1] = accData[1];
|
||||
|
||||
// official direction is RPY
|
||||
accData[0] = temp[1];
|
||||
accData[1] = -temp[0];
|
||||
accData[2] = accData[2];
|
||||
}
|
||||
|
||||
static void mpu6050GyroInit(void)
|
||||
static void mpu6050GyroInit(sensor_align_e align)
|
||||
{
|
||||
gpio_config_t gpio;
|
||||
|
||||
|
|
@ -277,7 +239,6 @@ static void mpu6050GyroInit(void)
|
|||
else if (hse_value == 12000000)
|
||||
gpioInit(GPIOC, &gpio);
|
||||
|
||||
#ifndef MPU6050_DMP
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
delay(5);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||
|
|
@ -289,522 +250,20 @@ static void mpu6050GyroInit(void)
|
|||
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
|
||||
// Accel scale 8g (4096 LSB/g)
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
|
||||
#endif
|
||||
|
||||
if (align > 0)
|
||||
gyroAlign = align;
|
||||
}
|
||||
|
||||
static void mpu6050GyroRead(int16_t * gyroData)
|
||||
static void mpu6050GyroRead(int16_t *gyroData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
#ifndef MPU6050_DMP
|
||||
int16_t data[3];
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
|
||||
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
#else
|
||||
gyroData[0] = dmpGyroData[0] / 4 ;
|
||||
gyroData[1] = dmpGyroData[1] / 4;
|
||||
gyroData[2] = dmpGyroData[2] / 4;
|
||||
#endif
|
||||
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
|
||||
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
|
||||
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
|
||||
|
||||
alignSensors(data, gyroData, gyroAlign);
|
||||
}
|
||||
|
||||
static void mpu6050GyroAlign(int16_t * gyroData)
|
||||
{
|
||||
// official direction is RPY
|
||||
gyroData[0] = gyroData[0];
|
||||
gyroData[1] = gyroData[1];
|
||||
gyroData[2] = -gyroData[2];
|
||||
}
|
||||
|
||||
#ifdef MPU6050_DMP
|
||||
|
||||
//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
|
||||
//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
|
||||
//library only supports 32 byte transmissions, including the register address to which you're writing,
|
||||
//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
|
||||
//
|
||||
//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
|
||||
//directly from that code. That is true of all transmissions in this sketch, and any documentation has
|
||||
//been added after the fact by referencing the Invensense code.
|
||||
|
||||
const unsigned char dmpMem[8][16][16] = {
|
||||
{
|
||||
{0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00},
|
||||
{0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01},
|
||||
{0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01},
|
||||
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00},
|
||||
{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
|
||||
{0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82},
|
||||
{0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00},
|
||||
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0},
|
||||
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC},
|
||||
{0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4},
|
||||
{0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10}
|
||||
},
|
||||
{
|
||||
{0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8},
|
||||
{0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7},
|
||||
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C},
|
||||
{0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C},
|
||||
{0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0}
|
||||
},
|
||||
{
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
|
||||
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
|
||||
},
|
||||
{
|
||||
{0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F},
|
||||
{0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2},
|
||||
{0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF},
|
||||
{0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C},
|
||||
{0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1},
|
||||
{0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01},
|
||||
{0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80},
|
||||
{0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C},
|
||||
{0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80},
|
||||
{0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E},
|
||||
{0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9},
|
||||
{0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24},
|
||||
{0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0},
|
||||
{0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86},
|
||||
{0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1},
|
||||
{0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86}
|
||||
},
|
||||
{
|
||||
{0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA},
|
||||
{0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C},
|
||||
{0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8},
|
||||
{0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3},
|
||||
{0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84},
|
||||
{0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5},
|
||||
{0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3},
|
||||
{0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1},
|
||||
{0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5},
|
||||
{0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D},
|
||||
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
|
||||
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D},
|
||||
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
|
||||
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A},
|
||||
{0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8},
|
||||
{0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87}
|
||||
},
|
||||
{
|
||||
{0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8},
|
||||
{0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68},
|
||||
{0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D},
|
||||
{0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94},
|
||||
{0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA},
|
||||
{0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56},
|
||||
{0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9},
|
||||
{0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA},
|
||||
{0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A},
|
||||
{0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60},
|
||||
{0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97},
|
||||
{0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04},
|
||||
{0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78},
|
||||
{0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79},
|
||||
{0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68},
|
||||
{0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68}
|
||||
},
|
||||
{
|
||||
{0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04},
|
||||
{0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66},
|
||||
{0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31},
|
||||
{0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60},
|
||||
{0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76},
|
||||
{0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56},
|
||||
{0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD},
|
||||
{0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91},
|
||||
{0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8},
|
||||
{0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE},
|
||||
{0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9},
|
||||
{0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD},
|
||||
{0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E},
|
||||
{0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8},
|
||||
{0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89},
|
||||
{0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79}
|
||||
},
|
||||
{
|
||||
{0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8},
|
||||
{0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA},
|
||||
{0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB},
|
||||
{0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3},
|
||||
{0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3},
|
||||
{0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3},
|
||||
{0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3},
|
||||
{0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC},
|
||||
{0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
//DMP update transmissions (Bank, Start Address, Update Length, Update Data...)
|
||||
|
||||
const uint8_t dmp_updates[29][9] = {
|
||||
{0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C}, //FCFG_1 inv_set_gyro_calibration
|
||||
{0x03, 0xAB, 0x03, 0x36, 0x56, 0x76}, //FCFG_3 inv_set_gyro_calibration
|
||||
{0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2}, //D_0_104 inv_set_gyro_calibration
|
||||
{0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1}, //D_0_24 inv_set_gyro_calibration
|
||||
{0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00}, //D_1_152 inv_set_accel_calibration
|
||||
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_accel_calibration
|
||||
{0x03, 0x89, 0x03, 0x26, 0x46, 0x66}, //FCFG_7 inv_set_accel_calibration
|
||||
{0x00, 0x6C, 0x02, 0x20, 0x00}, //D_0_108 inv_set_accel_calibration
|
||||
{0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_00 inv_set_compass_calibration
|
||||
{0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_01
|
||||
{0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_02
|
||||
{0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_10
|
||||
{0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_11
|
||||
{0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_12
|
||||
{0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_20
|
||||
{0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_21
|
||||
{0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_22
|
||||
{0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00}, //D_1_236 inv_apply_endian_accel
|
||||
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_mpu_sensors
|
||||
{0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D}, //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
|
||||
{0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D}, //FCFG_5 inv_set_bias_update
|
||||
{0x00, 0xA3, 0x01, 0x00}, //D_0_163 inv_set_dead_zone
|
||||
//SET INT_ENABLE at i=22
|
||||
{0x07, 0x86, 0x01, 0xFE}, //CFG_6 inv_set_fifo_interupt
|
||||
{0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38}, //CFG_8 inv_send_quaternion
|
||||
{0x07, 0x7E, 0x01, 0x30}, //CFG_16 inv_set_footer
|
||||
{0x07, 0x46, 0x01, 0x9A}, //CFG_GYRO_SOURCE inv_send_gyro
|
||||
{0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
|
||||
{0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
|
||||
{0x02, 0x16, 0x02, 0x00, 0x00}, //D_0_22 inv_set_fifo_rate
|
||||
};
|
||||
|
||||
static long dmp_lastRead = 0;
|
||||
static uint8_t dmp_processed_packet[8];
|
||||
static uint8_t dmp_received_packet[50];
|
||||
static uint8_t dmp_temp = 0;
|
||||
uint8_t dmp_fifoCountL = 0;
|
||||
static uint8_t dmp_fifoCountL2 = 0;
|
||||
static uint8_t dmp_packetCount = 0x00;
|
||||
static bool dmp_longPacket = false;
|
||||
static bool dmp_firstPacket = true;
|
||||
|
||||
static void mpu6050DmpMemInit(void);
|
||||
static void mpu6050DmpBankSelect(uint8_t bank);
|
||||
static bool mpu6050DmpFifoReady(void);
|
||||
static void mpu6050DmpGetPacket(void);
|
||||
static void mpu6050DmpProcessQuat(void);
|
||||
void mpu6050DmpResetFifo(void);
|
||||
|
||||
static void mpu6050DmpInit(void)
|
||||
{
|
||||
uint8_t temp = 0;
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0xC0); // device reset
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
|
||||
delay(10);
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x70);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x06);
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 1, &temp);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x00);
|
||||
|
||||
/*
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_XG_OFFS_TC);
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_YG_OFFS_TC);
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_ZG_OFFS_TC);
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL);
|
||||
*/
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0x32); // I2C bypass enabled, latch int enabled, int read clear
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
|
||||
delay(5);
|
||||
|
||||
mpu6050DmpMemInit();
|
||||
}
|
||||
|
||||
void mpu6050DmpLoop(void)
|
||||
{
|
||||
uint8_t temp;
|
||||
uint8_t buf[2];
|
||||
|
||||
if (mpu6050DmpFifoReady()) {
|
||||
LED1_ON;
|
||||
mpu6050DmpGetPacket();
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
|
||||
if (dmp_firstPacket) {
|
||||
delay(1);
|
||||
mpu6050DmpBankSelect(0x00);
|
||||
|
||||
mpu6050DmpBankSelect(0x00); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
|
||||
|
||||
mpu6050DmpBankSelect(0x01);
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x62);
|
||||
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 2, buf);
|
||||
dmp_firstPacket = false;
|
||||
mpu6050DmpFifoReady();
|
||||
}
|
||||
|
||||
if (dmp_fifoCountL == 42) {
|
||||
mpu6050DmpProcessQuat();
|
||||
}
|
||||
LED1_OFF;
|
||||
}
|
||||
}
|
||||
|
||||
#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
|
||||
extern int16_t angle[2];
|
||||
|
||||
static void mpu6050DmpProcessQuat(void)
|
||||
{
|
||||
float quat0, quat1, quat2, quat3;
|
||||
int32_t quatl0, quatl1, quatl2, quatl3;
|
||||
|
||||
quatl0 = dmp_quatTake32(dmp_received_packet, 0);
|
||||
quatl1 = dmp_quatTake32(dmp_received_packet, 1);
|
||||
quatl2 = dmp_quatTake32(dmp_received_packet, 2);
|
||||
quatl3 = dmp_quatTake32(dmp_received_packet, 3);
|
||||
|
||||
if (quatl0 > 32767)
|
||||
quatl0 -= 65536;
|
||||
if (quatl1 > 32767)
|
||||
quatl1 -= 65536;
|
||||
if (quatl2 > 32767)
|
||||
quatl2 -= 65536;
|
||||
if (quatl3 > 32767)
|
||||
quatl3 -= 65536;
|
||||
|
||||
quat0 = ((float) quatl0) / 16384.0f;
|
||||
quat1 = ((float) quatl1) / 16384.0f;
|
||||
quat2 = ((float) quatl2) / 16384.0f;
|
||||
quat3 = ((float) quatl3) / 16384.0f;
|
||||
|
||||
dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
|
||||
dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
|
||||
angle[0] = dmpdata[0] * 10;
|
||||
angle[1] = dmpdata[1] * 10;
|
||||
|
||||
dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
|
||||
dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
|
||||
dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
|
||||
}
|
||||
|
||||
void mpu6050DmpResetFifo(void)
|
||||
{
|
||||
uint8_t ctrl;
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &ctrl);
|
||||
ctrl |= 0x04;
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, ctrl);
|
||||
}
|
||||
|
||||
static void mpu6050DmpGetPacket(void)
|
||||
{
|
||||
if (dmp_fifoCountL > 32) {
|
||||
dmp_fifoCountL2 = dmp_fifoCountL - 32;
|
||||
dmp_longPacket = true;
|
||||
}
|
||||
|
||||
if (dmp_longPacket) {
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, 32, dmp_received_packet);
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet + 32);
|
||||
dmp_longPacket = false;
|
||||
} else {
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t dmpFifoLevel = 0;
|
||||
|
||||
static bool mpu6050DmpFifoReady(void)
|
||||
{
|
||||
uint8_t buf[2];
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
|
||||
|
||||
dmp_fifoCountL = buf[1];
|
||||
dmpFifoLevel = buf[0] << 8 | buf[1];
|
||||
|
||||
if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
|
||||
return true;
|
||||
else {
|
||||
// lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
|
||||
if (dmpFifoLevel > 100) {
|
||||
// clear out fifo
|
||||
uint8_t crap[16];
|
||||
do {
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
|
||||
dmpFifoLevel = buf[0] << 8 | buf[1];
|
||||
} while (dmpFifoLevel);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void mpu6050DmpBankSelect(uint8_t bank)
|
||||
{
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, bank);
|
||||
}
|
||||
|
||||
static void mpu6050DmpBankInit(void)
|
||||
{
|
||||
uint8_t i, j;
|
||||
uint8_t incoming[9];
|
||||
|
||||
for (i = 0; i < 7; i++) {
|
||||
mpu6050DmpBankSelect(i);
|
||||
for (j = 0; j < 16; j++) {
|
||||
uint8_t start_addy = j * 0x10;
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
|
||||
}
|
||||
}
|
||||
|
||||
mpu6050DmpBankSelect(7);
|
||||
|
||||
for (j = 0; j < 8; j++) {
|
||||
|
||||
uint8_t start_addy = j * 0x10;
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
|
||||
}
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, 0x80);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 8, incoming);
|
||||
}
|
||||
|
||||
|
||||
static void mpu6050DmpMemInit(void)
|
||||
{
|
||||
uint8_t i;
|
||||
uint8_t temp;
|
||||
|
||||
mpu6050DmpBankInit();
|
||||
|
||||
// Bank, Start Address, Update Length, Update Data...
|
||||
for (i = 0; i < 22; i++) {
|
||||
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
|
||||
}
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x32);
|
||||
|
||||
for (i = 22; i < 29; i++) {
|
||||
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
|
||||
}
|
||||
|
||||
/*
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_1);
|
||||
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_2);
|
||||
*/
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x02); // ??
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x04);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); // full scale 2000 deg/s
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_1, 0x03);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_2, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_XG_OFFS_TC, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_YG_OFFS_TC, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZG_OFFS_TC, 0x00);
|
||||
|
||||
// clear offsets
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
|
||||
|
||||
mpu6050DmpBankSelect(0x01); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0xB2);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
|
||||
|
||||
mpu6050DmpBankSelect(0x01); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x90);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &temp);
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
|
||||
|
||||
// Insert FIFO count read?
|
||||
mpu6050DmpFifoReady();
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
|
||||
delay(2);
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 1, &temp);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1, &temp);
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
|
||||
delay(2);
|
||||
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
|
||||
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_THR, 0x02);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_THR, 0x9C);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_DUR, 0x50);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_DUR, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00);
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0xC8); // fifo enable
|
||||
|
||||
mpu6050DmpBankSelect(0x01); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x6A);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
|
||||
|
||||
mpu6050DmpBankSelect(0x01); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
|
||||
|
||||
mpu6050DmpBankSelect(0x00); // bank
|
||||
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
|
||||
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
|
||||
}
|
||||
|
||||
#else /* MPU6050_DMP */
|
||||
void mpu6050DmpLoop(void)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void mpu6050DmpResetFifo(void)
|
||||
{
|
||||
|
||||
}
|
||||
#endif /* MPU6050_DMP */
|
||||
|
|
|
|||
|
|
@ -177,3 +177,51 @@ void systemReset(bool toBootloader)
|
|||
// Generate system reset
|
||||
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
|
||||
}
|
||||
|
||||
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
|
||||
{
|
||||
switch (rotation) {
|
||||
case CW0_DEG:
|
||||
dest[X] = src[X];
|
||||
dest[Y] = src[Y];
|
||||
dest[Z] = src[Z];
|
||||
break;
|
||||
case CW90_DEG:
|
||||
dest[X] = src[Y];
|
||||
dest[Y] = -src[X];
|
||||
dest[Z] = src[Z];
|
||||
break;
|
||||
case CW180_DEG:
|
||||
dest[X] = -src[X];
|
||||
dest[Y] = -src[Y];
|
||||
dest[Z] = src[Z];
|
||||
break;
|
||||
case CW270_DEG:
|
||||
dest[X] = -src[Y];
|
||||
dest[Y] = src[X];
|
||||
dest[Z] = src[Z];
|
||||
break;
|
||||
case CW0_DEG_FLIP:
|
||||
dest[X] = -src[X];
|
||||
dest[Y] = src[Y];
|
||||
dest[Z] = -src[Z];
|
||||
break;
|
||||
case CW90_DEG_FLIP:
|
||||
dest[X] = src[Y];
|
||||
dest[Y] = src[X];
|
||||
dest[Z] = -src[Z];
|
||||
break;
|
||||
case CW180_DEG_FLIP:
|
||||
dest[X] = src[X];
|
||||
dest[Y] = -src[Y];
|
||||
dest[Z] = -src[Z];
|
||||
break;
|
||||
case CW270_DEG_FLIP:
|
||||
dest[X] = -src[Y];
|
||||
dest[Y] = -src[X];
|
||||
dest[Z] = -src[Z];
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,3 +15,6 @@ void systemReset(bool toBootloader);
|
|||
|
||||
// current crystal frequency - 8 or 12MHz
|
||||
extern uint32_t hse_value;
|
||||
|
||||
// sensor orientation
|
||||
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
|
||||
|
|
|
|||
48
src/imu.c
48
src/imu.c
|
|
@ -147,10 +147,10 @@ void rotateV(struct fp_vector *v, float *delta)
|
|||
float cosx, sinx, cosy, siny, cosz, sinz;
|
||||
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
|
||||
|
||||
cosx = cosf(-delta[PITCH]);
|
||||
sinx = sinf(-delta[PITCH]);
|
||||
cosy = cosf(delta[ROLL]);
|
||||
siny = sinf(delta[ROLL]);
|
||||
cosx = cosf(delta[ROLL]);
|
||||
sinx = sinf(delta[ROLL]);
|
||||
cosy = cosf(delta[PITCH]);
|
||||
siny = sinf(delta[PITCH]);
|
||||
cosz = cosf(delta[YAW]);
|
||||
sinz = sinf(delta[YAW]);
|
||||
|
||||
|
|
@ -161,13 +161,13 @@ void rotateV(struct fp_vector *v, float *delta)
|
|||
sinzsinx = sinx * sinz;
|
||||
|
||||
mat[0][0] = coszcosy;
|
||||
mat[0][1] = sinz * cosy;
|
||||
mat[0][2] = -siny;
|
||||
mat[1][0] = (coszsinx * siny) - sinzcosx;
|
||||
mat[1][1] = (sinzsinx * siny) + (coszcosx);
|
||||
mat[1][2] = cosy * sinx;
|
||||
mat[2][0] = (coszcosx * siny) + (sinzsinx);
|
||||
mat[2][1] = (sinzcosx * siny) - (coszsinx);
|
||||
mat[0][1] = -cosy * sinz;
|
||||
mat[0][2] = siny;
|
||||
mat[1][0] = sinzcosx + (coszsinx * siny);
|
||||
mat[1][1] = coszcosx - (sinzsinx * siny);
|
||||
mat[1][2] = -sinx * cosy;
|
||||
mat[2][0] = (sinzsinx) - (coszcosx * siny);
|
||||
mat[2][1] = (coszsinx) + (sinzcosx * siny);
|
||||
mat[2][2] = cosy * cosx;
|
||||
|
||||
v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
|
||||
|
|
@ -257,13 +257,6 @@ static void getEstimatedAttitude(void)
|
|||
uint32_t currentT = micros();
|
||||
uint32_t deltaT;
|
||||
float scale, deltaGyroAngle[3];
|
||||
#ifndef BASEFLIGHT_CALC
|
||||
float sqGZ;
|
||||
float sqGX;
|
||||
float sqGY;
|
||||
float sqGX_sqGZ;
|
||||
float invmagXZ;
|
||||
#endif
|
||||
deltaT = currentT - previousT;
|
||||
scale = deltaT * gyro.scale;
|
||||
previousT = currentT;
|
||||
|
|
@ -304,21 +297,8 @@ static void getEstimatedAttitude(void)
|
|||
}
|
||||
|
||||
// Attitude of the estimated vector
|
||||
#ifdef BASEFLIGHT_CALC
|
||||
// This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
|
||||
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
|
||||
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
|
||||
#else
|
||||
// MW2.2 version
|
||||
sqGZ = EstG.V.Z * EstG.V.Z;
|
||||
sqGX = EstG.V.X * EstG.V.X;
|
||||
sqGY = EstG.V.Y * EstG.V.Y;
|
||||
sqGX_sqGZ = sqGX + sqGZ;
|
||||
invmagXZ = 1.0f / sqrtf(sqGX_sqGZ);
|
||||
invG = 1.0f / sqrtf(sqGX_sqGZ + sqGY);
|
||||
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
|
||||
angle[PITCH] = _atan2f(EstG.V.Y, invmagXZ * sqGX_sqGZ);
|
||||
#endif
|
||||
angle[ROLL] = _atan2f(EstG.V.Y, EstG.V.Z);
|
||||
angle[PITCH] = _atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
|
||||
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
|
|
@ -339,7 +319,7 @@ static void getEstimatedAttitude(void)
|
|||
heading = hd;
|
||||
#else
|
||||
// MW 2.2 calculation
|
||||
heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * sqGX_sqGZ - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
|
||||
heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * (EstG.V.X * EstG.V.X + EstG.V.Z * EstG.V.Z) - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
|
||||
heading = heading + magneticDeclination;
|
||||
heading = heading / 10;
|
||||
#endif
|
||||
|
|
|
|||
4
src/mw.h
4
src/mw.h
|
|
@ -251,7 +251,9 @@ typedef struct master_t {
|
|||
uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz)
|
||||
|
||||
// global sensor-related stuff
|
||||
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
|
||||
sensor_align_e gyro_align; // gyro alignment
|
||||
sensor_align_e acc_align; // acc alignment
|
||||
sensor_align_e mag_align; // mag alignment
|
||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
||||
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
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.
|
||||
|
|
|
|||
|
|
@ -50,8 +50,11 @@ void sensorsAutodetect(void)
|
|||
// Accelerometer. Fuck it. Let user break shit.
|
||||
retry:
|
||||
switch (mcfg.acc_hardware) {
|
||||
case 0: // autodetect
|
||||
case 1: // ADXL345
|
||||
case ACC_NONE: // disable ACC
|
||||
sensorsClear(SENSOR_ACC);
|
||||
break;
|
||||
case ACC_DEFAULT: // autodetect
|
||||
case ACC_ADXL345: // ADXL345
|
||||
acc_params.useFifo = false;
|
||||
acc_params.dataRate = 800; // unused currently
|
||||
if (adxl345Detect(&acc_params, &acc))
|
||||
|
|
@ -59,7 +62,7 @@ retry:
|
|||
if (mcfg.acc_hardware == ACC_ADXL345)
|
||||
break;
|
||||
; // fallthrough
|
||||
case 2: // MPU6050
|
||||
case ACC_MPU6050: // MPU6050
|
||||
if (haveMpu6k) {
|
||||
mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct
|
||||
accHardware = ACC_MPU6050;
|
||||
|
|
@ -68,7 +71,7 @@ retry:
|
|||
}
|
||||
; // fallthrough
|
||||
#ifndef OLIMEXINO
|
||||
case 3: // MMA8452
|
||||
case ACC_MMA8452: // MMA8452
|
||||
if (mma8452Detect(&acc)) {
|
||||
accHardware = ACC_MMA8452;
|
||||
if (mcfg.acc_hardware == ACC_MMA8452)
|
||||
|
|
@ -102,12 +105,12 @@ retry:
|
|||
|
||||
// Now time to init things, acc first
|
||||
if (sensors(SENSOR_ACC))
|
||||
acc.init();
|
||||
acc.init(mcfg.acc_align);
|
||||
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
||||
gyro.init();
|
||||
gyro.init(mcfg.gyro_align);
|
||||
|
||||
#ifdef MAG
|
||||
if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
|
||||
if (!hmc5883lDetect(mcfg.mag_align))
|
||||
sensorsClear(SENSOR_MAG);
|
||||
#endif
|
||||
|
||||
|
|
@ -147,28 +150,6 @@ void batteryInit(void)
|
|||
batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
|
||||
}
|
||||
|
||||
// ALIGN_GYRO = 0,
|
||||
// ALIGN_ACCEL = 1,
|
||||
// ALIGN_MAG = 2
|
||||
static void alignSensors(uint8_t type, int16_t *data)
|
||||
{
|
||||
int i;
|
||||
int16_t tmp[3];
|
||||
|
||||
// make a copy :(
|
||||
tmp[0] = data[0];
|
||||
tmp[1] = data[1];
|
||||
tmp[2] = data[2];
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
int8_t axis = mcfg.align[type][i];
|
||||
if (axis > 0)
|
||||
data[axis - 1] = tmp[i];
|
||||
else
|
||||
data[-axis - 1] = -tmp[i];
|
||||
}
|
||||
}
|
||||
|
||||
static void ACC_Common(void)
|
||||
{
|
||||
static int32_t a[3];
|
||||
|
|
@ -254,12 +235,6 @@ static void ACC_Common(void)
|
|||
void ACC_getADC(void)
|
||||
{
|
||||
acc.read(accADC);
|
||||
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
|
||||
if (mcfg.align[ALIGN_ACCEL][0])
|
||||
alignSensors(ALIGN_ACCEL, accADC);
|
||||
else
|
||||
acc.align(accADC);
|
||||
|
||||
ACC_Common();
|
||||
}
|
||||
|
||||
|
|
@ -392,12 +367,6 @@ void Gyro_getADC(void)
|
|||
{
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
gyro.read(gyroADC);
|
||||
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
|
||||
if (mcfg.align[ALIGN_GYRO][0])
|
||||
alignSensors(ALIGN_GYRO, gyroADC);
|
||||
else
|
||||
gyro.align(gyroADC);
|
||||
|
||||
GYRO_Common();
|
||||
}
|
||||
|
||||
|
|
@ -405,12 +374,6 @@ void Gyro_getADC(void)
|
|||
static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
|
||||
static uint8_t magInit = 0;
|
||||
|
||||
static void Mag_getRawADC(void)
|
||||
{
|
||||
// MAG driver will align itself, so no need to alignSensors()
|
||||
hmc5883lRead(magADC);
|
||||
}
|
||||
|
||||
void Mag_init(void)
|
||||
{
|
||||
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
||||
|
|
@ -432,11 +395,11 @@ int Mag_getADC(void)
|
|||
t = currentTime + 100000;
|
||||
|
||||
// Read mag sensor
|
||||
Mag_getRawADC();
|
||||
hmc5883lRead(magADC);
|
||||
|
||||
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
|
||||
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
|
||||
magADC[YAW] = magADC[YAW] * magCal[YAW];
|
||||
magADC[X] = magADC[X] * magCal[X];
|
||||
magADC[Y] = magADC[Y] * magCal[Y];
|
||||
magADC[Z] = magADC[Z] * magCal[Z];
|
||||
|
||||
if (f.CALIBRATE_MAG) {
|
||||
tCal = t;
|
||||
|
|
@ -449,9 +412,9 @@ int Mag_getADC(void)
|
|||
}
|
||||
|
||||
if (magInit) { // we apply offset only once mag calibration is done
|
||||
magADC[ROLL] -= mcfg.magZero[ROLL];
|
||||
magADC[PITCH] -= mcfg.magZero[PITCH];
|
||||
magADC[YAW] -= mcfg.magZero[YAW];
|
||||
magADC[X] -= mcfg.magZero[X];
|
||||
magADC[Y] -= mcfg.magZero[Y];
|
||||
magADC[Z] -= mcfg.magZero[Z];
|
||||
}
|
||||
|
||||
if (tCal != 0) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue