做10轴传感器时遇到了莫名问题

萧芸凤 · 发表于 2012-9-12 18:39:35

我的四翼飞行器开始了，论坛上东平西凑的找到了许多代码研究，有些在单一模块上可以正常运行，但是到了10轴中就出问题了，好不容易才找到一个，一开始没问题，完全是平铺直叙的写可以运行，但是编译后居然17K之多，加上滤波等等空间不够了，改称函数调用出了问题。
代码如下

int ADXAddress = 0x53; // ADXL345的I2C地址

int L3GAddress = 0x69; // L3G4200D的I2C地址

int HMCAddress = 0x1E; // HMC5883L的I2C地址

int BMPAddress = 0x77; // BMP085的I2C地址

long p0=101325;       // 海平面大气压

const unsigned char OSS = 0; // Oversampling 设置

int  acce[3],magn[3],gyro[3]; //存储传感器xyz三轴结果

long atmo[3]; //BMP085结果存储，温度，气压，高度

int  temp[6]; //临时变量，用于存取寄存器值

#include "Wire.h"    // IIC运行库

void setup()

{

  deviceInti();

 

}

void loop()

{

 

    acceRead();

    Serial.print("xAcc=");

    Serial.print(acce[0]);

    Serial.print("  yAcc=");

    Serial.print(acce[1]);

    Serial.print("  zAcc=");

    Serial.println(acce[2]);

    delay(100);

    gyroRead();

    Serial.print("xGyro=");

    Serial.print(gyro[0]);

    Serial.print("  yGyro=");

    Serial.print(gyro[1]);

    Serial.print("  zGyro=");

    Serial.println(gyro[2]);

    delay(100);

    magnRead();

   Serial.print("xMag=");

    Serial.print(magn[0]);

    Serial.print("  yMag=");

    Serial.print(magn[1]);

    Serial.print("  zMag=");

    Serial.println(magn[2]);

    Serial.println();

    atmoRead();

    Serial.print("Temperature: ");

    Serial.print(atmo[0], DEC);

    Serial.print(" *0.1 deg C");

    Serial.print(" Pressure: ");

    Serial.print(atmo[1], DEC);

    Serial.print(" Pa");

    Serial.print(" altitude: ");

    Serial.println(atmo[2], DEC);

    delay(1000);

}

void deviceInti() //初始化

{

Wire.begin();

Serial.begin(9600);

acceInti();

magnInti();

gyroInti();

atmoInti();

}

void registerSet(int deviceAddress, int registerAddress, int registerValue)  // 写寄存器

{

           

    Wire.beginTransmission(deviceAddress);

    Wire.write(registerAddress);

    Wire.write(registerValue);

    Wire.endTransmission();

}

void registerRead(int deviceAddress, int registerAddress) // 读寄存器

{ 

int i;

    Wire.beginTransmission(deviceAddress);

    Wire.write(registerAddress);

    Wire.endTransmission();

    Wire.requestFrom(deviceAddress,6);

if(Wire.available()<=6);;

{

    for(i=0;i<6;i++)

                    temp = Wire.read();

}

Wire.endTransmission();

}

void acceInti() // 加速度传感器初始化

{

 registerSet(ADXAddress,0x2D,0x08); // 测量模式

}

void acceRead() //加速度读取

{

registerRead(ADXAddress,0x32);

acce[0] = temp[1]<<8 | temp[0];

acce[1] = temp[3]<<8 | temp[2];

acce[2] = temp[5]<<8 | temp[4];

        

}

void magnInti() // 磁场传感器初始化

{

registerSet(HMCAddress, 0x02, 0x00);  // 连续测量

}

void magnRead() //磁场传感器读取

{

registerRead(HMCAddress,0x03);

magn[0] = temp[0]<<8 | temp[1];

magn[1] = temp[2]<<8 | temp[3];

magn[2] = temp[4]<<8 | temp[5];

}

void gyroInti() //陀螺仪初始化

{

  registerSet(L3GAddress, 0x20, 0x0F);   // 设置睡眠模式、x, y, z轴使能

  registerSet(L3GAddress, 0x21, 0x00);   // 选择高通滤波模式和高通截止频率

  registerSet(L3GAddress, 0x22, 0x00);   // 设置中断模式

  registerSet(L3GAddress, 0x23, 0x30);   // 设置量程(2000dps)、自检状态、SPI模式

  registerSet(L3GAddress, 0x24, 0x00);   // FIFO & 高通滤波

}

void gyroRead() // 陀螺仪读取

{

registerRead(L3GAddress,0x28);

gyro[0] = temp[1]<<8 | temp[0];

gyro[1] = temp[3]<<8 | temp[2];

gyro[2] = temp[5]<<8 | temp[4];

}

void atmoInti() //气压传感器初始化

{

}

void atmoRead() //气压传感器读取

{

int ac1,ac2,ac3;

unsigned int ac4,ac5,ac6,ut;

int b1,b2,mb,mc,md;

long b5,x1, x2, x3, b3, b6, p,pressure,altitude;

short temperature;

unsigned long up,b4, b7;

//////////////////////////////////////内部寄存器读取

registerRead(BMPAddress,0xAA);

ac1 = temp[0]<<8 | temp[1];

ac2 = temp[2]<<8 | temp[3];

ac3 = temp[4]<<8 | temp[5];

registerRead(BMPAddress,0xB0);

ac4 = temp[0]<<8 | temp[1];

ac5 = temp[2]<<8 | temp[3];

ac6 = temp[4]<<8 | temp[5];

registerRead(BMPAddress,0xB6);

b1 = temp[0]<<8 | temp[1];

b2 = temp[2]<<8 | temp[3];

;

registerRead(BMPAddress,0xBA);

mb = temp[0]<<8 | temp[1];

mc = temp[2]<<8 | temp[3];

md = temp[4]<<8 | temp[5];

/////////////////////////////////////////////////计算UP值

registerSet(BMPAddress,0xF4,0x34 + (OSS<<6));

delay(2 + (3<<OSS));

 

registerRead(BMPAddress,0xF6);

    

up = (((unsigned long) temp[0] << 16) | ((unsigned long) temp[1] << 8) | (unsigned long) temp[2]) >> (8-OSS);

/////////////////////////////////////////////////计算UT值

registerSet(BMPAddress,0xF4,0x2E);

delay(5);

registerRead(BMPAddress,0xF6);

ut = temp[0] << 8 | temp[1];

/////////////////////////////////////////////////计算温度

x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;

x2 = ((long)mc << 11)/(x1 + md);

b5 = x1 + x2;

 

temperature = ((b5 + 8)>>4); 

//////////////////////////////////////////////////计算气压

b6 = b5 - 4000;

x1 = (b2 * (b6 * b6)>>12)>>11;

x2 = (ac2 * b6)>>11;

x3 = x1 + x2;

b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;

 

x1 = (ac3 * b6)>>13;

x2 = (b1 * ((b6 * b6)>>12))>>16;

x3 = ((x1 + x2) + 2)>>2;

b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;

 

b7 = ((unsigned long)(up - b3) * (50000>>OSS));

if (b7 < 0x80000000)

p = (b7<<1)/b4;

else

p = (b7/b4)<<1;

   

x1 = (p>>8) * (p>>8);

x1 = (x1 * 3038)>>16;

x2 = (-7357 * p)>>16;

        p += (x1 + x2 + 3791)>>4;

pressure = p;

//////////////////////////////////////////////////计算海拔

altitude = 44330*(1-pow((double)p/p0,1/5.255))*100;

////////////////////////

atmo[0] = temperature;

atmo[1] = pressure;

atmo[2] = altitude;

}

运行结果

陀螺仪出现了问题，经过检查发现6个寄存器得到的结果是同一个数值，而其他的都正确。

为了验证是否是器件问题，用了另一个代码

#include <Wire.h>          // IIC运行库

const unsigned char OSS = 0;  // Oversampling Setting

float  p0=1013.25;

int BMPAddress = 0x77;

int ADXAddress = 0x53;       // ADXL345的I2C地址

int L3GAddress = 0x69;    // L3G4200D的I2C地址

int HMCAddress = 0x1E;    // HMC5883L的I2C地址

byte vL, vH,vOF;          // 存放低位、高位值

int xAcc,  yAcc,  zAcc;    // 存放加速度值(整型）

int xGyro, yGyro, zGyro; // 存放角速度值(整型）

int xMag,  yMag,  zMag;    // 存放地磁场值(整型）

int xOfst,yOfst,zOfst;    // 存放加速度偏移值(整型）

float  xAf,  yAf,  zAf;    // 存放加速度值(浮点）

float  xGf, yGf, zGf;    // 存放角速度值(浮点）

float  xMf,  yMf,  zMf;    // 存放地磁场值(浮点）

// 存放压力寄存器值

int ac1;

int ac2;

int ac3;

unsigned int ac4;

unsigned int ac5;

unsigned int ac6;

int b1;

int b2;

int mb;

int mc;

int md;

long b5;

short temperature; //存放温度变量值

long pressure; //存放压力变量值

void setup()

{

    Serial.begin(9600);       // 定义串口

    Wire.begin();

    delay(100);

    Serial.println(" Device Initializating......");

    void deviceInt(); //设备初始化

    delay(2000);

}

void loop()

{

    getAccValues(); //输出加速度

    Serial.print("xAcc=");

    Serial.print(xAcc);

    Serial.print("  yAcc=");

    Serial.print(yAcc);

    Serial.print("  zAcc=");

    Serial.println(zAcc);

    delay(100);

    getGyroValues(); //输出角加速度

    Serial.print("xGyro=");

    Serial.print(xGyro);

    Serial.print("  yGyro=");

    Serial.print(yGyro);

    Serial.print("  zGyro=");

    Serial.println(zGyro);

    delay(100);

    getMagValues(); //输出磁罗盘

    Serial.print("xMag=");

    Serial.print(xMag);

    Serial.print("  yMag=");

    Serial.print(yMag);

    Serial.print("  zMag=");

    Serial.println(zMag);

    Serial.println();

temperature = getTemperature(readUT()); //输出温度

pressure = getPressure(readUP()); //输出压力

Serial.print("Temperature: ");

Serial.print(temperature, DEC);

Serial.println(" *0.1 deg C");

Serial.print("Pressure: ");

Serial.print(pressure, DEC);

Serial.println(" Pa");

Serial.println();

    delay(1000);

}

void deviceInt()

{

    writeRegister(ADXAddress, 0x2D, 0b00001000);   // 测量模式

                            // 配置ADXL345

    writeRegister(L3GAddress, 0x20, 0b00001111);   // 设置睡眠模式、x, y, z轴使能

    writeRegister(L3GAddress, 0x21, 0b00000000);   // 选择高通滤波模式和高通截止频率

    writeRegister(L3GAddress, 0x22, 0b00000000);   // 设置中断模式

    writeRegister(L3GAddress, 0x23, 0b00110000);   // 设置量程(2000dps)、自检状态、SPI模式

    writeRegister(L3GAddress, 0x24, 0b00000000);   // FIFO & 高通滤波

                            // 配置L3G4200D(2000 deg/sec)

    writeRegister(HMCAddress, 0x02, 0x00);         // 连续测量

                            // 配置HMC5883L

bmpCalibration();

}

void setAccOffset() //加速度偏移设置

{

writeRegister(ADXAddress, 0x1E, 0x00);

writeRegister(ADXAddress, 0x1F, 0x00);

writeRegister(ADXAddress, 0x20, 0x00);

}

void getAccValues()         // 加速度值读取

{

    vL = readRegister(ADXAddress, 0x32);

    vH = readRegister(ADXAddress, 0x33);

vOF = readRegister(ADXAddress, 0x1E);

    xAcc = (vH << 8) | vL;

xAcc = xAcc +vOF;

xAf = xAcc/2560.0000;

    vL = readRegister(ADXAddress, 0x34);

    vH = readRegister(ADXAddress, 0x35);

vOF = readRegister(ADXAddress, 0x1F);

    yAcc = (vH << 8) | vL;

yAcc = yAcc + vOF;

yAf = yAcc/2560.0000;

    vL = readRegister(ADXAddress, 0x36);

    vH = readRegister(ADXAddress, 0x37);

vOF = readRegister(ADXAddress, 0x20);

    zAcc = (vH << 8) | vL;

zAcc = zAcc + vOF;

zAf = zAcc/2560.0000;

}

void getGyroValues()       // 角速度值读取

{

    vL = readRegister(L3GAddress, 0x28);

    vH = readRegister(L3GAddress, 0x29);

    xGyro = (vH << 8) | vL;

    vL = readRegister(L3GAddress, 0x2A);

    vH = readRegister(L3GAddress, 0x2B);

    yGyro = (vH << 8) | vL;

    vL = readRegister(L3GAddress, 0x2C);

    vH = readRegister(L3GAddress, 0x2D);

    zGyro = (vH << 8) | vL;

}

void getMagValues()       // 磁场值读取

{

    vH = readRegister(HMCAddress, 0x03);

    vL = readRegister(HMCAddress, 0x04);

    xMag = (vH << 8) | vL;

    vH = readRegister(HMCAddress, 0x05);

    vL = readRegister(HMCAddress, 0x06);

    yMag = (vH << 8) | vL;

    vH = readRegister(HMCAddress, 0x07);

    vL = readRegister(HMCAddress, 0x08);

    zMag = (vH << 8) | vL;

}

void bmpCalibration() //气压传感器值读取

{

  ac1 = readInt(0xAA);

  ac2 = readInt(0xAC);

  ac3 = readInt(0xAE);

  ac4 = readInt(0xB0);

  ac5 = readInt(0xB2);

  ac6 = readInt(0xB4);

  b1 = readInt(0xB6);

  b2 = readInt(0xB8);

  mb = readInt(0xBA);

  mc = readInt(0xBC);

  md = readInt(0xBE);

}

short getTemperature(unsigned int ut) //温度计算

{

  long x1, x2;

 

  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;

  x2 = ((long)mc << 11)/(x1 + md);

  b5 = x1 + x2;

 

  return ((b5 + 8)>>4);

}

long getPressure(unsigned long up) //气压计算

{

  long x1, x2, x3, b3, b6, p;

  unsigned long b4, b7,ut;

  ut=readUT();

  

  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;

  x2 = ((long)mc << 11)/(x1 + md);

  b5 = x1 + x2;

 

  b6 = b5 - 4000;

  // 计算 B3

  x1 = (b2 * (b6 * b6)>>12)>>11;

  x2 = (ac2 * b6)>>11;

  x3 = x1 + x2;

  b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;

 

  // 计算 B4

  x1 = (ac3 * b6)>>13;

  x2 = (b1 * ((b6 * b6)>>12))>>16;

  x3 = ((x1 + x2) + 2)>>2;

  b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;

 

  b7 = ((unsigned long)(up - b3) * (50000>>OSS));

  if (b7 < 0x80000000)

    p = (b7<<1)/b4;

  else

    p = (b7/b4)<<1;

   

  x1 = (p>>8) * (p>>8);

  x1 = (x1 * 3038)>>16;

  x2 = (-7357 * p)>>16;

  p += (x1 + x2 + 3791)>>4;

 

  return p;

}

 

  float getAltitude(long p)

 {

float al;

al = 44330*(1-pow(p/p0,1/5.255));

return al;

 }

 

int readInt(int address) //两字节寄存器读取

{

  unsigned char msb, lsb;

 

  msb = readRegister(BMPAddress,address);

  lsb = readRegister(BMPAddress,address+0x01);

 

  return (int) msb<<8 | lsb;

}

unsigned int readUT() //UT计算

{

unsigned int ut;

 

  // Write 0x2E into Register 0xF4

  // This requests a temperature reading

 

writeRegister(BMPAddress,0xF4,0x2E);

 

  // Wait at least 4.5ms

delay(5);

 

  // Read two bytes from registers 0xF6 and 0xF7

ut = readInt(0xF6);

return ut;

}

unsigned long readUP() //三字节寄存器读取

{

  unsigned char msb, lsb, xlsb;

  unsigned long up = 0;

 

  // Write 0x34+(OSS<<6) into register 0xF4

  // Request a pressure reading w/ oversampling setting

 writeRegister(BMPAddress,0xF4,(0x34 + (OSS<<6)));

 

  // Wait for conversion, delay time dependent on OSS

  delay(2 + (3<<OSS));

 

  // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)

 

  msb = readRegister(BMPAddress,0xF6);

  lsb = readRegister(BMPAddress,0xF7);

  xlsb = readRegister(BMPAddress,0xF8);

 

  up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);

 

  return up;

}

int readRegister(int deviceAddress, byte address) // 读寄存器

{

           

    int v;

    Wire.beginTransmission(deviceAddress);

    Wire.write(address);

    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1);

    while(!Wire.available()) {}

    v = Wire.read();

    return v;

}

void writeRegister(int deviceAddress, byte address, byte val)  // 写寄存器

{

           

    Wire.beginTransmission(deviceAddress);

    Wire.write(address);

    Wire.write(val);

    Wire.endTransmission();

}

这一次运行正常了，结果

搞不懂什么原因。求达人指导。

萧芸凤 · 发表于 2012-9-12 21:41:32

我发了好几个求助贴都没有解决

Malc · 发表于 2012-9-12 23:45:01

这应该不是陀螺仪的问题
看了你的代码
只见你把temp[]值赋给其他变量，没给temp[]更新啊？？
估计这就是问题

萧芸凤 · 发表于 2012-9-13 00:13:38

Malc 发表于 2012-9-12 23:45
这应该不是陀螺仪的问题
看了你的代码
只见你把temp[]值赋给其他变量，没给temp[]更新啊？？

如果是因为temp［］没更新，那么后面的5883也应该有问题才对。而且我做了很多尝试，屏蔽了其他函数，同样陀螺仪读数不正常。就是四个传感器一起运行，我做过测试，每次temp［］付值前输出都是0。这个是基于C语言的，按照C语言的规则，函数调用变量后并不需要清零，再说了，读取寄存器时已经相当于一次重新付值了，现在的关键是六组数据在陀螺仪中是相同的，而且移动传感器寄存器值不变。
按照网上的另一种说法是陀螺仪寄存器不能用0x28,要用0xA8,最高位要是1寄存器指针才会自动加1,明天测试以下在看了。看了动力老男孩的代码，他用的是0xA8作为地址。

Malc · 发表于 2012-9-13 00:54:30

萧芸凤发表于 2012-9-13 00:13
如果是因为temp［］没更新，那么后面的5883也应该有问题才对。而且我做了很多尝试，屏蔽了其他函数，同样 ...

你看一下代码里面的registeRead（）
里面那段for里头temp=Wire.read();
这个应该是语法错误，编译无法通过的
查了l3g4200的datasheet，没有0xa8这个寄存器。。

Malc · 发表于 2012-9-13 01:06:49

http://www.geek-workshop.com/forum.php?mod=viewthread&tid=659
貌似有人曾经也遇到了类似问题，也是读取0xA8解决。。
这个以后再研究

萧芸凤 · 发表于 2012-9-13 17:49:38

Malc 发表于 2012-9-13 00:54
你看一下代码里面的registeRead（）
里面那段for里头temp=Wire.read();
这个应该是语法错误，编译无法通 ...

解决了，要把0x28改成0xA8,4200中是没有这个寄存器，其实就是哪个数据寄存器28,最高位设1就成了A8了。

萧芸凤 · 发表于 2012-9-13 17:59:22

现在寄存器是输出了，但是，但是这些结果是什么意思，我是说是什么单位。
L345我知道除以256就是g值，但是4200输出的是什么单位，直接积分就是角度吗，还是需要转换以下才能积分？5883输出的又是什么单位，如何才能得到角度，比如说北偏东多少度？

最新的结果如图

我是参考论坛上的几个代码做的转换，很奇怪，只要稍稍的动一下陀螺仪的数据就变的很大，而且也不会再回到原来的水平。
代码如下

void accAngle()

{

int i;

float accR = 0,

acceRead();

for( i = 0 ; i < 3 ; i++ )

{     

            acce = acce + accofst;

            accef = acce /256.00;

    accR = accR + accef*accef;

}

accR = sqrt(accR);

for( i = 0 ; i < 3 ; i++ )

{

accangle = 90-(180.00 * acos(accef/accR) / PI);

}

}

void gyrAngle()

{

int i;

gyroRead();

for( i = 0; i < 3 ; i++)

{

gyrangle = gyrangle + GYRO_GAIN * (gyro + gyrofst) * i_time/1000;

}

}

void magAngle()

{

int i;

float scale = scaleSet(1);

magnRead();

for( i = 0 ; i < 3 ; i++)

{

magn =scale * ( magn + magofst );

}

magnf[0] = atan2( magn[1] , magn[0] );

magnf[1] = atan2( magn[2] , magn[0] );

magnf[2] = atan2( magn[2] , magn[1] );

for( i=0; i<3;i++)

{

if (magnf<0)

magnf = magnf + 2 * PI;

else if (magnf > 2*PI )

magnf = magnf - 2 * PI;

magangle = magnf * 180.00 / PI;

}

}

float scaleSet(float guass)

{

int registerB;

float scale;

switch( guass)

{

default :

break;

case 0:    //0.88 :

scale = 0.73;

registerB = 0x00;

break;

case 1:    //1.3 :

scale = 0.92;

registerB = 0x01;

break;

case 2 :  // 1.9

scale = 1.22;

registerB = 0x02;

break;

case 3 :    //2.5 :

scale = 1.52;

registerB = 0x03;

break;

case 4:       //4.0 :

scale = 2.27;

registerB = 0x04;

break;

case 5:          //4.7 :

scale = 2.56;

registerB = 0x05;

break;

case 6:          //5.6 :

scale = 3.03;

registerB = 0x06;

break;

case 7:          //8.1 :

scale = 4.35;

registerB = 0x07;

break;

}

registerSet(HMCAddress,0x01,registerB<<5);

return scale;

}

萧芸凤 · 发表于 2012-9-14 17:56:26

代码写完了，输出的结果很奇怪，放在那里不动，角度一直在增加，而如果不断的移动增加的速度居然又跟不上了。主要是在陀螺仪上面，静止的时候有漂移，但要是运动起来输出又不足以让速度（角度）变化起来。
还有一个问题，在做滤波融合的时候，加速的X轴计算结果应该是和陀螺仪的Y轴进行的吧，X轴移动的角度可以看作绕Y轴的旋转。不知道这样理解是否正确。
到目前为止还不知道磁场测出的结果有什么用。
同样的程序，今天运行后得到的海拔居然是－18米，昨天是24米，气压有变化，不知道是不是和今天杭州下雨有关。

ohenry · 发表于 2013-6-30 20:56:01

陀螺仪有漂移是正常的，会导致角度一直增加，解决方法大概是用加速度传感器来修正，陀螺仪的动态响应不错，所以用来测试角速度和角加速度比较好。
建议查找一本书，两轮自平衡机器人原理，好像是这个书名，里面对陀螺仪和加速度计结合测试角度，角速度和角加速度有深刻的分析，还有各种参考系，还有个惯性导联系统之类，说的就是这些原理性的东西。

学慧放弃 · 发表于 2013-6-30 21:37:18

楼主发帖果然引出了不少高手啊，继续学习楼主的10轴为以后做参考

b9ss · 发表于 2014-6-30 21:43:37

好资料留个爪，以后再看

帐号		自动登录	找回密码
密码			注册

做10轴传感器时遇到了莫名问题

本帖子中包含更多资源

本帖子中包含更多资源