我的四翼飞行器开始了，论坛上东平西凑的找到了许多代码研究，有些在单一模块上可以正常运行，但是到了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 18:39:35
楼主
