请教L3G4200D+ADXL345姿态测量问题
按照弘毅前辈的代码: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;
readfrom(ACCEL_ADDRESS,0x32,0x06,buff);
accel.x = (((int) buff) << 8) | buff;// X axis (internal sensor y axis)
accel.y = (((int) buff) << 8) | buff;// Y axis (internal sensor x axis)
accel.z = (((int) buff) << 8) | buff;// Z axis (internal sensor z axis)
}
void Magn_Init()
{
writeTo(MAGN_ADDRESS, 0x02, 0x00); // 连续测量
}
void Read_Magn()
{
int i = 0;
byte buff;
readfrom(MAGN_ADDRESS,0x03,0x06,buff);
// MSB byte first, then LSB; Y and Z reversed: X, Z, Y
magnetom.x =((((int) buff) << 8) | buff);// X axis (internal sensor -y axis)
magnetom.y = -1 * ((((int) buff) << 8) | buff);// Y axis (internal sensor -x axis)
magnetom.z = -1 * ((((int) buff) << 8) | buff);// 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;
readfrom(GYRO_ADDRESS,0xA8,0x08,buff);
gyro.x =((((int) buff) << 8) | buff); // X axis (internal sensor -y axis)
gyro.y =((((int) buff) << 8) | buff); // Y axis (internal sensor -x axis)
gyro.z = ((((int) buff) << 8) | buff); // Z axis (internal sensor -z axis)
tgyro=buff;
}
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();
}
页:
[1]