Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 2193 → Rev 2194

/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