/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/bmp085.c |
---|
0,0 → 1,224 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY bmp085.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für BMP085/BMP180 Luftdrucksensor, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdio.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "bmp085.h" |
//path to i2c fleury lib |
#include BMP085_I2CFLEURYPATH |
/* |
* i2c write |
*/ |
void bmp085_writemem(uint8_t reg, uint8_t value) { |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_write(value); |
i2c_stop(); |
} |
/* |
* i2c read |
*/ |
void bmp085_readmem(uint8_t reg, uint8_t buff[], uint8_t bytes) { |
uint8_t i =0; |
i2c_start_wait(BMP085_ADDR | I2C_WRITE); |
i2c_write(reg); |
i2c_rep_start(BMP085_ADDR | I2C_READ); |
for(i=0; i<bytes; i++) { |
if(i==bytes-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
#if BMP085_FILTERPRESSURE == 1 |
#define BMP085_AVARAGECOEF 21 |
static long k[BMP085_AVARAGECOEF]; |
long bmp085_avaragefilter(long input) { |
uint8_t i=0; |
long sum=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
k[i] = k[i+1]; |
} |
k[BMP085_AVARAGECOEF-1] = input; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
sum += k[i]; |
} |
return (sum /BMP085_AVARAGECOEF) ; |
} |
#endif |
/* |
* read calibration registers |
*/ |
void bmp085_getcalibration() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
bmp085_readmem(BMP085_REGAC1, buff, 2); |
bmp085_regac1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC2, buff, 2); |
bmp085_regac2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC3, buff, 2); |
bmp085_regac3 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGAC4, buff, 2); |
bmp085_regac4 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC5, buff, 2); |
bmp085_regac5 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGAC6, buff, 2); |
bmp085_regac6 = ((unsigned int)buff[0] <<8 | ((unsigned int)buff[1])); |
bmp085_readmem(BMP085_REGB1, buff, 2); |
bmp085_regb1 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGB2, buff, 2); |
bmp085_regb2 = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMB, buff, 2); |
bmp085_regmb = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMC, buff, 2); |
bmp085_regmc = ((int)buff[0] <<8 | ((int)buff[1])); |
bmp085_readmem(BMP085_REGMD, buff, 2); |
bmp085_regmd = ((int)buff[0] <<8 | ((int)buff[1])); |
} |
/* |
* get raw temperature as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawtemperature() { |
uint8_t buff[2]; |
memset(buff, 0, sizeof(buff)); |
long ut,x1,x2; |
//read raw temperature |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADTEMPERATURE); |
_delay_ms(5); // min. 4.5ms read Temp delay |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 2); |
ut = ((long)buff[0] << 8 | ((long)buff[1])); //uncompensated temperature value |
//calculate raw temperature |
x1 = ((long)ut - bmp085_regac6) * bmp085_regac5 >> 15; |
x2 = ((long)bmp085_regmc << 11) / (x1 + bmp085_regmd); |
bmp085_rawtemperature = x1 + x2; |
} |
/* |
* get raw pressure as read by registers, and do some calculation to convert it |
*/ |
void bmp085_getrawpressure() { |
uint8_t buff[3]; |
memset(buff, 0, sizeof(buff)); |
long up,x1,x2,x3,b3,b6,p; |
unsigned long b4,b7; |
#if BMP085_AUTOUPDATETEMP == 1 |
bmp085_getrawtemperature(); |
#endif |
//read raw pressure |
bmp085_writemem(BMP085_REGCONTROL, BMP085_REGREADPRESSURE+(BMP085_MODE << 6)); |
_delay_ms(2 + (3<<BMP085_MODE)); |
bmp085_readmem(BMP085_REGCONTROLOUTPUT, buff, 3); |
up = ((((long)buff[0] <<16) | ((long)buff[1] <<8) | ((long)buff[2])) >> (8-BMP085_MODE)); // uncompensated pressure value |
//calculate raw pressure |
b6 = bmp085_rawtemperature - 4000; |
x1 = (bmp085_regb2* (b6 * b6) >> 12) >> 11; |
x2 = (bmp085_regac2 * b6) >> 11; |
x3 = x1 + x2; |
b3 = (((((long)bmp085_regac1) * 4 + x3) << BMP085_MODE) + 2) >> 2; |
x1 = (bmp085_regac3 * b6) >> 13; |
x2 = (bmp085_regb1 * ((b6 * b6) >> 12)) >> 16; |
x3 = ((x1 + x2) + 2) >> 2; |
b4 = (bmp085_regac4 * (uint32_t)(x3 + 32768)) >> 15; |
b7 = ((uint32_t)up - b3) * (50000 >> BMP085_MODE); |
p = b7 < 0x80000000 ? (b7 << 1) / b4 : (b7 / b4) << 1; |
x1 = (p >> 8) * (p >> 8); |
x1 = (x1 * 3038) >> 16; |
x2 = (-7357 * p) >> 16; |
bmp085_rawpressure = p + ((x1 + x2 + 3791) >> 4); |
#if BMP085_FILTERPRESSURE == 1 |
bmp085_rawpressure = bmp085_avaragefilter(bmp085_rawpressure); |
#endif |
} |
/* |
* get celsius temperature |
*/ |
double bmp085_gettemperature() { |
bmp085_getrawtemperature(); |
double temperature = ((bmp085_rawtemperature + 8)>>4); |
temperature = temperature /10; |
return temperature; |
} |
/* |
* get pressure |
*/ |
int32_t bmp085_getpressure() { |
bmp085_getrawpressure(); |
return bmp085_rawpressure + BMP085_UNITPAOFFSET; |
} |
/* |
* get altitude |
*/ |
double bmp085_getaltitude() { |
bmp085_getrawpressure(); |
return ((1 - pow(bmp085_rawpressure/(double)101325, 0.1903 )) / 0.0000225577) + BMP085_UNITMOFFSET; |
} |
/* |
* init bmp085 |
*/ |
void bmp085_init() { |
#if BMP085_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
bmp085_getcalibration(); //get calibration data |
bmp085_getrawtemperature(); //update raw temperature, at least the first time |
#if BMP085_FILTERPRESSURE == 1 |
//initialize the avarage filter |
uint8_t i=0; |
for (i=0; i<BMP085_AVARAGECOEF; i++) { |
bmp085_getrawpressure(); |
} |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/bmp085.h |
---|
0,0 → 1,74 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- this library is a porting of the bmp085driver 0.4 ardunio library |
http://code.google.com/p/bmp085driver/ |
Notes: |
- to compile with avrgcc you may define -lm flag for some math functions |
*/ |
#ifndef BMP085_H_ |
#define BMP085_H_ |
#include <stdio.h> |
#include <avr/io.h> |
#define BMP085_ADDR (0x77<<1) //0x77 default I2C address |
#define BMP085_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define BMP085_I2CINIT 1 //init i2c |
//registers |
#define BMP085_REGAC1 0xAA |
#define BMP085_REGAC2 0xAC |
#define BMP085_REGAC3 0xAE |
#define BMP085_REGAC4 0xB0 |
#define BMP085_REGAC5 0xB2 |
#define BMP085_REGAC6 0xB4 |
#define BMP085_REGB1 0xB6 |
#define BMP085_REGB2 0xB8 |
#define BMP085_REGMB 0xBA |
#define BMP085_REGMC 0xBC |
#define BMP085_REGMD 0xBE |
#define BMP085_REGCONTROL 0xF4 //control |
#define BMP085_REGCONTROLOUTPUT 0xF6 //output 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB |
#define BMP085_REGREADTEMPERATURE 0x2E //read temperature |
#define BMP085_REGREADPRESSURE 0x34 //read pressure |
//modes |
#define BMP085_MODEULTRALOWPOWER 0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5 |
#define BMP085_MODESTANDARD 1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4 |
#define BMP085_MODEHIGHRES 2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3 |
#define BMP085_MODEULTRAHIGHRES 3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25 |
//autoupdate temperature enabled |
#define BMP085_AUTOUPDATETEMP 1 //autoupdate temperature every read |
//setup parameters |
#define BMP085_MODE BMP085_MODEULTRAHIGHRES //define a mode |
#define BMP085_UNITPAOFFSET 0 //define a unit offset (pa) |
#define BMP085_UNITMOFFSET 0 //define a unit offset (m) |
//avarage filter |
#define BMP085_FILTERPRESSURE 1 //avarage filter for pressure |
//variables |
int bmp085_regac1, bmp085_regac2, bmp085_regac3, bmp085_regb1, bmp085_regb2, bmp085_regmb, bmp085_regmc, bmp085_regmd; |
unsigned int bmp085_regac4, bmp085_regac5, bmp085_regac6; |
long bmp085_rawtemperature, bmp085_rawpressure; |
//functions |
extern void bmp085_init(); |
extern int32_t bmp085_getpressure(); |
extern double bmp085_getaltitude(); |
extern double bmp085_gettemperature(); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/hmc5883l.c |
---|
0,0 → 1,131 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY hmc5883l.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für HMC5883L Compass, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <avr/io.h> |
#include <util/delay.h> |
#include "hmc5883l.h" |
//path to i2c fleury lib |
#include HMC5883L_I2CFLEURYPATH |
static double hmc5883l_scale = 0; |
/* |
* init the hmc5883l |
*/ |
void hmc5883l_init() { |
#if HMC5883L_I2CINIT == 1 |
//init i2c |
i2c_init(); |
_delay_us(10); |
#endif |
//set scale |
hmc5883l_scale = 0; |
uint8_t regValue = 0x00; |
#if HMC5883L_SCALE == HMC5883L_SCALE088 |
regValue = 0x00; |
hmc5883l_scale = 0.73; |
#elif HMC5883L_SCALE == HMC5883L_SCALE13 |
regValue = 0x01; |
hmc5883l_scale = 0.92; |
#elif HMC5883L_SCALE == HMC5883L_SCALE19 |
regValue = 0x02; |
hmc5883l_scale = 1.22; |
#elif HMC5883L_SCALE == HMC5883L_SCALE25 |
regValue = 0x03; |
hmc5883l_scale = 1.52; |
#elif HMC5883L_SCALE == HMC5883L_SCALE40 |
regValue = 0x04; |
hmc5883l_scale = 2.27; |
#elif HMC5883L_SCALE == HMC5883L_SCALE47 |
regValue = 0x05; |
hmc5883l_scale = 2.56; |
#elif HMC5883L_SCALE == HMC5883L_SCALE56 |
regValue = 0x06; |
hmc5883l_scale = 3.03; |
#elif HMC5883L_SCALE == HMC5883L_SCALE81 |
regValue = 0x07; |
hmc5883l_scale = 4.35; |
#endif |
//setting is in the top 3 bits of the register. |
regValue = regValue << 5; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_CONFREGB); |
i2c_write(regValue); |
i2c_stop(); |
//set measurement mode |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_MODEREG); |
i2c_write(HMC5883L_MEASUREMODE); |
i2c_stop(); |
} |
/* |
* get raw data |
*/ |
void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) { |
uint8_t i = 0; |
uint8_t buff[6]; |
i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
i2c_write(HMC5883L_DATAREGBEGIN); |
i2c_stop(); |
i2c_start_wait(HMC5883L_ADDR | I2C_READ); |
for(i=0; i<6; i++) { |
if(i==6-1) |
buff[i] = i2c_readNak(); |
else |
buff[i] = i2c_readAck(); |
} |
i2c_stop(); |
*mxraw = (int16_t)((buff[0] << 8) | buff[1]); |
*mzraw = (int16_t)((buff[2] << 8) | buff[3]); |
*myraw = (int16_t)((buff[4] << 8) | buff[5]); |
} |
/* |
* get scaled data |
*/ |
void hmc5883l_getdata(double *mx, double *my, double *mz) { |
int16_t mxraw = 0; |
int16_t myraw = 0; |
int16_t mzraw = 0; |
hmc5883l_getrawdata(&mxraw, &myraw, &mzraw); |
#if HMC5883L_CALIBRATED == 1 |
float mxt = mxraw - HMC5883L_OFFSETX; |
float myt = myraw - HMC5883L_OFFSETY; |
float mzt = mzraw - HMC5883L_OFFSETZ; |
*mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt; |
*my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt; |
*mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt; |
#else |
*mx = mxraw * hmc5883l_scale; |
*my = myraw * hmc5883l_scale; |
*mz = mzraw * hmc5883l_scale; |
#endif |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/hmc5883l.h |
---|
0,0 → 1,71 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- HMC5883L Triple Axis Magnetometer Arduino Library |
http://bildr.org/2012/02/hmc5883l_arduino/ |
*/ |
#ifndef HMC5883L_H_ |
#define HMC5883L_H_ |
//definitions |
#define HMC5883L_ADDR (0x1E<<1) //device address |
//i2c settings |
#define HMC5883L_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define HMC5883L_I2CINIT 1 //init i2c |
//registers |
#define HMC5883L_CONFREGA 0x00 |
#define HMC5883L_CONFREGB 0x01 |
#define HMC5883L_MODEREG 0x02 |
#define HMC5883L_DATAREGBEGIN 0x03 |
//setup measurement mode |
#define HMC5883L_MEASURECONTINOUS 0x00 |
#define HMC5883L_MEASURESINGLESHOT 0x01 |
#define HMC5883L_MEASUREIDLE 0x03 |
#define HMC5883L_MEASUREMODE HMC5883L_MEASURECONTINOUS |
//setup scale |
#define HMC5883L_SCALE088 1 //0.88 |
#define HMC5883L_SCALE13 2 //1.3 |
#define HMC5883L_SCALE19 3 //1.9 |
#define HMC5883L_SCALE25 4 //2.5 |
#define HMC5883L_SCALE40 5 //4.0 |
#define HMC5883L_SCALE47 6 //4.7 |
#define HMC5883L_SCALE56 7 //5.6 |
#define HMC5883L_SCALE81 8 //8.1 |
#define HMC5883L_SCALE HMC5883L_SCALE13 |
#define HMC5883L_CALIBRATED 1 //enable this if this magn is calibrated |
//calibration values |
#if HMC5883L_CALIBRATED == 1 |
#define HMC5883L_OFFSETX -99.7 |
#define HMC5883L_OFFSETY -154.0 |
#define HMC5883L_OFFSETZ -22.7 |
#define HMC5883L_GAINX1 0.952017 |
#define HMC5883L_GAINX2 0.00195895 |
#define HMC5883L_GAINX3 0.0139661 |
#define HMC5883L_GAINY1 0.00195895 |
#define HMC5883L_GAINY2 0.882824 |
#define HMC5883L_GAINY3 0.00760243 |
#define HMC5883L_GAINZ1 0.0139661 |
#define HMC5883L_GAINZ2 0.00760243 |
#define HMC5883L_GAINZ3 0.995365 |
#endif |
//functions |
extern void hmc5883l_init(); |
extern void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw); |
extern void hmc5883l_getdata(double *mx, double *my, double *mz); |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050.c |
---|
0,0 → 1,694 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include <stdlib.h> |
#include <string.h> |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2 |
#include <math.h> //include libm |
#endif |
//path to i2c fleury lib |
#include "i2cmaster.h" |
volatile uint8_t buffer[14]; |
/* |
* read bytes from chip register |
*/ |
int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data) { |
uint8_t i = 0; |
int8_t count = 0; |
if(length > 0) { |
//request register |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); |
_delay_us(10); |
//read data |
i2c_start(MPU6050_ADDR | I2C_READ); |
for(i=0; i<length; i++) { |
count++; |
if(i==length-1) |
data[i] = i2c_readNak(); |
else |
data[i] = i2c_readAck(); |
} |
i2c_stop(); |
} |
return count; |
} |
/* |
* read 1 byte from chip register |
*/ |
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) { |
return mpu6050_readBytes(regAddr, 1, data); |
} |
/* |
* write bytes to chip register |
*/ |
void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data) { |
if(length > 0) { |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (uint8_t i = 0; i < length; i++) { |
i2c_write((uint8_t) data[i]); |
} |
i2c_stop(); |
} |
} |
/* |
* write 1 byte to chip register |
*/ |
void mpu6050_writeByte(uint8_t regAddr, uint8_t data) { |
return mpu6050_writeBytes(regAddr, 1, &data); |
} |
/* |
* read bits from chip register |
*/ |
int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) { |
// 01101001 read byte |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 010 masked |
// -> 010 shifted |
int8_t count = 0; |
if(length > 0) { |
uint8_t b; |
if ((count = mpu6050_readByte(regAddr, &b)) != 0) { |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
b &= mask; |
b >>= (bitStart - length + 1); |
*data = b; |
} |
} |
return count; |
} |
/* |
* read 1 bit from chip register |
*/ |
int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data) { |
uint8_t b; |
uint8_t count = mpu6050_readByte(regAddr, &b); |
*data = b & (1 << bitNum); |
return count; |
} |
/* |
* write bit/bits to chip register |
*/ |
void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { |
// 010 value to write |
// 76543210 bit numbers |
// xxx args: bitStart=4, length=3 |
// 00011100 mask byte |
// 10101111 original value (sample) |
// 10100011 original & ~mask |
// 10101011 masked | value |
if(length > 0) { |
uint8_t b = 0; |
if (mpu6050_readByte(regAddr, &b) != 0) { //get current data |
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); |
data <<= (bitStart - length + 1); // shift data into correct position |
data &= mask; // zero all non-important bits in data |
b &= ~(mask); // zero all important bits in existing byte |
b |= data; // combine data with existing byte |
mpu6050_writeByte(regAddr, b); |
} |
} |
} |
/* |
* write one bit to chip register |
*/ |
void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data) { |
uint8_t b; |
mpu6050_readByte(regAddr, &b); |
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); |
mpu6050_writeByte(regAddr, b); |
} |
#if MPU6050_GETATTITUDE == 2 |
/* |
* write word/words to chip register |
*/ |
void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data) { |
if(length > 0) { |
uint8_t i = 0; |
//write data |
i2c_start(MPU6050_ADDR | I2C_WRITE); |
i2c_write(regAddr); //reg |
for (i = 0; i < length * 2; i++) { |
i2c_write((uint8_t)(data[i++] >> 8)); // send MSB |
i2c_write((uint8_t)data[i]); // send LSB |
} |
i2c_stop(); |
} |
} |
/* |
* set a chip memory bank |
*/ |
void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank) { |
bank &= 0x1F; |
if (userBank) bank |= 0x20; |
if (prefetchEnabled) bank |= 0x40; |
mpu6050_writeByte(MPU6050_RA_BANK_SEL, bank); |
} |
/* |
* set memory start address |
*/ |
void mpu6050_setMemoryStartAddress(uint8_t address) { |
mpu6050_writeByte(MPU6050_RA_MEM_START_ADDR, address); |
} |
/* |
* read a memory block |
*/ |
void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
for (uint16_t i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
// read the chunk of data as specified |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i); |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
} |
/* |
* write a memory block |
*/ |
uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
uint8_t chunkSize; |
uint8_t *verifyBuffer = 0; |
uint8_t *progBuffer = 0; |
uint16_t i; |
uint8_t j; |
if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); |
for (i = 0; i < dataSize;) { |
// determine correct chunk size according to bank position and data size |
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE; |
// make sure we don't go past the data size |
if (i + chunkSize > dataSize) chunkSize = dataSize - i; |
// make sure this chunk doesn't go past the bank boundary (256 bytes) |
if (chunkSize > 256 - address) chunkSize = 256 - address; |
if (useProgMem) { |
// write the chunk of data as specified |
for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
// write the chunk of data as specified |
progBuffer = (uint8_t *)data + i; |
} |
mpu6050_writeBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer); |
// verify data if needed |
if (verify && verifyBuffer) { |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
mpu6050_readBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); |
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { |
free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 0; // uh oh. |
} |
} |
// increase byte index by [chunkSize] |
i += chunkSize; |
// uint8_t automatically wraps to 0 at 256 |
address += chunkSize; |
// if we aren't done, update bank (if necessary) and address |
if (i < dataSize) { |
if (address == 0) bank++; |
mpu6050_setMemoryBank(bank, 0, 0); |
mpu6050_setMemoryStartAddress(address); |
} |
} |
if (verify) free(verifyBuffer); |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* write a dmp configuration set |
*/ |
uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem) { |
uint8_t *progBuffer = 0; |
uint8_t success, special; |
uint16_t i, j; |
if (useProgMem) { |
progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary |
} |
// config set data is a long string of blocks with the following structure: |
// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] |
uint8_t bank, offset, length; |
for (i = 0; i < dataSize;) { |
if (useProgMem) { |
bank = pgm_read_byte(data + i++); |
offset = pgm_read_byte(data + i++); |
length = pgm_read_byte(data + i++); |
} else { |
bank = data[i++]; |
offset = data[i++]; |
length = data[i++]; |
} |
// write data or perform special action |
if (length > 0) { |
// regular block of data to write |
if (useProgMem) { |
if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length); |
for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j); |
} else { |
progBuffer = (uint8_t *)data + i; |
} |
success = mpu6050_writeMemoryBlock(progBuffer, length, bank, offset, 1, 0); |
i += length; |
} else { |
// special instruction |
// NOTE: this kind of behavior (what and when to do certain things) |
// is totally undocumented. This code is in here based on observed |
// behavior only, and exactly why (or even whether) it has to be here |
// is anybody's guess for now. |
if (useProgMem) { |
special = pgm_read_byte(data + i++); |
} else { |
special = data[i++]; |
} |
if (special == 0x01) { |
// enable DMP-related interrupts |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, 1); //setIntZeroMotionEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, 1); //setIntFIFOBufferOverflowEnabled |
//mpu6050_writeBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, 1); //setIntDMPEnabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x32); // single operation |
success = 1; |
} else { |
// unknown special command |
success = 0; |
} |
} |
if (!success) { |
if (useProgMem) free(progBuffer); |
return 0; // uh oh |
} |
} |
if (useProgMem) free(progBuffer); |
return 1; |
} |
/* |
* get the fifo count |
*/ |
uint16_t mpu6050_getFIFOCount(void) { |
mpu6050_readBytes(MPU6050_RA_FIFO_COUNTH, 2, (uint8_t *)buffer); |
return (((uint16_t)buffer[0]) << 8) | buffer[1]; |
} |
/* |
* read fifo bytes |
*/ |
void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length) { |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, length, data); |
} |
/* |
* get the interrupt status |
*/ |
uint8_t mpu6050_getIntStatus(void) { |
mpu6050_readByte(MPU6050_RA_INT_STATUS, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* reset fifo |
*/ |
void mpu6050_resetFIFO(void) { |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} |
/* |
* get gyro offset X |
*/ |
int8_t mpu6050_getXGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset X |
*/ |
void mpu6050_setXGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Y |
*/ |
int8_t mpu6050_getYGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Y |
*/ |
void mpu6050_setYGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
/* |
* get gyro offset Z |
*/ |
int8_t mpu6050_getZGyroOffset(void) { |
mpu6050_readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, (uint8_t *)buffer); |
return buffer[0]; |
} |
/* |
* set gyro offset Z |
*/ |
void mpu6050_setZGyroOffset(int8_t offset) { |
mpu6050_writeBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); |
} |
#endif |
/* |
* set sleep disabled |
*/ |
void mpu6050_setSleepDisabled() { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0); |
} |
/* |
* set sleep enabled |
*/ |
void mpu6050_setSleepEnabled(void) { |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1); |
} |
/* |
* test connectino to chip |
*/ |
uint8_t mpu6050_testConnection(void) { |
mpu6050_readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, (uint8_t *)buffer); |
if(buffer[0] == 0x34) |
return 1; |
else |
return 0; |
} |
/* |
* initialize the accel and gyro |
*/ |
void mpu6050_init(void) { |
//allow mpu6050 chip clocks to start up |
_delay_ms(100); |
//set sleep disabled |
mpu6050_setSleepDisabled(); |
//wake up delay needed sleep disabled |
_delay_ms(10); |
//set clock source |
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) |
// as the clock reference for improved stability |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set sampe rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); //1khz / (1 + 4) = 200Hz |
//set gyro range |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS); |
//set accel range |
mpu6050_writeBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS); |
#if MPU6050_GETATTITUDE == 1 |
#error "Do not enable timer 0 it is in use elsewhere!" |
//MPU6050_TIMER0INIT |
#endif |
} |
//can not accept many request if we alreay have getattitude requests |
/* |
* get raw data |
*/ |
void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
*ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
*ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
*az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
*gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
*gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
*gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
} |
/* |
* get raw data converted to g and deg/sec values |
*/ |
void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz); |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
*axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
*ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
*azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
*gxds = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN; |
*gyds = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN; |
*gzds = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN; |
#else |
*axg = (double)(ax)/MPU6050_AGAIN; |
*ayg = (double)(ay)/MPU6050_AGAIN; |
*azg = (double)(az)/MPU6050_AGAIN; |
*gxds = (double)(gx)/MPU6050_GGAIN; |
*gyds = (double)(gy)/MPU6050_GGAIN; |
*gzds = (double)(gz)/MPU6050_GGAIN; |
#endif |
} |
#if MPU6050_GETATTITUDE == 1 |
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; |
volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; |
/* |
* Mahony update function (for 6DOF) |
*/ |
void mpu6050_mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) { |
float norm; |
float halfvx, halfvy, halfvz; |
float halfex, halfey, halfez; |
float qa, qb, qc; |
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) |
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { |
// Normalise accelerometer measurement |
norm = sqrt(ax * ax + ay * ay + az * az); |
ax /= norm; |
ay /= norm; |
az /= norm; |
// Estimated direction of gravity and vector perpendicular to magnetic flux |
halfvx = q1 * q3 - q0 * q2; |
halfvy = q0 * q1 + q2 * q3; |
halfvz = q0 * q0 - 0.5f + q3 * q3; |
// Error is sum of cross product between estimated and measured direction of gravity |
halfex = (ay * halfvz - az * halfvy); |
halfey = (az * halfvx - ax * halfvz); |
halfez = (ax * halfvy - ay * halfvx); |
// Compute and apply integral feedback if enabled |
if(mpu6050_mahonytwoKiDef > 0.0f) { |
integralFBx += mpu6050_mahonytwoKiDef * halfex * (1.0f / mpu6050_mahonysampleFreq); // integral error scaled by Ki |
integralFBy += mpu6050_mahonytwoKiDef * halfey * (1.0f / mpu6050_mahonysampleFreq); |
integralFBz += mpu6050_mahonytwoKiDef * halfez * (1.0f / mpu6050_mahonysampleFreq); |
gx += integralFBx; // apply integral feedback |
gy += integralFBy; |
gz += integralFBz; |
} else { |
integralFBx = 0.0f; // prevent integral windup |
integralFBy = 0.0f; |
integralFBz = 0.0f; |
} |
// Apply proportional feedback |
gx += mpu6050_mahonytwoKpDef * halfex; |
gy += mpu6050_mahonytwoKpDef * halfey; |
gz += mpu6050_mahonytwoKpDef * halfez; |
} |
// Integrate rate of change of quaternion |
gx *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); // pre-multiply common factors |
gy *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
gz *= (0.5f * (1.0f / mpu6050_mahonysampleFreq)); |
qa = q0; |
qb = q1; |
qc = q2; |
q0 += (-qb * gx - qc * gy - q3 * gz); |
q1 += (qa * gx + qc * gz - q3 * gy); |
q2 += (qa * gy - qb * gz + q3 * gx); |
q3 += (qa * gz + qb * gy - qc * gx); |
// Normalise quaternion |
norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); |
q0 /= norm; |
q1 /= norm; |
q2 /= norm; |
q3 /= norm; |
} |
/* |
* update quaternion |
*/ |
void mpu6050_updateQuaternion(void) { |
int16_t ax = 0; |
int16_t ay = 0; |
int16_t az = 0; |
int16_t gx = 0; |
int16_t gy = 0; |
int16_t gz = 0; |
double axg = 0; |
double ayg = 0; |
double azg = 0; |
double gxrs = 0; |
double gyrs = 0; |
double gzrs = 0; |
//get raw data |
while(1) { |
mpu6050_readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, (uint8_t *)buffer); |
if(buffer[0]) |
break; |
_delay_us(10); |
} |
mpu6050_readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, (uint8_t *)buffer); |
ax = (((int16_t)buffer[0]) << 8) | buffer[1]; |
ay = (((int16_t)buffer[2]) << 8) | buffer[3]; |
az = (((int16_t)buffer[4]) << 8) | buffer[5]; |
gx = (((int16_t)buffer[8]) << 8) | buffer[9]; |
gy = (((int16_t)buffer[10]) << 8) | buffer[11]; |
gz = (((int16_t)buffer[12]) << 8) | buffer[13]; |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN; |
ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN; |
azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN; |
gxrs = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN*0.01745329; //degree to radians |
#else |
axg = (double)(ax)/MPU6050_AGAIN; |
ayg = (double)(ay)/MPU6050_AGAIN; |
azg = (double)(az)/MPU6050_AGAIN; |
gxrs = (double)(gx)/MPU6050_GGAIN*0.01745329; //degree to radians |
gyrs = (double)(gy)/MPU6050_GGAIN*0.01745329; //degree to radians |
gzrs = (double)(gz)/MPU6050_GGAIN*0.01745329; //degree to radians |
#endif |
//compute data |
mpu6050_mahonyUpdate(gxrs, gyrs, gzrs, axg, ayg, azg); |
} |
/* |
* update timer for attitude |
*/ |
ISR(TIMER0_OVF_vect) { |
mpu6050_updateQuaternion(); |
} |
/* |
* get quaternion |
*/ |
void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz) { |
*qw = q0; |
*qx = q1; |
*qy = q2; |
*qz = q3; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1); |
*pitch = -asin(2*q1*q3 + 2*q0*q2); |
*roll = atan2(2*q2*q3 - 2*q0*q1, 2*q0*q0 + 2*q3*q3 - 1); |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050.h |
---|
0,0 → 1,174 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
References: |
- most of the code is a port of the arduino mpu6050 library by Jeff Rowberg |
https://github.com/jrowberg/i2cdevlib |
- Mahony complementary filter for attitude estimation |
http://www.x-io.co.uk |
*/ |
#ifndef MPU6050_H_ |
#define MPU6050_H_ |
#include <avr/io.h> |
#include "mpu6050registers.h" |
#ifdef __cplusplus |
extern "C" { |
#endif |
//i2c settings |
#define MPU6050_I2CFLEURYPATH "i2cmaster.h" //define the path to i2c fleury lib |
#define MPU6050_I2CINIT 0 //init i2c |
//definitions |
#define MPU6050_ADDR (0x68 <<1) //device address - 0x68 pin low (GND), 0x69 pin high (VCC) |
//enable the getattitude functions |
//because we do not have a magnetometer, we have to start the chip always in the same position |
//then to obtain your object attitude you have to apply the aerospace sequence |
//0 disabled |
//1 mahony filter |
//2 dmp chip processor |
#define MPU6050_GETATTITUDE 2 |
//definitions for raw data |
//gyro and acc scale |
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 |
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 |
#define MPU6050_GYRO_LSB_250 131.0 |
#define MPU6050_GYRO_LSB_500 65.5 |
#define MPU6050_GYRO_LSB_1000 32.8 |
#define MPU6050_GYRO_LSB_2000 16.4 |
#if MPU6050_GYRO_FS == MPU6050_GYRO_FS_250 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_250 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_500 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_500 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_1000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_1000 |
#elif MPU6050_GYRO_FS == MPU6050_GYRO_FS_2000 |
#define MPU6050_GGAIN MPU6050_GYRO_LSB_2000 |
#endif |
#define MPU6050_ACCEL_LSB_2 16384.0 |
#define MPU6050_ACCEL_LSB_4 8192.0 |
#define MPU6050_ACCEL_LSB_8 4096.0 |
#define MPU6050_ACCEL_LSB_16 2048.0 |
#if MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_2 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_2 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_4 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_4 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_8 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_8 |
#elif MPU6050_ACCEL_FS == MPU6050_ACCEL_FS_16 |
#define MPU6050_AGAIN MPU6050_ACCEL_LSB_16 |
#endif |
#define MPU6050_CALIBRATEDACCGYRO 1 //set to 1 if is calibrated |
#if MPU6050_CALIBRATEDACCGYRO == 1 |
#define MPU6050_AXOFFSET 0 |
#define MPU6050_AYOFFSET 0 |
#define MPU6050_AZOFFSET 0 |
#define MPU6050_AXGAIN 16384.0 |
#define MPU6050_AYGAIN 16384.0 |
#define MPU6050_AZGAIN 16384.0 |
#define MPU6050_GXOFFSET -42 |
#define MPU6050_GYOFFSET 9 |
#define MPU6050_GZOFFSET -29 |
#define MPU6050_GXGAIN 16.4 |
#define MPU6050_GYGAIN 16.4 |
#define MPU6050_GZGAIN 16.4 |
#endif |
//definitions for attitude 1 function estimation |
#if MPU6050_GETATTITUDE == 1 |
#error "GETATTITUDE == 1 is not supported!" |
//setup timer0 overflow event and define madgwickAHRSsampleFreq equal to timer0 frequency |
//timerfreq = (FCPU / prescaler) / timerscale |
// timerscale 8-bit = 256 |
// es. 61 = (16000000 / 1024) / 256 |
#define MPU6050_TIMER0INIT TCCR0B |=(1<<CS02)|(1<<CS00); TIMSK0 |=(1<<TOIE0); |
#define mpu6050_mahonysampleFreq 61.0f // sample frequency in Hz |
#define mpu6050_mahonytwoKpDef (2.0f * 0.5f) // 2 * proportional gain |
#define mpu6050_mahonytwoKiDef (2.0f * 0.1f) // 2 * integral gain |
#endif |
#if MPU6050_GETATTITUDE == 2 |
//dmp definitions |
//packet size |
#define MPU6050_DMP_dmpPacketSize 42 |
//define INT0 rise edge interrupt |
#define MPU6050_DMP_INT0SETUP EICRA |= (1<<ISC01) | (1<<ISC00) |
//define enable and disable INT0 rise edge interrupt |
#define MPU6050_DMP_INT0DISABLE EIMSK &= ~(1<<INT0) |
#define MPU6050_DMP_INT0ENABLE EIMSK |= (1<<INT0) |
extern volatile uint8_t mpu6050_mpuInterrupt; |
#endif |
//functions |
extern void mpu6050_init(void); |
extern uint8_t mpu6050_testConnection(void); |
#if MPU6050_GETATTITUDE == 0 |
extern void mpu6050_getRawData(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); |
extern void mpu6050_getConvData(double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds); |
#endif |
extern void mpu6050_setSleepDisabled(void); |
extern void mpu6050_setSleepEnabled(void); |
extern int8_t mpu6050_readBytes(uint8_t regAddr, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data); |
extern void mpu6050_writeBytes(uint8_t regAddr, uint8_t length, uint8_t* data); |
extern void mpu6050_writeByte(uint8_t regAddr, uint8_t data); |
extern int8_t mpu6050_readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data); |
extern int8_t mpu6050_readBit(uint8_t regAddr, uint8_t bitNum, uint8_t *data); |
extern void mpu6050_writeBits(uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); |
extern void mpu6050_writeBit(uint8_t regAddr, uint8_t bitNum, uint8_t data); |
#if MPU6050_GETATTITUDE == 1 |
extern void mpu6050_updateQuaternion(void); |
extern void mpu6050_getQuaternion(double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double *pitch, double *roll, double *yaw); |
#endif |
#if MPU6050_GETATTITUDE == 2 |
extern void mpu6050_writeWords(uint8_t regAddr, uint8_t length, uint16_t* data); |
extern void mpu6050_setMemoryBank(uint8_t bank, uint8_t prefetchEnabled, uint8_t userBank); |
extern void mpu6050_setMemoryStartAddress(uint8_t address); |
extern void mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address); |
extern uint8_t mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, uint8_t verify, uint8_t useProgMem); |
extern uint8_t mpu6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, uint8_t useProgMem); |
extern uint16_t mpu6050_getFIFOCount(void); |
extern void mpu6050_getFIFOBytes(uint8_t *data, uint8_t length); |
extern uint8_t mpu6050_getIntStatus(void); |
extern void mpu6050_resetFIFO(); |
extern int8_t mpu6050_getXGyroOffset(void); |
extern void mpu6050_setXGyroOffset(int8_t offset); |
extern int8_t mpu6050_getYGyroOffset(void); |
extern void mpu6050_setYGyroOffset(int8_t offset); |
extern int8_t mpu6050_getZGyroOffset(void); |
extern void mpu6050_setZGyroOffset(int8_t offset); |
//base dmp |
extern uint8_t mpu6050_dmpInitialize(void); |
extern void mpu6050_dmpEnable(void); |
extern void mpu6050_dmpDisable(void); |
extern void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz); |
extern void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw); |
extern uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz); |
#endif |
#ifdef __cplusplus |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050dmp6.c |
---|
0,0 → 1,495 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
//############################################################################ |
//# HISTORY mpu6050dmp6.c |
//# |
//# 19.09.2015 cebra |
//# - add: Library für MPU6050 Gyro, ACC, DMP-Routinen für IMU, GY-87 Sensorboard |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
/* |
* This file contains all the functions needed to process 6-axis orientation by the internal chip processor |
*/ |
//to enable get roll, pitch and yaw function we include the math function |
//if some error appears we must add -lm and -lc to C linker, the linker line should become somethink like this |
//${COMMAND} -lm ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS} -lc |
#include "mpu6050.h" |
#if MPU6050_GETATTITUDE == 2 |
#include <avr/io.h> |
#include <avr/pgmspace.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include <math.h> //include libm |
#define MPU6050_DMP_CODE_SIZE 1929 |
#define MPU6050_DMP_CONFIG_SIZE 192 |
#define MPU6050_DMP_UPDATES_SIZE 47 |
volatile uint8_t mpu6050_mpuInterrupt = 0; |
uint8_t *dmpPacketBuffer; |
uint16_t mpu6050_fifoCount = 0; |
uint8_t mpu6050_mpuIntStatus = 0; |
uint8_t mpu6050_fifoBuffer[64]; |
/* ================================================================================================ * |
| Default MotionApps v2.0 42-byte FIFO packet structure: | |
| | |
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| | |
| [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | |
* ================================================================================================ */ |
// this block of memory gets written to the MPU on start-up, and it seems |
// to be volatile memory, so it has to be done each time (it only takes ~1 |
// second though) |
const unsigned char mpu6050_dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { |
// bank 0, 256 bytes |
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, |
// bank 1, 256 bytes |
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, |
// bank 2, 256 bytes |
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, |
// bank 3, 256 bytes |
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, |
// bank 4, 256 bytes |
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, |
// bank 5, 256 bytes |
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, |
// bank 6, 256 bytes |
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, |
// bank 7, 138 bytes (remainder) |
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 |
}; |
const unsigned char mpu6050_dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { |
// BANK OFFSET LENGTH [DATA] |
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 |
// SPECIAL 0x01 = enable interrupts |
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION |
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, 0x09 // D_0_22 inv_set_fifo_rate |
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
// It is important to make sure the host processor can keep up with reading and processing |
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
}; |
const unsigned char mpu6050_dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { |
0x01, 0xB2, 0x02, 0xFF, 0xFF, |
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, |
0x01, 0x6A, 0x02, 0x06, 0x00, |
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
0x01, 0x62, 0x02, 0x00, 0x00, |
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
}; |
/* |
* initialize mpu6050 dmp |
*/ |
uint8_t mpu6050_dmpInitialize() { |
//setup interrupt |
MPU6050_DMP_INT0SETUP; |
//reset |
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); |
_delay_ms(30);//wait after reset |
//disable sleep mode |
mpu6050_setSleepDisabled(); |
//set memorybank to 0 |
mpu6050_setMemoryBank(0, 0, 0); |
//get X/Y/Z gyro offsets |
int8_t xgOffset = mpu6050_getXGyroOffset(); |
int8_t ygOffset = mpu6050_getYGyroOffset(); |
int8_t zgOffset = mpu6050_getZGyroOffset(); |
//setting slave 0 address to 0x7F |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); |
//disabling I2C Master mode |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); |
//setting slave 0 address to 0x68 (self) |
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); |
//resetting I2C Master control |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); |
_delay_ms(20); |
//load DMP code into memory banks |
if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { |
if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { |
//set clock source |
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); |
//set DMP and FIFO_OFLOW interrupts enabled |
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); |
//set sample rate |
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz |
//set external frame sync to TEMP_OUT_L[0] |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); |
//set DLPF bandwidth to 42Hz |
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); |
//set gyro sensitivity to +/- 2000 deg/sec |
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); |
//set DMP configuration bytes (function unknown) |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); |
mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); |
//clear OTP Bank flag |
mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); |
//set X/Y/Z gyro offsets to previous values |
//xgOffset = 0; |
//ygOffset = 0; |
zgOffset = 90; |
mpu6050_setXGyroOffset(xgOffset); |
mpu6050_setYGyroOffset(ygOffset); |
mpu6050_setZGyroOffset(zgOffset); |
//set X/Y/Z gyro user offsets to zero |
mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); |
mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 0); |
//writing final memory update 1/7 (function unknown) |
uint8_t dmpUpdate[16], j; |
uint16_t pos = 0; |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 2/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reset FIFO |
mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); |
//reading FIFO count |
uint8_t fifoCount = mpu6050_getFIFOCount(); |
uint8_t fifoBuffer[128]; |
//current FIFO count |
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); |
//setting motion detection threshold to 2 |
mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); |
//setting zero-motion detection threshold to 156 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); |
//setting motion detection duration to 80 |
mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); |
//setting zero-motion detection duration to 0 |
mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); |
//reset FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
//enabling FIFO |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); |
//enabling DMP |
mpu6050_dmpEnable(); |
//resetting DMP |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//writing final memory update 3/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 4/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//writing final memory update 5/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//reading FIFO data...")); |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//reading final memory update 6/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
//waiting for FIFO count > 2 |
while ((fifoCount = mpu6050_getFIFOCount()) < 3); |
//reading FIFO data |
mpu6050_getFIFOBytes(fifoBuffer, fifoCount); |
//writing final memory update 7/7 (function unknown) |
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); |
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); |
//disabling DMP (you turn it on later) |
mpu6050_dmpDisable(); |
//resetting FIFO and clearing INT status one last time |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); |
} else { |
return 2; // configuration block loading failed |
} |
} else { |
return 1; // main binary block loading failed |
} |
return 0; // success |
} |
/* |
* enable dmp |
*/ |
void mpu6050_dmpEnable() { |
MPU6050_DMP_INT0ENABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1); |
} |
/* |
* disable dmp |
*/ |
void mpu6050_dmpDisable() { |
MPU6050_DMP_INT0DISABLE; |
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 0); |
} |
/* |
* get quaternion from packet |
*/ |
void mpu6050_getQuaternion(const uint8_t* packet, double *qw, double *qx, double *qy, double *qz) { |
if (packet == 0) packet = dmpPacketBuffer; |
*qw = (double)((packet[0] << 8) + packet[1]) / 16384.0f; |
*qx = (double)((packet[4] << 8) + packet[5]) / 16384.0f; |
*qy = (double)((packet[8] << 8) + packet[9]) / 16384.0f; |
*qz = (double)((packet[12] << 8) + packet[13]) / 16384.0f; |
} |
/* |
* get euler angles |
* aerospace sequence, to obtain sensor attitude: |
* 1. rotate around sensor Z plane by yaw |
* 2. rotate around sensor Y plane by pitch |
* 3. rotate around sensor X plane by roll |
*/ |
void mpu6050_getRollPitchYaw(double qw, double qx, double qy, double qz, double *roll, double *pitch, double *yaw) { |
*yaw = atan2(2*qx*qy - 2*qw*qz, 2*qw*qw + 2*qx*qx - 1); |
*pitch = -asin(2*qx*qz + 2*qw*qy); |
*roll = atan2(2*qy*qz - 2*qw*qx, 2*qw*qw + 2*qz*qz - 1); |
} |
/* |
* get quaternion and wait |
*/ |
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) { |
while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize); |
//reset interrupt |
mpu6050_mpuInterrupt = 0; |
//check for overflow |
mpu6050_mpuIntStatus = mpu6050_getIntStatus(); |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) { |
//reset |
mpu6050_resetFIFO(); |
} else if (mpu6050_mpuIntStatus & 0x02) { |
//wait for correct available data length, should be a VERY short wait |
while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize) |
mpu6050_fifoCount = mpu6050_getFIFOCount(); |
//read a packet from FIFO |
mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize); |
mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize; |
//get quaternion |
mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz); |
return 1; |
} |
return 0; |
} |
/* |
* on interrupt set data availabe |
*/ |
ISR (INT0_vect) { |
mpu6050_mpuInterrupt = 1; |
} |
#endif |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/mpu6050registers.h |
---|
0,0 → 1,365 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#ifndef MPU6050REGISTERS_H_ |
#define MPU6050REGISTERS_H_ |
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD |
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN |
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN |
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN |
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS |
#define MPU6050_RA_XA_OFFS_L_TC 0x07 |
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS |
#define MPU6050_RA_YA_OFFS_L_TC 0x09 |
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS |
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B |
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR |
#define MPU6050_RA_XG_OFFS_USRL 0x14 |
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR |
#define MPU6050_RA_YG_OFFS_USRL 0x16 |
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR |
#define MPU6050_RA_ZG_OFFS_USRL 0x18 |
#define MPU6050_RA_SMPLRT_DIV 0x19 |
#define MPU6050_RA_CONFIG 0x1A |
#define MPU6050_RA_GYRO_CONFIG 0x1B |
#define MPU6050_RA_ACCEL_CONFIG 0x1C |
#define MPU6050_RA_FF_THR 0x1D |
#define MPU6050_RA_FF_DUR 0x1E |
#define MPU6050_RA_MOT_THR 0x1F |
#define MPU6050_RA_MOT_DUR 0x20 |
#define MPU6050_RA_ZRMOT_THR 0x21 |
#define MPU6050_RA_ZRMOT_DUR 0x22 |
#define MPU6050_RA_FIFO_EN 0x23 |
#define MPU6050_RA_I2C_MST_CTRL 0x24 |
#define MPU6050_RA_I2C_SLV0_ADDR 0x25 |
#define MPU6050_RA_I2C_SLV0_REG 0x26 |
#define MPU6050_RA_I2C_SLV0_CTRL 0x27 |
#define MPU6050_RA_I2C_SLV1_ADDR 0x28 |
#define MPU6050_RA_I2C_SLV1_REG 0x29 |
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A |
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B |
#define MPU6050_RA_I2C_SLV2_REG 0x2C |
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D |
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E |
#define MPU6050_RA_I2C_SLV3_REG 0x2F |
#define MPU6050_RA_I2C_SLV3_CTRL 0x30 |
#define MPU6050_RA_I2C_SLV4_ADDR 0x31 |
#define MPU6050_RA_I2C_SLV4_REG 0x32 |
#define MPU6050_RA_I2C_SLV4_DO 0x33 |
#define MPU6050_RA_I2C_SLV4_CTRL 0x34 |
#define MPU6050_RA_I2C_SLV4_DI 0x35 |
#define MPU6050_RA_I2C_MST_STATUS 0x36 |
#define MPU6050_RA_INT_PIN_CFG 0x37 |
#define MPU6050_RA_INT_ENABLE 0x38 |
#define MPU6050_RA_DMP_INT_STATUS 0x39 |
#define MPU6050_RA_INT_STATUS 0x3A |
#define MPU6050_RA_ACCEL_XOUT_H 0x3B |
#define MPU6050_RA_ACCEL_XOUT_L 0x3C |
#define MPU6050_RA_ACCEL_YOUT_H 0x3D |
#define MPU6050_RA_ACCEL_YOUT_L 0x3E |
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F |
#define MPU6050_RA_ACCEL_ZOUT_L 0x40 |
#define MPU6050_RA_TEMP_OUT_H 0x41 |
#define MPU6050_RA_TEMP_OUT_L 0x42 |
#define MPU6050_RA_GYRO_XOUT_H 0x43 |
#define MPU6050_RA_GYRO_XOUT_L 0x44 |
#define MPU6050_RA_GYRO_YOUT_H 0x45 |
#define MPU6050_RA_GYRO_YOUT_L 0x46 |
#define MPU6050_RA_GYRO_ZOUT_H 0x47 |
#define MPU6050_RA_GYRO_ZOUT_L 0x48 |
#define MPU6050_RA_EXT_SENS_DATA_00 0x49 |
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A |
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B |
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C |
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D |
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E |
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F |
#define MPU6050_RA_EXT_SENS_DATA_07 0x50 |
#define MPU6050_RA_EXT_SENS_DATA_08 0x51 |
#define MPU6050_RA_EXT_SENS_DATA_09 0x52 |
#define MPU6050_RA_EXT_SENS_DATA_10 0x53 |
#define MPU6050_RA_EXT_SENS_DATA_11 0x54 |
#define MPU6050_RA_EXT_SENS_DATA_12 0x55 |
#define MPU6050_RA_EXT_SENS_DATA_13 0x56 |
#define MPU6050_RA_EXT_SENS_DATA_14 0x57 |
#define MPU6050_RA_EXT_SENS_DATA_15 0x58 |
#define MPU6050_RA_EXT_SENS_DATA_16 0x59 |
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A |
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B |
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C |
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D |
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E |
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F |
#define MPU6050_RA_EXT_SENS_DATA_23 0x60 |
#define MPU6050_RA_MOT_DETECT_STATUS 0x61 |
#define MPU6050_RA_I2C_SLV0_DO 0x63 |
#define MPU6050_RA_I2C_SLV1_DO 0x64 |
#define MPU6050_RA_I2C_SLV2_DO 0x65 |
#define MPU6050_RA_I2C_SLV3_DO 0x66 |
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 |
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 |
#define MPU6050_RA_MOT_DETECT_CTRL 0x69 |
#define MPU6050_RA_USER_CTRL 0x6A |
#define MPU6050_RA_PWR_MGMT_1 0x6B |
#define MPU6050_RA_PWR_MGMT_2 0x6C |
#define MPU6050_RA_BANK_SEL 0x6D |
#define MPU6050_RA_MEM_START_ADDR 0x6E |
#define MPU6050_RA_MEM_R_W 0x6F |
#define MPU6050_RA_DMP_CFG_1 0x70 |
#define MPU6050_RA_DMP_CFG_2 0x71 |
#define MPU6050_RA_FIFO_COUNTH 0x72 |
#define MPU6050_RA_FIFO_COUNTL 0x73 |
#define MPU6050_RA_FIFO_R_W 0x74 |
#define MPU6050_RA_WHO_AM_I 0x75 |
#define MPU6050_TC_PWR_MODE_BIT 7 |
#define MPU6050_TC_OFFSET_BIT 6 |
#define MPU6050_TC_OFFSET_LENGTH 6 |
#define MPU6050_TC_OTP_BNK_VLD_BIT 0 |
#define MPU6050_VDDIO_LEVEL_VLOGIC 0 |
#define MPU6050_VDDIO_LEVEL_VDD 1 |
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 |
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 |
#define MPU6050_CFG_DLPF_CFG_BIT 2 |
#define MPU6050_CFG_DLPF_CFG_LENGTH 3 |
#define MPU6050_EXT_SYNC_DISABLED 0x0 |
#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 |
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 |
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 |
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 |
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 |
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 |
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 |
#define MPU6050_DLPF_BW_256 0x00 |
#define MPU6050_DLPF_BW_188 0x01 |
#define MPU6050_DLPF_BW_98 0x02 |
#define MPU6050_DLPF_BW_42 0x03 |
#define MPU6050_DLPF_BW_20 0x04 |
#define MPU6050_DLPF_BW_10 0x05 |
#define MPU6050_DLPF_BW_5 0x06 |
#define MPU6050_GCONFIG_FS_SEL_BIT 4 |
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 |
#define MPU6050_GYRO_FS_250 0x00 |
#define MPU6050_GYRO_FS_500 0x01 |
#define MPU6050_GYRO_FS_1000 0x02 |
#define MPU6050_GYRO_FS_2000 0x03 |
#define MPU6050_ACONFIG_XA_ST_BIT 7 |
#define MPU6050_ACONFIG_YA_ST_BIT 6 |
#define MPU6050_ACONFIG_ZA_ST_BIT 5 |
#define MPU6050_ACONFIG_AFS_SEL_BIT 4 |
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 |
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 |
#define MPU6050_ACCEL_FS_2 0x00 |
#define MPU6050_ACCEL_FS_4 0x01 |
#define MPU6050_ACCEL_FS_8 0x02 |
#define MPU6050_ACCEL_FS_16 0x03 |
#define MPU6050_DHPF_RESET 0x00 |
#define MPU6050_DHPF_5 0x01 |
#define MPU6050_DHPF_2P5 0x02 |
#define MPU6050_DHPF_1P25 0x03 |
#define MPU6050_DHPF_0P63 0x04 |
#define MPU6050_DHPF_HOLD 0x07 |
#define MPU6050_TEMP_FIFO_EN_BIT 7 |
#define MPU6050_XG_FIFO_EN_BIT 6 |
#define MPU6050_YG_FIFO_EN_BIT 5 |
#define MPU6050_ZG_FIFO_EN_BIT 4 |
#define MPU6050_ACCEL_FIFO_EN_BIT 3 |
#define MPU6050_SLV2_FIFO_EN_BIT 2 |
#define MPU6050_SLV1_FIFO_EN_BIT 1 |
#define MPU6050_SLV0_FIFO_EN_BIT 0 |
#define MPU6050_MULT_MST_EN_BIT 7 |
#define MPU6050_WAIT_FOR_ES_BIT 6 |
#define MPU6050_SLV_3_FIFO_EN_BIT 5 |
#define MPU6050_I2C_MST_P_NSR_BIT 4 |
#define MPU6050_I2C_MST_CLK_BIT 3 |
#define MPU6050_I2C_MST_CLK_LENGTH 4 |
#define MPU6050_CLOCK_DIV_348 0x0 |
#define MPU6050_CLOCK_DIV_333 0x1 |
#define MPU6050_CLOCK_DIV_320 0x2 |
#define MPU6050_CLOCK_DIV_308 0x3 |
#define MPU6050_CLOCK_DIV_296 0x4 |
#define MPU6050_CLOCK_DIV_286 0x5 |
#define MPU6050_CLOCK_DIV_276 0x6 |
#define MPU6050_CLOCK_DIV_267 0x7 |
#define MPU6050_CLOCK_DIV_258 0x8 |
#define MPU6050_CLOCK_DIV_500 0x9 |
#define MPU6050_CLOCK_DIV_471 0xA |
#define MPU6050_CLOCK_DIV_444 0xB |
#define MPU6050_CLOCK_DIV_421 0xC |
#define MPU6050_CLOCK_DIV_400 0xD |
#define MPU6050_CLOCK_DIV_381 0xE |
#define MPU6050_CLOCK_DIV_364 0xF |
#define MPU6050_I2C_SLV_RW_BIT 7 |
#define MPU6050_I2C_SLV_ADDR_BIT 6 |
#define MPU6050_I2C_SLV_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV_EN_BIT 7 |
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 |
#define MPU6050_I2C_SLV_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV_GRP_BIT 4 |
#define MPU6050_I2C_SLV_LEN_BIT 3 |
#define MPU6050_I2C_SLV_LEN_LENGTH 4 |
#define MPU6050_I2C_SLV4_RW_BIT 7 |
#define MPU6050_I2C_SLV4_ADDR_BIT 6 |
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 |
#define MPU6050_I2C_SLV4_EN_BIT 7 |
#define MPU6050_I2C_SLV4_INT_EN_BIT 6 |
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 |
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 |
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 |
#define MPU6050_MST_PASS_THROUGH_BIT 7 |
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 |
#define MPU6050_MST_I2C_LOST_ARB_BIT 5 |
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 |
#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 |
#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 |
#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 |
#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 |
#define MPU6050_INTCFG_INT_LEVEL_BIT 7 |
#define MPU6050_INTCFG_INT_OPEN_BIT 6 |
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 |
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 |
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 |
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 |
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 |
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 |
#define MPU6050_INTMODE_ACTIVEHIGH 0x00 |
#define MPU6050_INTMODE_ACTIVELOW 0x01 |
#define MPU6050_INTDRV_PUSHPULL 0x00 |
#define MPU6050_INTDRV_OPENDRAIN 0x01 |
#define MPU6050_INTLATCH_50USPULSE 0x00 |
#define MPU6050_INTLATCH_WAITCLEAR 0x01 |
#define MPU6050_INTCLEAR_STATUSREAD 0x00 |
#define MPU6050_INTCLEAR_ANYREAD 0x01 |
#define MPU6050_INTERRUPT_FF_BIT 7 |
#define MPU6050_INTERRUPT_MOT_BIT 6 |
#define MPU6050_INTERRUPT_ZMOT_BIT 5 |
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 |
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 |
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 |
#define MPU6050_INTERRUPT_DMP_INT_BIT 1 |
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 |
// TODO: figure out what these actually do |
// UMPL source code is not very obivous |
#define MPU6050_DMPINT_5_BIT 5 |
#define MPU6050_DMPINT_4_BIT 4 |
#define MPU6050_DMPINT_3_BIT 3 |
#define MPU6050_DMPINT_2_BIT 2 |
#define MPU6050_DMPINT_1_BIT 1 |
#define MPU6050_DMPINT_0_BIT 0 |
#define MPU6050_MOTION_MOT_XNEG_BIT 7 |
#define MPU6050_MOTION_MOT_XPOS_BIT 6 |
#define MPU6050_MOTION_MOT_YNEG_BIT 5 |
#define MPU6050_MOTION_MOT_YPOS_BIT 4 |
#define MPU6050_MOTION_MOT_ZNEG_BIT 3 |
#define MPU6050_MOTION_MOT_ZPOS_BIT 2 |
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 |
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 |
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 |
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 |
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 |
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 |
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 |
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 |
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 |
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 |
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 |
#define MPU6050_DETECT_FF_COUNT_BIT 3 |
#define MPU6050_DETECT_FF_COUNT_LENGTH 2 |
#define MPU6050_DETECT_MOT_COUNT_BIT 1 |
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 |
#define MPU6050_DETECT_DECREMENT_RESET 0x0 |
#define MPU6050_DETECT_DECREMENT_1 0x1 |
#define MPU6050_DETECT_DECREMENT_2 0x2 |
#define MPU6050_DETECT_DECREMENT_4 0x3 |
#define MPU6050_USERCTRL_DMP_EN_BIT 7 |
#define MPU6050_USERCTRL_FIFO_EN_BIT 6 |
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 |
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 |
#define MPU6050_USERCTRL_DMP_RESET_BIT 3 |
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 |
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 |
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 |
#define MPU6050_PWR1_DEVICE_RESET_BIT 7 |
#define MPU6050_PWR1_SLEEP_BIT 6 |
#define MPU6050_PWR1_CYCLE_BIT 5 |
#define MPU6050_PWR1_TEMP_DIS_BIT 3 |
#define MPU6050_PWR1_CLKSEL_BIT 2 |
#define MPU6050_PWR1_CLKSEL_LENGTH 3 |
#define MPU6050_CLOCK_INTERNAL 0x00 |
#define MPU6050_CLOCK_PLL_XGYRO 0x01 |
#define MPU6050_CLOCK_PLL_YGYRO 0x02 |
#define MPU6050_CLOCK_PLL_ZGYRO 0x03 |
#define MPU6050_CLOCK_PLL_EXT32K 0x04 |
#define MPU6050_CLOCK_PLL_EXT19M 0x05 |
#define MPU6050_CLOCK_KEEP_RESET 0x07 |
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 |
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 |
#define MPU6050_PWR2_STBY_XA_BIT 5 |
#define MPU6050_PWR2_STBY_YA_BIT 4 |
#define MPU6050_PWR2_STBY_ZA_BIT 3 |
#define MPU6050_PWR2_STBY_XG_BIT 2 |
#define MPU6050_PWR2_STBY_YG_BIT 1 |
#define MPU6050_PWR2_STBY_ZG_BIT 0 |
#define MPU6050_WAKE_FREQ_1P25 0x0 |
#define MPU6050_WAKE_FREQ_2P5 0x1 |
#define MPU6050_WAKE_FREQ_5 0x2 |
#define MPU6050_WAKE_FREQ_10 0x3 |
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 |
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 |
#define MPU6050_BANKSEL_MEM_SEL_BIT 4 |
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 |
#define MPU6050_WHO_AM_I_BIT 6 |
#define MPU6050_WHO_AM_I_LENGTH 6 |
#define MPU6050_DMP_MEMORY_BANKS 8 |
#define MPU6050_DMP_MEMORY_BANK_SIZE 256 |
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/sensor.c |
---|
0,0 → 1,30 |
/* |
* sensor.c |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
//############################################################################ |
//# HISTORY sensor.c |
//# |
//# 19.09.2015 cebra |
//# - add: Routinen für das GY-87 Sensorboard zur Berechnung der Kompasswerte etc. |
//# |
//############################################################################ |
#ifdef USE_KOMPASS |
#include "mpu6050.h" |
#include "sensor.h" |
void Init_Sensors(void) |
{ |
} |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/sensor.h |
---|
0,0 → 1,15 |
/* |
* sensor.h |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
#ifndef SENSOR_H_ |
#define SENSOR_H_ |
#endif /* 10DOF_SENSOR_H_ */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/branches/branch_FollowMeStep2/10DOF/. |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+_doc |