STM32單片機：四旋翼飛行器的飛控實現

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

 

 

嘗試制作這個四旋翼飛控的過程，感觸頗多，整理了思緒之后，把重要的點一一記下來；

這個飛控是基於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;
}

 

　　四元數轉歐拉角的算法可參考 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html

 

4.獲取期望姿態；

　　也就是遙控部分了，讓用戶介入控制。

　　本着拿來主義的原則，用上”圓點博士開源項目”提供的安卓的開源藍牙控制端。

 

　　圓點博士給出了數據包格式，同過HC-06藍牙模塊接連到STM32串口1，再無線連接到控制端，這樣我們就可以獲得控制端不斷發送的數據包了，並實時更新期望姿態角，這里只需要注意輸出的姿態角和實時姿態角方向一致以及數據包的校驗就行了。

 

 

5.PID控制算法；

　　由於簡單的線性控制不可能滿足四軸飛行器這個靈敏的系統，引入PID控制器來更好的糾正系統。

　　簡介：PID實指“比例proportional”、“積分integral”、“微分derivative”，這三項構成PID基本要素。每一項完成不同任務，對系統功能產生不同的影響。

 

以Pitch為例：

 

　　

error為期望角減去實時角度得到的誤差；

iState為積分i參數對應累積過去時間里的誤差總和；

if語句限定iState范圍，繁殖修正過度；

微分d參數為當前姿態減去上次姿態，估算當前速度（瞬間速度）；

總調整量為p,i,d三者之和；

 

這樣，P代表控制系統的響應速度，越大，響應越快。

I，用來累積過去時間內的誤差，修正P無法達到的期望姿態值（靜差）；

D，加強對機體變化的快速響應，對P有抑制作用。

 

PID各參數的整定需要綜合考慮控制系統的各個方面，才能達到最佳效果。

 

 

輸出PWM信號：

PID計算完成之后，便可以通過STM32自帶的定時資源很容易的調制出四路pwm信號，采用的電調pwm格式為50Hz，高電平持續時間0.5ms-2.5ms；

我以1.0ms-2.0ms為每個電機的油門行程，這樣，1ms的寬度均勻的對應電調的從最低到最高轉速。

 

 

至此，一個用stm32和mpu6050搭建的飛控系統就算實現了。

 

剩下的調試PID參數了。

 

2014-04-23