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