做10轴传感器时遇到了莫名问题
我的四翼飞行器开始了，论坛上东平西凑的找到了许多代码研究，有些在单一模块上可以正常运行，但是到了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 设置
intacce,magn,gyro; //存储传感器xyz三轴结果
long atmo; //BMP085结果存储，温度，气压，高度
inttemp; //临时变量，用于存取寄存器值
#include "Wire.h" // IIC运行库
void setup()
{
deviceInti();
}
void loop()
{
acceRead();
Serial.print("xAcc=");
Serial.print(acce);
Serial.print("yAcc=");
Serial.print(acce);
Serial.print("zAcc=");
Serial.println(acce);
delay(100);
gyroRead();
Serial.print("xGyro=");
Serial.print(gyro);
Serial.print("yGyro=");
Serial.print(gyro);
Serial.print("zGyro=");
Serial.println(gyro);
delay(100);
magnRead();
Serial.print("xMag=");
Serial.print(magn);
Serial.print("yMag=");
Serial.print(magn);
Serial.print("zMag=");
Serial.println(magn);
Serial.println();
atmoRead();
Serial.print("Temperature: ");
Serial.print(atmo, DEC);
Serial.print(" *0.1 deg C");
Serial.print(" Pressure: ");
Serial.print(atmo, DEC);
Serial.print(" Pa");
Serial.print(" altitude: ");
Serial.println(atmo, 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 = temp<<8 | temp;
acce = temp<<8 | temp;
acce = temp<<8 | temp;
}
void magnInti() // 磁场传感器初始化
{
registerSet(HMCAddress, 0x02, 0x00); // 连续测量
}
void magnRead() //磁场传感器读取
{
registerRead(HMCAddress,0x03);
magn = temp<<8 | temp;
magn = temp<<8 | temp;
magn = temp<<8 | temp;
}
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 = temp<<8 | temp;
gyro = temp<<8 | temp;
gyro = temp<<8 | temp;
}
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<<8 | temp;
ac2 = temp<<8 | temp;
ac3 = temp<<8 | temp;
registerRead(BMPAddress,0xB0);
ac4 = temp<<8 | temp;
ac5 = temp<<8 | temp;
ac6 = temp<<8 | temp;
registerRead(BMPAddress,0xB6);
b1 = temp<<8 | temp;
b2 = temp<<8 | temp;
;
registerRead(BMPAddress,0xBA);
mb = temp<<8 | temp;
mc = temp<<8 | temp;
md = temp<<8 | temp;
/////////////////////////////////////////////////计算UP值
registerSet(BMPAddress,0xF4,0x34 + (OSS<<6));
delay(2 + (3<<OSS));
registerRead(BMPAddress,0xF6);
up = (((unsigned long) temp << 16) | ((unsigned long) temp << 8) | (unsigned long) temp) >> (8-OSS);
/////////////////////////////////////////////////计算UT值
registerSet(BMPAddress,0xF4,0x2E);
delay(5);
registerRead(BMPAddress,0xF6);
ut = temp << 8 | temp;
/////////////////////////////////////////////////计算温度
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 = temperature;
atmo = pressure;
atmo = altitude;
}
运行结果
陀螺仪出现了问题，经过检查发现6个寄存器得到的结果是同一个数值，而其他的都正确。
为了验证是否是器件问题，用了另一个代码
#include <Wire.h> // IIC运行库
const unsigned char OSS = 0;// Oversampling Setting
floatp0=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; // 存放加速度偏移值(整型）
floatxAf,yAf,zAf; // 存放加速度值(浮点）
floatxGf, yGf, zGf; // 存放角速度值(浮点）
floatxMf,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();
}
这一次运行正常了，结果
搞不懂什么原因。求达人指导。 我发了好几个求助贴都没有解决 这应该不是陀螺仪的问题
看了你的代码
只见你把temp[]值赋给其他变量，没给temp[]更新啊？？
估计这就是问题 Malc 发表于 2012-9-12 23:45 static/image/common/back.gif
这应该不是陀螺仪的问题
看了你的代码
只见你把temp[]值赋给其他变量，没给temp[]更新啊？？
如果是因为temp［］没更新，那么后面的5883也应该有问题才对。而且我做了很多尝试，屏蔽了其他函数，同样陀螺仪读数不正常。就是四个传感器一起运行，我做过测试，每次temp［］付值前输出都是0。这个是基于C语言的，按照C语言的规则，函数调用变量后并不需要清零，再说了，读取寄存器时已经相当于一次重新付值了，现在的关键是六组数据在陀螺仪中是相同的，而且移动传感器寄存器值不变。
按照网上的另一种说法是陀螺仪寄存器不能用0x28,要用0xA8,最高位要是1寄存器指针才会自动加1,明天测试以下在看了。看了动力老男孩的代码，他用的是0xA8作为地址。 萧芸凤 发表于 2012-9-13 00:13 static/image/common/back.gif
如果是因为temp［］没更新，那么后面的5883也应该有问题才对。而且我做了很多尝试，屏蔽了其他函数，同样 ...
你看一下代码里面的registeRead（）
里面那段for里头temp=Wire.read();
这个应该是语法错误，编译无法通过的
查了l3g4200的datasheet，没有0xa8这个寄存器。。 http://www.geek-workshop.com/forum.php?mod=viewthread&tid=659
貌似有人曾经也遇到了类似问题，也是读取0xA8解决。。
这个以后再研究 Malc 发表于 2012-9-13 00:54 static/image/common/back.gif
你看一下代码里面的registeRead（）
里面那段for里头temp=Wire.read();
这个应该是语法错误，编译无法通 ...
解决了，要把0x28改成0xA8,4200中是没有这个寄存器，其实就是哪个数据寄存器28,最高位设1就成了A8了。 现在寄存器是输出了，但是，但是这些结果是什么意思，我是说是什么单位。
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 = atan2( magn , magn );
magnf = atan2( magn , magn );
magnf = atan2( magn , magn );
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;
}
代码写完了，输出的结果很奇怪，放在那里不动，角度一直在增加，而如果不断的移动增加的速度居然又跟不上了。主要是在陀螺仪上面，静止的时候有漂移，但要是运动起来输出又不足以让速度（角度） 变化起来。
还有一个问题，在做滤波融合的时候，加速的X轴计算结果应该是和陀螺仪的Y轴进行的吧，X轴移动的角度可以看作绕Y轴的旋转。不知道这样理解是否正确。
到目前为止还不知道磁场测出的结果有什么用。
同样的程序，今天运行后得到的海拔居然是－18米，昨天是24米，气压有变化，不知道是不是和今天杭州下雨有关。 陀螺仪有漂移是正常的，会导致角度一直增加，解决方法大概是用加速度传感器来修正，陀螺仪的动态响应不错，所以用来测试角速度和角加速度比较好。
建议查找一本书，两轮自平衡机器人原理，好像是这个书名，里面对陀螺仪和加速度计结合测试角度，角速度和角加速度有深刻的分析，还有各种参考系，还有个惯性导联系统之类，说的就是这些原理性的东西。 楼主发帖果然引出了不少高手啊 ，继续学习楼主的10轴为以后做参考 好资料 留个爪，以后再看
页:
[1]