/Transportables_Koptertool/PKT/trunk/10DOF/bmp085.c |
---|
0,0 → 1,206 |
/* |
bmp085 lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#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 |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/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/trunk/10DOF/hmc5883l.c |
---|
0,0 → 1,123 |
/* |
hmc5883l lib 0x01 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#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 |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/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/trunk/10DOF/mpu6050.c |
---|
0,0 → 1,686 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
#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 |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/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/trunk/10DOF/mpu6050dmp6.c |
---|
0,0 → 1,485 |
/* |
MPU6050 lib 0x02 |
copyright (c) Davide Gironi, 2012 |
Released under GPLv3. |
Please refer to LICENSE file for licensing information. |
*/ |
/* |
* 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 |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/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/trunk/10DOF/sensor.c |
---|
0,0 → 1,20 |
/* |
* sensor.c |
* |
* Created on: 18.09.2015 |
* Author: cebra |
*/ |
#include "mpu6050.h" |
#include "sensor.h" |
void Init_Sensors(void) |
{ |
} |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/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/trunk/10DOF |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+_doc |
/Transportables_Koptertool/PKT/trunk/HAL_HW3_9.c |
---|
132,7 → 132,7 |
#include "timer/timer.h" |
#include "wi232/Wi232.h" |
#include "motortest/twimaster.h" |
#include "i2cmaster.h" |
#include "bluetooth/bluetooth.h" |
#include "bluetooth/error.h" |
#include "connect.h" |
228,7 → 228,8 |
Display_on = 1; |
USART_Init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); |
uart1_init (UART_BAUD_SELECT(USART_BAUD,F_CPU)); // USB |
I2C_Init(1); |
// I2C_Init(1); |
i2c_init(); |
LCD_Init(0); // muss vor "ReadParameter" stehen |
ReadParameter(); // aktuelle Werte aus EEProm auslesen |
/Transportables_Koptertool/PKT/trunk/followme/followme.c |
---|
584,22 → 584,18 |
void Debug_GPS (void) |
{ |
uint8_t redraw; |
//char printbuff[100]; |
// |
double l1; |
GPS_Pos_t currpos; |
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep |
redraw = true; |
uint8_t mktimeout = false; |
uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete |
uint32_t send_followme_counter; |
Point_t FollowMe; |
while( true ) |
{ |
608,68 → 604,15 |
// NMEApos.lat = 0x0FB51D1F; //# |
// NMEApos.lon = 0xE2CF4105; //# |
//# |
// NMEApos.lat = 0x1f1db5fb; //# |
// NMEApos.lon = 0x0541cfe2; //# |
NMEApos.lat = 0x1f1db5fb; //# |
NMEApos.lon = 0x0541cfe2; //# |
// l1 = NMEApos.lat; |
//# |
//######################################################### |
int ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix |
if( ok <= 0 ) |
{ |
return; // Fehler bzgl. BT GPS-Maus -> exit |
} |
if(NMEA_isdataready() && receiveNMEA) |
{ |
if( NMEA.Counter > NMEA_GPGGA_counter_old ) |
{ |
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) ) |
{ |
NMEApos.lat = NMEA.Latitude; |
NMEApos.lon = NMEA.Longitude; |
//Config.FM_Refresh |
/* |
FollowMe.Position.Status = NEWDATA; |
FollowMe.Position.Longitude = NMEA.Longitude; |
FollowMe.Position.Latitude = NMEA.Latitude; |
FollowMe.Position.Altitude = 1; // 20.7.2015 CB |
// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B. |
FollowMe.Heading = -1; // invalid heading |
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default |
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ?????? |
// FollowMe.Event_Flag = 0; // no event |
FollowMe.Event_Flag = 1; // 20.7.2015 CB |
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII) |
FollowMe.Name[1] = 'O'; |
FollowMe.Name[2] = 'L'; |
FollowMe.Name[3] = 'L'; |
// FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015 |
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint |
FollowMe.Speed = Config.FM_Speed; // rate to change the Position |
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint |
send_followme_counter++; |
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ... |
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
//SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint |
*/ |
} |
NMEA_GPGGA_counter_old = NMEA.Counter; |
} |
//----------------------------------------- |
// Screen redraw |
//----------------------------------------- |
719,10 → 662,9 |
//############################## Test GPS Offset |
//nmea_move_horz(&NMEApos,&NMEATarget, Config.FM_Azimuth, Config.FM_Distance/1000); // neues Ziel berechnen |
nmea_move_horz(&NMEApos,&NMEATarget, Config.FM_Azimuth, Config.FM_Distance/1000); // neues Ziel berechnen |
// neues Ziel berechnen |
followme_calculate_offset(&NMEApos, &NMEATarget, 1, 1); |
writex_gpspos( 1, 7, (int32_t)NMEATarget.lat , MNORMAL,0,0 ); // Ziel Latitude |
writex_gpspos(10, 7, (int32_t)NMEATarget.lon , MNORMAL, 0,0 ); // Ziel Longitude |
745,13 → 687,9 |
redraw = true; |
// break; |
} |
} |
} |
} |
#endif |
#endif // #ifdef USE_FOLLOWME |
/Transportables_Koptertool/PKT/trunk/gps/gps.c |
---|
65,13 → 65,6 |
#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ |
#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ |
// Definitonen für FollowMeStep2 |
#define LONG_DIV 10000000 |
#define LAT_DIV LONG_DIV |
#define FOLLOWME_DEG2M 1/111111*LONG_DIV |
# define NMEA_POSIX(x) x |
118,26 → 111,7 |
} |
// Berechnet die Position der Kopters für FollowMeStep2 |
// Momentan wird die gleich Position ausgegeben |
int followme_calculate_offset( |
const nmeaPOS *pkt_pos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_lon /**< Distance long(m) */ |
) |
{ |
// only for test the "Debug-Mode" |
target_pos->lat = pkt_pos->lat; |
target_pos->lon = pkt_pos->lon; |
return 1; |
} |
//############################################################################################### |
/Transportables_Koptertool/PKT/trunk/gps/gps.h |
---|
29,6 → 29,7 |
extern nmeaPOS NMEATarget; |
int nmea_move_horz( |
const nmeaPOS *start_pos, /**< Start position in radians */ |
nmeaPOS *end_pos, /**< Result position in radians */ |
44,14 → 45,4 |
double distance /**< Distance (km) */ |
); |
int followme_calculate_offset( |
const nmeaPOS *pkt_pos, /**< Start position in radians */ |
nmeaPOS *target_pos, /**< Result position in radians */ |
int d_lat, /**< Distance lat(m) */ |
int d_lon /**< Distance long(m) */ |
); |
#endif // #define GPS_H_ |
/Transportables_Koptertool/PKT/trunk/main.h |
---|
349,7 → 349,7 |
//--------------------------------------------- |
//- Module unfertig bzw. noch in der Entwicklung |
//--------------------------------------------- |
#define USE_TRACKING // Antennentracking, benoetigt spezielle Hardware (ca. 6 KByte) |
//#define USE_TRACKING // Antennentracking, benoetigt spezielle Hardware (ca. 6 KByte) |
//--------------------------------------------- |
/Transportables_Koptertool/PKT/trunk/utils/i2cmaster.h |
---|
0,0 → 1,178 |
#ifndef _I2CMASTER_H |
#define _I2CMASTER_H 1 |
/************************************************************************* |
* Title: C include file for the I2C master interface |
* (i2cmaster.S or twimaster.c) |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: i2cmaster.h,v 1.10 2005/03/06 22:39:57 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device |
* Usage: see Doxygen manual |
**************************************************************************/ |
#ifdef DOXYGEN |
/** |
@defgroup pfleury_ic2master I2C Master library |
@code #include <i2cmaster.h> @endcode |
@brief I2C (TWI) Master Software Library |
Basic routines for communicating with I2C slave devices. This single master |
implementation is limited to one bus master on the I2C bus. |
This I2c library is implemented as a compact assembler software implementation of the I2C protocol |
which runs on any AVR (i2cmaster.S) and as a TWI hardware interface for all AVR with built-in TWI hardware (twimaster.c). |
Since the API for these two implementations is exactly the same, an application can be linked either against the |
software I2C implementation or the hardware I2C implementation. |
Use 4.7k pull-up resistor on the SDA and SCL pin. |
Adapt the SCL and SDA port and pin definitions and eventually the delay routine in the module |
i2cmaster.S to your target when using the software I2C implementation ! |
Adjust the CPU clock frequence F_CPU in twimaster.c or in the Makfile when using the TWI hardware implementaion. |
@note |
The module i2cmaster.S is based on the Atmel Application Note AVR300, corrected and adapted |
to GNU assembler and AVR-GCC C call interface. |
Replaced the incorrect quarter period delays found in AVR300 with |
half period delays. |
@author Peter Fleury pfleury@gmx.ch http://jump.to/fleury |
@par API Usage Example |
The following code shows typical usage of this library, see example test_i2cmaster.c |
@code |
#include <i2cmaster.h> |
#define Dev24C02 0xA2 // device address of EEPROM 24C02, see datasheet |
int main(void) |
{ |
unsigned char ret; |
i2c_init(); // initialize I2C library |
// write 0x75 to EEPROM address 5 (Byte Write) |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_write(0x75); // write value 0x75 to EEPROM |
i2c_stop(); // set stop conditon = release bus |
// read previously written value back from EEPROM address 5 |
i2c_start_wait(Dev24C02+I2C_WRITE); // set device address and write mode |
i2c_write(0x05); // write address = 5 |
i2c_rep_start(Dev24C02+I2C_READ); // set device address and read mode |
ret = i2c_readNak(); // read one byte from EEPROM |
i2c_stop(); |
for(;;); |
} |
@endcode |
*/ |
#endif /* DOXYGEN */ |
/**@{*/ |
#if (__GNUC__ * 100 + __GNUC_MINOR__) < 304 |
#error "This library requires AVR-GCC 3.4 or later, update to newer AVR-GCC compiler !" |
#endif |
#include <avr/io.h> |
/** defines the data direction (reading from I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_READ 1 |
/** defines the data direction (writing to I2C device) in i2c_start(),i2c_rep_start() */ |
#define I2C_WRITE 0 |
/** |
@brief initialize the I2C master interace. Need to be called only once |
@param void |
@return none |
*/ |
extern void i2c_init(void); |
/** |
@brief Terminates the data transfer and releases the I2C bus |
@param void |
@return none |
*/ |
extern void i2c_stop(void); |
/** |
@brief Issues a start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_start(unsigned char addr); |
/** |
@brief Issues a repeated start condition and sends address and transfer direction |
@param addr address and transfer direction of I2C device |
@retval 0 device accessible |
@retval 1 failed to access device |
*/ |
extern unsigned char i2c_rep_start(unsigned char addr); |
/** |
@brief Issues a start condition and sends address and transfer direction |
If device is busy, use ack polling to wait until device ready |
@param addr address and transfer direction of I2C device |
@return none |
*/ |
extern void i2c_start_wait(unsigned char addr); |
/** |
@brief Send one byte to I2C device |
@param data byte to be transfered |
@retval 0 write successful |
@retval 1 write failed |
*/ |
extern unsigned char i2c_write(unsigned char data); |
/** |
@brief read one byte from the I2C device, request more data from device |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readAck(void); |
/** |
@brief read one byte from the I2C device, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_readNak(void); |
/** |
@brief read one byte from the I2C device |
Implemented as a macro, which calls either i2c_readAck or i2c_readNak |
@param ack 1 send ack, request more data from device<br> |
0 send nak, read is followed by a stop condition |
@return byte read from I2C device |
*/ |
extern unsigned char i2c_read(unsigned char ack); |
#define i2c_read(ack) (ack) ? i2c_readAck() : i2c_readNak(); |
/**@}*/ |
#endif |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twi_slave.c |
---|
0,0 → 1,373 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.c |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : This is sample driver to AVRs TWI module. |
* It is interupt driveren. All functionality is controlled through |
* passing information to and from functions. Se main.c for samples |
* of how to use the driver. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
#if defined(__ICCAVR__) |
#include "ioavr.h" |
#include "inavr.h" |
#else |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#endif |
#include "twi_slave.h" |
// Emulate GCC ISR() statement in IAR |
#if defined(__ICCAVR__) |
#define PRAGMA(x) _Pragma( #x ) |
#define ISR(vec) PRAGMA( vector=vec ) __interrupt void handler_##vec(void) |
#endif |
static unsigned char TWI_buf[TWI_BUFFER_SIZE]; // Transceiver buffer. Set the size in the header file |
static unsigned char TWI_msgSize = 0; // Number of bytes to be transmitted. |
static unsigned char TWI_state = TWI_NO_STATE; // State byte. Default set to TWI_NO_STATE. |
// This is true when the TWI is in the middle of a transfer |
// and set to false when all bytes have been transmitted/received |
// Also used to determine how deep we can sleep. |
static volatile unsigned char TWI_busy = 0; |
union TWI_statusReg_t TWI_statusReg = {0}; // TWI_statusReg is defined in TWI_Slave.h |
/**************************************************************************** |
Call this function to set up the TWI slave to its initial standby state. |
Remember to enable interrupts from the main application after initializing the TWI. |
Pass both the slave address and the requrements for triggering on a general call in the |
same byte. Use e.g. this notation when calling this function: |
TWI_Slave_Initialise( (TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) ); |
The TWI module is configured to NACK on any requests. Use a TWI_Start_Transceiver function to |
start the TWI. |
****************************************************************************/ |
void twi_slave_init( unsigned char TWI_ownAddress ) |
{ |
TWAR = TWI_ownAddress; // Set own TWI slave address. Accept TWI General Calls. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins. |
(0<<TWIE)|(0<<TWINT)| // Disable TWI Interupt. |
(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Do not ACK on any requests, yet. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to test if the TWI_ISR is busy transmitting. |
****************************************************************************/ |
unsigned char twi_slave_busy( void ) |
{ |
return TWI_busy; |
} |
/**************************************************************************** |
Call this function to fetch the state information of the previous operation. The function will hold execution (loop) |
until the TWI_ISR has completed with the previous operation. If there was an error, then the function |
will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_status( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI has completed the transmission. |
return ( TWI_state ); // Return error state. |
} |
/**************************************************************************** |
Call this function to send a prepared message, or start the Transceiver for reception. Include |
a pointer to the data to be sent if a SLA+W is received. The data will be copied to the TWI buffer. |
Also include how many bytes that should be sent. Note that unlike the similar Master function, the |
Address byte is not included in the message buffers. |
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation, |
then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start_with_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char temp; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_msgSize = msgSize; // Number of data to transmit. |
for ( temp = 0; temp < msgSize; temp++ ) // Copy data that may be transmitted if the TWI Master requests data. |
{ |
TWI_buf[ temp ] = msg[ temp ]; |
} |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 1; |
} |
/**************************************************************************** |
Call this function to start the Transceiver without specifing new transmission data. Useful for restarting |
a transmission, or just starting the transceiver for reception. The driver will reuse the data previously put |
in the transceiver buffers. The function will hold execution (loop) until the TWI_ISR has completed with the |
previous operation, then initialize the next operation and return. |
****************************************************************************/ |
void twi_slave_start( void ) |
{ |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
TWI_statusReg.all = 0; |
TWI_state = TWI_NO_STATE ; |
TWCR = (1<<TWEN)| // TWI Interface enabled. |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag. |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Prepare to ACK next time the Slave is addressed. |
(0<<TWWC); // |
TWI_busy = 0; |
} |
/**************************************************************************** |
Call this function to read out the received data from the TWI transceiver buffer. I.e. first call |
TWI_Start_Transceiver to get the TWI Transceiver to fetch data. Then Run this function to collect the |
data when they have arrived. Include a pointer to where to place the data and the number of bytes |
to fetch in the function call. The function will hold execution (loop) until the TWI_ISR has completed |
with the previous operation, before reading out the data and returning. |
If there was an error in the previous transmission the function will return the TWI State code. |
****************************************************************************/ |
unsigned char twi_slave_get_data( unsigned char *msg, unsigned char msgSize ) |
{ |
unsigned char i; |
while ( twi_slave_busy() ) {} // Wait until TWI is ready for next transmission. |
if( TWI_statusReg.lastTransOK ) // Last transmission completed successfully. |
{ |
for ( i=0; i<msgSize; i++ ) // Copy data from Transceiver buffer. |
{ |
msg[ i ] = TWI_buf[ i ]; |
} |
TWI_statusReg.RxDataInBuf = FALSE; // Slave Receive data has been read from buffer. |
} |
return( TWI_statusReg.lastTransOK ); |
} |
// ********** Interrupt Handlers ********** // |
/**************************************************************************** |
This function is the Interrupt Service Routine (ISR), and called when the TWI interrupt is triggered; |
that is whenever a TWI event has occurred. This function should not be called directly from the main |
application. |
****************************************************************************/ |
ISR(TWI_vect) |
{ |
static unsigned char TWI_bufPtr; |
switch (TWSR) |
{ |
case TWI_STX_ADR_ACK: // Own SLA+R has been received; ACK has been returned |
// case TWI_STX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
case TWI_STX_DATA_ACK: // Data byte in TWDR has been transmitted; ACK has been received |
TWDR = TWI_buf[TWI_bufPtr++]; |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_STX_DATA_NACK: // Data byte in TWDR has been transmitted; NACK has been received. |
// I.e. this could be the end of the transmission. |
if (TWI_bufPtr == TWI_msgSize) // Have we transceived all expected data? |
{ |
TWI_statusReg.lastTransOK = TRUE; // Set status bits to completed successfully. |
} |
else // Master has sent a NACK before all data where sent. |
{ |
TWI_state = TWSR; // Store TWI State as errormessage. |
} |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Answer on next address match |
(0<<TWWC); // |
TWI_busy = 0; // Transmit is finished, we are not busy anymore |
break; |
case TWI_SRX_GEN_ACK: // General call address has been received; ACK has been returned |
// case TWI_SRX_GEN_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
TWI_statusReg.genAddressCall = TRUE; |
case TWI_SRX_ADR_ACK: // Own SLA+W has been received ACK has been returned |
// case TWI_SRX_ADR_ACK_M_ARB_LOST: // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
// Dont need to clear TWI_S_statusRegister.generalAddressCall due to that it is the default state. |
TWI_statusReg.RxDataInBuf = TRUE; |
TWI_bufPtr = 0; // Set buffer pointer to first data location |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Expect ACK on this transmission |
(0<<TWWC); |
TWI_busy = 1; |
break; |
case TWI_SRX_ADR_DATA_ACK: // Previously addressed with own SLA+W; data has been received; ACK has been returned |
case TWI_SRX_GEN_DATA_ACK: // Previously addressed with general call; data has been received; ACK has been returned |
// TODO: What is this? Seems to be no bounds checking! |
TWI_buf[TWI_bufPtr++] = TWDR; |
TWI_statusReg.lastTransOK = TRUE; // Set flag transmission successfull. |
// Reset the TWI Interupt to wait for a new event. |
TWCR = (1<<TWEN)| // TWI Interface enabled |
(1<<TWIE)|(1<<TWINT)| // Enable TWI Interupt and clear the flag to send byte |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Send ACK after next reception |
(0<<TWWC); // |
TWI_busy = 1; |
break; |
case TWI_SRX_STOP_RESTART: // A STOP condition or repeated START condition has been received while still addressed as Slave |
// Enter not addressed mode and listen to address match |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Enable interrupt and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Wait for new address match |
(0<<TWWC); // |
TWI_busy = 0; // We are waiting for a new address match, so we are not busy |
break; |
case TWI_SRX_ADR_DATA_NACK: // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
case TWI_SRX_GEN_DATA_NACK: // Previously addressed with general call; data has been received; NOT ACK has been returned |
case TWI_STX_DATA_ACK_LAST_BYTE: // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// case TWI_NO_STATE // No relevant state information available; TWINT = �0� |
case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition |
TWI_state = TWSR; //Store TWI State as errormessage, operation also clears noErrors bit |
TWCR = (1<<TWSTO)|(1<<TWINT); //Recover from TWI_BUS_ERROR, this will release the SDA and SCL pins thus enabling other devices to use the bus |
break; |
default: |
TWI_state = TWSR; // Store TWI State as errormessage, operation also clears the Success bit. |
TWCR = (1<<TWEN)| // Enable TWI-interface and release TWI pins |
(1<<TWIE)|(1<<TWINT)| // Keep interrupt enabled and clear the flag |
(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)| // Acknowledge on any new requests. |
(0<<TWWC); // |
TWI_busy = 0; // Unknown status, so we wait for a new address match that might be something we can handle |
} |
} |
/* |
void example(){ |
unsigned char messageBuf[TWI_BUFFER_SIZE]; |
unsigned char TWI_slaveAddress; |
// LED feedback port - connect port B to the STK500 LEDS |
DDRB = 0xFF; // Set to ouput |
PORTB = 0x55; // Startup pattern |
// Own TWI slave address |
TWI_slaveAddress = 0x10; |
// Initialise TWI module for slave operation. Include address and/or enable General Call. |
TWI_Slave_Initialise( (unsigned char)((TWI_slaveAddress<<TWI_ADR_BITS) | (TRUE<<TWI_GEN_BIT) )); |
SEI(); |
// Start the TWI transceiver to enable reseption of the first command from the TWI Master. |
TWI_Start_Transceiver(); |
// This example is made to work together with the AVR315 TWI Master application note. In adition to connecting the TWI |
// pins, also connect PORTB to the LEDS. The code reads a message as a TWI slave and acts according to if it is a |
// general call, or an address call. If it is an address call, then the first byte is considered a command byte and |
// it then responds differently according to the commands. |
// This loop runs forever. If the TWI is busy the execution will just continue doing other operations. |
for(;;) |
{ |
#ifdef POWER_MANAGEMENT_ENABLED |
// Sleep while waiting for TWI transceiver to complete or waiting for new commands. |
// If we have data in the buffer, we can't enter sleep because we have to take care |
// of it first. |
// If the transceiver is busy, we enter idle mode because it will wake up by all TWI |
// interrupts. |
// If the transceiver not is busy, we can enter power-down mode because next receive |
// should be a TWI address match and it wakes the device up from all sleep modes. |
if( ! TWI_statusReg.RxDataInBuf ) { |
if(TWI_Transceiver_Busy()) { |
MCUCR = (1<<SE)|(0<<SM2)|(0<<SM1)|(0<<SM0); // Enable sleep with idle mode |
} else { |
MCUCR = (1<<SE)|(0<<SM2)|(1<<SM1)|(0<<SM0); // Enable sleep with power-down mode |
} |
SLEEP(); |
} else { |
NOP(); // There is data in the buffer, code below takes care of it. |
} |
#else // No power management |
// Here you can add your own code that should be run while waiting for the TWI to finish |
NOP(); // Put own code here. |
#endif |
// Check if the TWI Transceiver has completed an operation. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
// Check if the last operation was successful |
if ( TWI_statusReg.lastTransOK ) |
{ |
// Check if the last operation was a reception |
if ( TWI_statusReg.RxDataInBuf ) |
{ |
TWI_Get_Data_From_Transceiver(messageBuf, 2); |
// Check if the last operation was a reception as General Call |
if ( TWI_statusReg.genAddressCall ) |
{ |
// Put data received out to PORTB as an example. |
PORTB = messageBuf[0]; |
} |
else // Ends up here if the last operation was a reception as Slave Address Match |
{ |
// Example of how to interpret a command and respond. |
// TWI_CMD_MASTER_WRITE stores the data to PORTB |
if (messageBuf[0] == TWI_CMD_MASTER_WRITE) |
{ |
PORTB = messageBuf[1]; |
} |
// TWI_CMD_MASTER_READ prepares the data from PINB in the transceiver buffer for the TWI master to fetch. |
if (messageBuf[0] == TWI_CMD_MASTER_READ) |
{ |
messageBuf[0] = PINB; |
TWI_Start_Transceiver_With_Data( messageBuf, 1 ); |
} |
} |
} |
else // Ends up here if the last operation was a transmission |
{ |
NOP(); // Put own code here. |
} |
// Check if the TWI Transceiver has already been started. |
// If not then restart it to prepare it for new receptions. |
if ( ! TWI_Transceiver_Busy() ) |
{ |
TWI_Start_Transceiver(); |
} |
} |
else // Ends up here if the last operation completed unsuccessfully |
{ |
TWI_Act_On_Failure_In_Last_Transmission( TWI_Get_State_Info() ); |
} |
} |
} |
}*/ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twi_slave.h |
---|
0,0 → 1,121 |
/***************************************************************************** |
* |
* Atmel Corporation |
* |
* File : TWI_Slave.h |
* Compiler : IAR EWAAVR 2.28a/3.10c |
* Revision : $Revision: 2475 $ |
* Date : $Date: 2007-09-20 12:00:43 +0200 (to, 20 sep 2007) $ |
* Updated by : $Author: mlarsson $ |
* |
* Support mail : avr@atmel.com |
* |
* Supported devices : All devices with a TWI module can be used. |
* The example is written for the ATmega16 |
* |
* AppNote : AVR311 - TWI Slave Implementation |
* |
* Description : Header file for TWI_slave.c |
* Include this file in the application. |
* |
****************************************************************************/ |
/*! \page MISRA |
* |
* General disabling of MISRA rules: |
* * (MISRA C rule 1) compiler is configured to allow extensions |
* * (MISRA C rule 111) bit fields shall only be defined to be of type unsigned int or signed int |
* * (MISRA C rule 37) bitwise operations shall not be performed on signed integer types |
* As it does not work well with 8bit architecture and/or IAR |
* Other disabled MISRA rules |
* * (MISRA C rule 109) use of union - overlapping storage shall not be used |
* * (MISRA C rule 61) every non-empty case clause in a switch statement shall be terminated with a break statement |
*/ |
/**************************************************************************** |
TWI Status/Control register definitions |
****************************************************************************/ |
#define TWI_BUFFER_SIZE 4 // Reserves memory for the drivers transceiver buffer. |
// Set this to the largest message size that will be sent including address byte. |
/**************************************************************************** |
Global definitions |
****************************************************************************/ |
union TWI_statusReg_t // Status byte holding flags. |
{ |
unsigned char all; |
struct |
{ |
unsigned char lastTransOK:1; |
unsigned char RxDataInBuf:1; |
unsigned char genAddressCall:1; // TRUE = General call, FALSE = TWI Address; |
unsigned char unusedBits:5; |
}; |
}; |
extern union TWI_statusReg_t TWI_statusReg; |
/**************************************************************************** |
Function definitions |
****************************************************************************/ |
void twi_slave_init( unsigned char ); |
unsigned char twi_slave_busy( void ); |
unsigned char twi_slave_get_status( void ); |
void twi_slave_start_with_data( unsigned char * , unsigned char ); |
void twi_slave_start( void ); |
unsigned char twi_slave_get_data( unsigned char *, unsigned char ); |
/**************************************************************************** |
Bit and byte definitions |
****************************************************************************/ |
#define TWI_READ_BIT 0 // Bit position for R/W bit in "address byte". |
#define TWI_ADR_BITS 1 // Bit position for LSB of the slave address bits in the init byte. |
#define TWI_GEN_BIT 0 // Bit position for LSB of the general call bit in the init byte. |
#define TRUE 1 |
#define FALSE 0 |
/**************************************************************************** |
TWI State codes |
****************************************************************************/ |
// General TWI Master staus codes |
#define TWI_START 0x08 // START has been transmitted |
#define TWI_REP_START 0x10 // Repeated START has been transmitted |
#define TWI_ARB_LOST 0x38 // Arbitration lost |
// TWI Master Transmitter staus codes |
#define TWI_MTX_ADR_ACK 0x18 // SLA+W has been tramsmitted and ACK received |
#define TWI_MTX_ADR_NACK 0x20 // SLA+W has been tramsmitted and NACK received |
#define TWI_MTX_DATA_ACK 0x28 // Data byte has been tramsmitted and ACK received |
#define TWI_MTX_DATA_NACK 0x30 // Data byte has been tramsmitted and NACK received |
// TWI Master Receiver staus codes |
#define TWI_MRX_ADR_ACK 0x40 // SLA+R has been tramsmitted and ACK received |
#define TWI_MRX_ADR_NACK 0x48 // SLA+R has been tramsmitted and NACK received |
#define TWI_MRX_DATA_ACK 0x50 // Data byte has been received and ACK tramsmitted |
#define TWI_MRX_DATA_NACK 0x58 // Data byte has been received and NACK tramsmitted |
// TWI Slave Transmitter staus codes |
#define TWI_STX_ADR_ACK 0xA8 // Own SLA+R has been received; ACK has been returned |
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0 // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned |
#define TWI_STX_DATA_ACK 0xB8 // Data byte in TWDR has been transmitted; ACK has been received |
#define TWI_STX_DATA_NACK 0xC0 // Data byte in TWDR has been transmitted; NOT ACK has been received |
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8 // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received |
// TWI Slave Receiver staus codes |
#define TWI_SRX_ADR_ACK 0x60 // Own SLA+W has been received ACK has been returned |
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68 // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK 0x70 // General call address has been received; ACK has been returned |
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78 // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_ACK 0x80 // Previously addressed with own SLA+W; data has been received; ACK has been returned |
#define TWI_SRX_ADR_DATA_NACK 0x88 // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned |
#define TWI_SRX_GEN_DATA_ACK 0x90 // Previously addressed with general call; data has been received; ACK has been returned |
#define TWI_SRX_GEN_DATA_NACK 0x98 // Previously addressed with general call; data has been received; NOT ACK has been returned |
#define TWI_SRX_STOP_RESTART 0xA0 // A STOP condition or repeated START condition has been received while still addressed as Slave |
// TWI Miscellaneous status codes |
#define TWI_NO_STATE 0xF8 // No relevant state information available; TWINT = �0� |
#define TWI_BUS_ERROR 0x00 // Bus error due to an illegal START or STOP condition |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils/twimaster.c |
---|
0,0 → 1,202 |
/************************************************************************* |
* Title: I2C master library using hardware TWI interface |
* Author: Peter Fleury <pfleury@gmx.ch> http://jump.to/fleury |
* File: $Id: twimaster.c,v 1.3 2005/07/02 11:14:21 Peter Exp $ |
* Software: AVR-GCC 3.4.3 / avr-libc 1.2.3 |
* Target: any AVR device with hardware TWI |
* Usage: API compatible with I2C Software Library i2cmaster.h |
**************************************************************************/ |
#include <inttypes.h> |
#include <compat/twi.h> |
#include <i2cmaster.h> |
/* define CPU frequency in Mhz here if not defined in Makefile */ |
#ifndef F_CPU |
#define F_CPU 16000000UL |
#endif |
/* I2C clock in Hz */ |
#define SCL_CLOCK 400000L |
/************************************************************************* |
Initialization of the I2C bus interface. Need to be called only once |
*************************************************************************/ |
void i2c_init(void) |
{ |
/* initialize TWI clock: 100 kHz clock, TWPS = 0 => prescaler = 1 */ |
TWSR = 0; /* no prescaler */ |
TWBR = ((F_CPU/SCL_CLOCK)-16)/2; /* must be > 10 for stable operation */ |
}/* i2c_init */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
return 0 = device accessible, 1= failed to access device |
*************************************************************************/ |
unsigned char i2c_start(unsigned char address) |
{ |
uint8_t twst; |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) return 1; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed and ACK/NACK has been received |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_MT_SLA_ACK) && (twst != TW_MR_SLA_ACK) ) return 1; |
return 0; |
}/* i2c_start */ |
/************************************************************************* |
Issues a start condition and sends address and transfer direction. |
If device is busy, use ack polling to wait until device is ready |
Input: address and transfer direction of I2C device |
*************************************************************************/ |
void i2c_start_wait(unsigned char address) |
{ |
uint8_t twst; |
while ( 1 ) |
{ |
// send START condition |
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst != TW_START) && (twst != TW_REP_START)) continue; |
// send device address |
TWDR = address; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wail until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits. |
twst = TW_STATUS & 0xF8; |
if ( (twst == TW_MT_SLA_NACK )||(twst ==TW_MR_DATA_NACK) ) |
{ |
/* device busy, send stop condition to terminate write operation */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
continue; |
} |
//if( twst != TW_MT_SLA_ACK) return 1; |
break; |
} |
}/* i2c_start_wait */ |
/************************************************************************* |
Issues a repeated start condition and sends address and transfer direction |
Input: address and transfer direction of I2C device |
Return: 0 device accessible |
1 failed to access device |
*************************************************************************/ |
unsigned char i2c_rep_start(unsigned char address) |
{ |
return i2c_start( address ); |
}/* i2c_rep_start */ |
/************************************************************************* |
Terminates the data transfer and releases the I2C bus |
*************************************************************************/ |
void i2c_stop(void) |
{ |
/* send stop condition */ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); |
// wait until stop condition is executed and bus released |
while(TWCR & (1<<TWSTO)); |
}/* i2c_stop */ |
/************************************************************************* |
Send one byte to I2C device |
Input: byte to be transfered |
Return: 0 write successful |
1 write failed |
*************************************************************************/ |
unsigned char i2c_write( unsigned char data ) |
{ |
uint8_t twst; |
// send data to the previously addressed device |
TWDR = data; |
TWCR = (1<<TWINT) | (1<<TWEN); |
// wait until transmission completed |
while(!(TWCR & (1<<TWINT))); |
// check value of TWI Status Register. Mask prescaler bits |
twst = TW_STATUS & 0xF8; |
if( twst != TW_MT_DATA_ACK) return 1; |
return 0; |
}/* i2c_write */ |
/************************************************************************* |
Read one byte from the I2C device, request more data from device |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readAck(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readAck */ |
/************************************************************************* |
Read one byte from the I2C device, read is followed by a stop condition |
Return: byte read from I2C device |
*************************************************************************/ |
unsigned char i2c_readNak(void) |
{ |
TWCR = (1<<TWINT) | (1<<TWEN); |
while(!(TWCR & (1<<TWINT))); |
return TWDR; |
}/* i2c_readNak */ |
Property changes: |
Added: svn:mime-type |
+text/plain |
\ No newline at end of property |
/Transportables_Koptertool/PKT/trunk/utils |
---|
Property changes: |
Added: svn:ignore |
+_old |
+ |
+maniacbug-mighty-1284p-68ed99c |
+ |
+Arduino |
+ |
+_doc |
+ |
+maniacbug-mighty-1284p-68ed99c.zip |