STM32单片机：四旋翼飞行器的飞控实现

只看该作者 · 2019-6-17 16:11

-------------------------------------------------------------------------------------------------------------------

尝试制作这个四旋翼飞控的过程，感触颇多，整理了思绪之后，把重要的点一一记下来；

这个飞控是基于STM32，整合了MPU6050，即陀螺仪和重力加速计，但没有融合电子罗盘；

另外，四旋翼飞行器的运动方式请百度百科，不太复杂，具体不再赘述；

这是飞控程序的控制流程（一个执行周期）：

比较重要的地方：

1.i2c通信方式；

　　因为我不是学电类专业，最开始对i2c这些是没有一点概念，最后通过Google了解了一些原理，然后发现STM32的开发库是带有i2c通信的相关函数的，但是我最后还是没有用这些函数。

我通过GPIO模拟i2c,这样也能获得mpu6050的数据，虽然代码多了一些，但是比较好的理解i2c的原理。

　　STM32库实现的模拟i2c代码（注释好像因为编码问题跪了）：

复制

/*******************************************************************************

// file                :    i2c_conf.h

// MCU                : STM32F103VET6

// IDE                : Keil uVision4

// date                £º2014.2.28

*******************************************************************************/

#include "stm32f10x.h"



#define   uchar unsigned char

#define   uint unsigned int



#define      FALSE 0  

#define   TRUE  1



void I2C_GPIO_Config(void);

void I2C_delay(void);

void delay5ms(void);

int I2C_Start(void);

void I2C_Stop(void);

void I2C_Ack(void);

void I2C_NoAck(void);

int I2C_WaitAck(void);

void I2C_SendByte(u8 SendByte);

unsigned char I2C_RadeByte(void);

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

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

复制

/*******************************************************************************

// file                :  i2c_conf.c

// MCU                : STM32F103VET6

// IDE                : Keil uVision4

// date                £º2014.2.28

*******************************************************************************/



#include "i2c_conf.h"



#define SCL_H         GPIOB->BSRR = GPIO_Pin_6       

#define SCL_L         GPIOB->BRR  = GPIO_Pin_6         

#define SDA_H         GPIOB->BSRR = GPIO_Pin_7

#define SDA_L         GPIOB->BRR  = GPIO_Pin_7



#define SCL_read      GPIOB->IDR  & GPIO_Pin_6        //IDR:¶Ë¿ÚÊäÈë¼Ä´æÆ÷¡£

#define SDA_read      GPIOB->IDR  & GPIO_Pin_7



                     





void I2C_GPIO_Config(void)

{

  GPIO_InitTypeDef  GPIO_InitStructure; 

 

  GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_6;

  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;   //¿ªÂ©Êä³öÄ£Ê½

  GPIO_Init(GPIOB, &GPIO_InitStructure);



  GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_7;

  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;

  GPIO_Init(GPIOB, &GPIO_InitStructure);

}





void I2C_delay(void)

{

        

   int i=6; //ÕâÀï¿ÉÒÔÓÅ»¯ËÙ¶È    £¬¾²âÊÔ×îµÍµ½5»¹ÄÜÐ´Èë

   while(i) 

   { 

     i--; 

   }  

}



void delay5ms(void)

{

        

   int i=5000;  

   while(i) 

   { 

     i--; 

   }  

}





int I2C_Start(void)

{

    SDA_H;                                            //II2ÐÒé¹æ¶¨±ØÐëÔÚÊ±ÖÓÏßÎªµÍµçÆ½µÄÇ°ÌáÏÂ£¬²Å¿ÉÒÔÈÃ Êý¾ÝÏßÐÅºÅ¸Ä±ä

    SCL_H;

    I2C_delay();

    

    if(!SDA_read)

        return FALSE;                //SDAÏßÎªµÍµçÆ½Ôò×ÜÏßÃ¦,ÍË³ö

    SDA_L;                                

    

    I2C_delay();

    

    if(SDA_read) 

        return FALSE;                //SDAÏßÎª¸ßµçÆ½Ôò×ÜÏß³ö´í,ÍË³ö

    SDA_L;                                

    

    I2C_delay();                    

    

    return TRUE;

}







void I2C_Stop(void)

{

    SCL_L;

    I2C_delay();

    SDA_L;

    I2C_delay();

    

    SCL_H;

    I2C_delay();

    SDA_H;

    I2C_delay();

} 





void I2C_Ack(void)

{    

    SCL_L;

    I2C_delay();

    SDA_L;

    I2C_delay();

    SCL_H;

    I2C_delay();

    SCL_L;

    I2C_delay();

}   





void I2C_NoAck(void)

{    

    SCL_L;

    I2C_delay();

    SDA_H;

    I2C_delay();

    SCL_H;

    I2C_delay();

    SCL_L;

    I2C_delay();

} 







int I2C_WaitAck(void)      //·µ»ØÎª:=1ÓÐACK,  =0ÎÞACK

{

    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) //Êý¾Ý´Ó¸ßÎ»µ½µÍÎ»//

{

    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)  //Êý¾Ý´Ó¸ßÎ»µ½µÍÎ»//

{ 

    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; //Ð´Èë

      }

    }

    SCL_L;

    return ReceiveByte;

} 







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

{

      if(!I2C_Start())

            return FALSE;

    I2C_SendByte(SlaveAddress);   //·¢ËÍÉè±¸µØÖ·+Ð´ÐÅºÅ    //I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);    //ÉèÖÃ¸ßÆðÊ¼µØÖ·+Æ÷¼þµØÖ· 

    

        if(!I2C_WaitAck())

        {

            I2C_Stop(); 

            return FALSE;

        }

    

        I2C_SendByte(REG_Address );   //ÉèÖÃµÍÆðÊ¼µØÖ·      

    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);//ÉèÖÃ¸ßÆðÊ¼µØÖ·+Æ÷¼þµØÖ· 

    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大大为开源做出的贡献。

复制

#define sampleFreq    512.0f        // sample frequency in Hz

#define betaDef        0.1f        // 2 * proportional gain





volatile float beta = betaDef;                                

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



float invSqrt(float x);



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

    float recipNorm;

    float s0, s1, s2, s3;

    float qDot1, qDot2, qDot3, qDot4;

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



    // Rate of change of quaternion from gyroscope

    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);

    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);

    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);

    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);



    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {



        // Normalise accelerometer measurement

        recipNorm = invSqrt(ax * ax + ay * ay + az * az);

        ax *= recipNorm;

        ay *= recipNorm;

        az *= recipNorm;   



        // Auxiliary variables to avoid repeated arithmetic

        _2q0 = 2.0f * q0;

        _2q1 = 2.0f * q1;

        _2q2 = 2.0f * q2;

        _2q3 = 2.0f * q3;

        _4q0 = 4.0f * q0;

        _4q1 = 4.0f * q1;

        _4q2 = 4.0f * q2;

        _8q1 = 8.0f * q1;

        _8q2 = 8.0f * q2;

        q0q0 = q0 * q0;

        q1q1 = q1 * q1;

        q2q2 = q2 * q2;

        q3q3 = q3 * q3;



        // Gradient decent algorithm corrective step

        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;

        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;

        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;

        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;

        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude

        s0 *= recipNorm;

        s1 *= recipNorm;

        s2 *= recipNorm;

        s3 *= recipNorm;



        // Apply feedback step

        qDot1 -= beta * s0;

        qDot2 -= beta * s1;

        qDot3 -= beta * s2;

        qDot4 -= beta * s3;

    }



    // Integrate rate of change of quaternion to yield quaternion

    q0 += qDot1 * (1.0f / sampleFreq);

    q1 += qDot2 * (1.0f / sampleFreq);

    q2 += qDot3 * (1.0f / sampleFreq);

    q3 += qDot4 * (1.0f / sampleFreq);



    // Normalise quaternion

    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

    q0 *= recipNorm;

    q1 *= recipNorm;

    q2 *= recipNorm;

    q3 *= recipNorm;

}





float invSqrt(float x) {

    float halfx = 0.5f * x;

    float y = x;

    long i = *(long*)&y;

    i = 0x5f3759df - (i>>1);

    y = *(float*)&i;

    y = y * (1.5f - (halfx * y * y));

    return y;

}