四旋翼飞行器的飞控实现

SCL_L;

I2C_delay();

SDA_H;

I2C_delay();

SCL_H;

I2C_delay();

SCL_L;

I2C_delay();

}

int I2C_WaitAck(void) //·μ???a:=1óDACK, =0?TACK

{

SCL_L;

I2C_delay();

SDA_H;

I2C_delay();

SCL_H;

I2C_delay();

if(SDA_read)

{

SCL_L;

I2C_delay();

return FALSE;

}

SCL_L;

I2C_delay();

return TRUE;

}

void I2C_SendByte(u8 SendByte) //êy?Y′ó????μ?μí??//

{

u8 i=8;

while(i--)

{

SCL_L;

I2C_delay();

if(SendByte&0x80) // 0x80 = 1000 0000;

SDA_H;

else

SDA_L;

SendByte<<=1; // SendByte×óò?ò????￡

I2C_delay();

SCL_H;

I2C_delay();

}

SCL_L;

}

unsigned char I2C_RadeByte(void) //êy?Y′ó????μ?μí??//

{

u8 i=8;

u8 ReceiveByte=0;

SDA_H;

while(i--)

{

ReceiveByte<<=1; //×óò?ò???￡?

SCL_L;

I2C_delay();

SCL_H;

I2C_delay();

if(SDA_read)

{

ReceiveByte"=0x01; //D′è?

}

}

SCL_L;

return ReceiveByte;

}

int Single_Write(uchar SlaveAddress,uchar REG_Address,uchar REG_data)

{

if(!I2C_Start())

return FALSE;

I2C_SendByte(SlaveAddress); //·￠?íéè±?μ??·+D′D?o? //I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE); //éè?????eê?μ??·+?÷?tμ??·

if(!I2C_WaitAck())

{

I2C_Stop();

return FALSE;

}

I2C_SendByte(REG_Address ); //éè??μí?eê?μ??·

I2C_WaitAck();

I2C_SendByte(REG_data);

I2C_WaitAck();

I2C_Stop();

delay5ms();

return TRUE;

}

unsigned char Single_Read(unsigned char SlaveAddress,unsigned char REG_Address)

{

unsigned char REG_data;

if(!I2C_Start())

return FALSE;

I2C_SendByte(SlaveAddress); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//éè?????eê?μ??·+?÷?tμ??·

if(!I2C_WaitAck())

{

I2C_Stop();

return FALSE;

}

I2C_SendByte((u8) REG_Address); 

I2C_WaitAck();

I2C_Start();

I2C_SendByte(SlaveAddress+1);

I2C_WaitAck();

REG_data= I2C_RadeByte();

I2C_NoAck();

I2C_Stop();

return REG_data;

}

2.mpu6050;

然后用写好的模拟i2c函数读取mpu6050，根据mpu6050手册的各寄存器地址，读取到了重力加速计和陀螺仪的各分量;

传感器采样率设置为200Hz,这个值是因为我电调频率为200Hz，也就是说，我的程序循环一次0.005s,一般来说，采样率高点没问题，别比执行一次闭环控制的周期长就行了;

陀螺仪量程±2000°/s，加速计量程±2g， 量程越大，取值越不精确;

这里注意，由于我们没有采用磁力计，而陀螺仪存在零偏，所以最终在yaw方向上没有绝对的参考系，不能建立绝对的地理坐标系，这样最好的结果也仅仅是在yaw上存在缓慢漂移。

3.互补滤波;

融合时，陀螺仪的积分运算很大程度上决定了飞行器的瞬时运动情况，而重力加速计通过长时间的累积不断矫正陀螺仪产生的误差，最终得到准确的机体姿态。

这里我们采用Madgwick提供的UpdateIMU算法来得到姿态角所对应的四元数，之后只需要经过简单运算便可转换为实时欧拉角。感谢Madgwick大大为开源做出的贡献。

1 #define sampleFreq 512.0f // sample frequency in Hz

2 #define betaDef 0.1f // 2 * proportional gain

3

4

5 volatile float beta = betaDef;

6 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;

7

8 float invSqrt(float x);

9

10 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {

11 float recipNorm;

12 float s0, s1, s2, s3;

13 float qDot1, qDot2, qDot3, qDot4;

14 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;