请教L3G4200D+ADXL345姿态测量问题

hymeeg

按照弘毅前辈的代码:http://www.geek-workshop.com/forum.php?mod=viewthread&tid=236

我把3205改成4200，能正常显示姿态，但有个问题，就是纵向快速改变模块姿态的时候，显示角度会瞬间反向增大，然后才正常显示。横向显示却正常。

模块初始化和读取代码如下：
/* This file is part of the Razor AHRS Firmware */
#include <Wire.h>  // 调用I2C库
// Sensor I2C addresses
#define ACCEL_ADDRESS ((int) 0x53) // 0x53 = 0xA6 / 2
#define MAGN_ADDRESS  ((int) 0x1E) // 0x1E = 0x3C / 2
#define GYRO_ADDRESS  ((int) 0x69) // 0x68 = 0xD2 / 2

void I2C_Init()
{
  Wire.begin();
}

void Accel_Init()
{
  writeTo(ACCEL_ADDRESS, 0x2D, 0x08);  
  writeTo(ACCEL_ADDRESS, 0x31, 0x0b);  
  writeTo(ACCEL_ADDRESS, 0x2c, 0x09);  
}

// Reads x, y and z accelerometer registers
void Read_Accel()
{
    int i = 0;
    byte buff[6];
    readfrom(ACCEL_ADDRESS,0x32,0x06,buff);  
    accel.x = (((int) buff[1]) << 8) | buff[0];  // X axis (internal sensor y axis)
    accel.y = (((int) buff[3]) << 8) | buff[2];  // Y axis (internal sensor x axis)
    accel.z = (((int) buff[5]) << 8) | buff[4];  // Z axis (internal sensor z axis)
}

void Magn_Init()
{
  writeTo(MAGN_ADDRESS, 0x02, 0x00);   // 连续测量
}

void Read_Magn()
{
    int i = 0;
    byte buff[6];
    readfrom(MAGN_ADDRESS,0x03,0x06,buff);
    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
    magnetom.x =  ((((int) buff[1]) << 8) | buff[0]);  // X axis (internal sensor -y axis)
    magnetom.y = -1 * ((((int) buff[5]) << 8) | buff[4]);  // Y axis (internal sensor -x axis)
    magnetom.z = -1 * ((((int) buff[3]) << 8) | buff[2]);  // Z axis (internal sensor -z axis)
}

void Gyro_Init()
{
    writeTo(GYRO_ADDRESS, 0x20, 0xaf);   // 设置睡眠模式、x, y, z轴使能
    writeTo(GYRO_ADDRESS, 0x21, 0x00);   // 选择高通滤波模式和高通截止频率
    writeTo(GYRO_ADDRESS, 0x22, 0x00);   // 设置中断模式
    writeTo(GYRO_ADDRESS, 0x23, 0x30);   // 设置量程(2000dps)、自检状态、SPI模式
    writeTo(GYRO_ADDRESS, 0x24, 0x02);   // FIFO & 高通滤波  // 配置L3G4200D(2000 deg/sec)

}

// Reads x, y and z gyroscope registers
void Read_Gyro()
{
  int i = 0;
  byte buff[8];
  readfrom(GYRO_ADDRESS,0xA8,0x08,buff);
  gyro.x =  ((((int) buff[3]) << 8) | buff[2]);    // X axis (internal sensor -y axis)
  gyro.y =  ((((int) buff[5]) << 8) | buff[4]);    // Y axis (internal sensor -x axis)
  gyro.z = ((((int) buff[7]) << 8) | buff[6]);    // Z axis (internal sensor -z axis)
  tgyro=buff[0];
}

void writeTo(int DEVICE, byte address, byte val) {
  Wire.beginTransmission(DEVICE); //传送到加速度传感器
  Wire.write(address);        // 发送寄存器地址
  Wire.write(val);        // 发送要写入的值
  Wire.endTransmission(); //结束传输
}

void readfrom(byte DEVIES,byte address,byte num,byte buff[])
{
     int i=0;
      Wire.beginTransmission(DEVIES);
      Wire.write(address);  // Sends address to read from
      Wire.endTransmission();
      
      Wire.beginTransmission(DEVIES);
      Wire.requestFrom(DEVIES, num);  // Request 6 bytes
      while(Wire.available())  // ((Wire.available())&&(i<6))
      {
        buff = Wire.read();  // Read one byte
        i++;
      }
      Wire.endTransmission();  
}