地球角速度

#include

#defineAcc0x1D

#defineGyr0x69

#defineMag0x1E

#defineGry_offt-13//陀螺仪偏移量

#defineGyr_Gain0.07//满量程2000dps时灵敏度(dps/digital)

#definepi3.14159

floatangleG;

unsignedlongtimer=0;//采样时间

voidtup(){

nsor_init();//配置传感器

(19200);//开启串口以便监视数据

delay(1000);

}

voidloop(){

longo_timer=timer;//上一次采样时间(ms)

floatY_Accelerometer=gDat(Acc,1);//获取向前的加速度

floatZ_Accelerometer=gDat(Acc,2);//获取向下的加速度

floatangleA=atan(Y_Accelerometer/Z_Accelerometer)*180/pi;

//根据加速度分量得到的角度(degree)

timer=millis();//当前时间(ms)

intdt=timer-o_timer;//微分时间

angleG=angleG+Gyr_Gain*(gDat(Gyr,0)+Gry_offt)*dt/1000;

//对角速度积分得到的角度(degree)

(timer);

(",");

(angleA,6);

(",");

(angleG,6);

(";");//输出数据

delay(10);

}

intgDat(intdevice,intaxis){

//读九轴姿态传感器寄存器函数

//ForArduino,by黑马

//调用参数表

//typedeviceaxis

//012

//ADXL345Accxyz

//L3G4200DGyrxyz

//HMC5883LMagxzy

//Example

//00#include

//01#defineAcc0x1D;

//02#defineGyr0x69;

//03#defineMag0x1E;

//04

//05voidtup(){

//06nsor_init();

//07delay(1000);

//08}

//09

//10voidloop(){

//11intZ-Gyroscope;

//12Z-Gyroscope=gDat(Gyr,2);

//13delay(50);

//14}

intv;

bytevL,vH,address;//存放byte数值

if(device==Acc)address=0x32;//ADXL345的读数地址

if(device==Gyr)address=0xA8;//L3G4200D的读数地址

if(device==Mag)address=0x03;//HMC5883L的读数地址

address=address+axis*2;//数据偏移-坐标轴

ransmission(device);//开始传输数据

(address);//发送指针

tFrom(device,2);//请求2byte数据

while(ble()<2);//成功获取前等待

vL=e();

vH=e();//读取数据

nsmission();//结束传输

if(device==Mag)v=(vL<<8)|vH;

elv=(vH<<8)|vL;//将byte数据合并为Int

returnv;//返回读书值

}

voidnsor_init(){//配置九轴姿态传感器

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

//配置ADXL345

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

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

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

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

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

//配置L3G4200D(2000deg/c)

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

//配置HMC5883L

}

voidwriteRegister(intdevice,byteaddress,byteval){//写寄存器

ransmission(device);

(address);

(val);

nsmission();

}