開源三軸雲台EVVGC（simple BGC）分析

一． 主程序分析

主程序結構清晰，流程如圖所示，下面將對每個部分做詳細分析

 

二. 系統初始化

系統初始化部分的流程如上圖所示，下面對每部分做具體分析

 

1. 時鍾初始化

 該部分主要是使能DWT，用DWT進行精確延時，沒有使用systick進行延時是因為systick作為時基用來確定各任務的運行頻率

 

2. 初始化各參數到EEPROM

 系統使用FLASH的最后一頁模擬EEPROM來存儲參數

// use the last KB for sensor config storage
#define FLASH_WRITE_EEPROM_CONFIG_ADDR  (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1))

將各參數初始化，然后寫入flash，寫入flash步驟如下：

①FLASH解鎖

②清零EOP、PGERR、WRPRTERR標志位

③進行擦除

④如果擦除完成就進行編程

⑤FLASH上鎖

⑥讀取FLASH

 

3. 電機驅動初始化

該函數一開始就檢查一個標志位（全局靜態變量），該標志位默認清零，在本函數執行完后置位，確保本函數只執行一次。

該部分用到了二個高級定時器TIM1和TIM8，還有二個通用定時器TIM4和TIM5，因此把定時器部分的初始化函數獨立出來做成二個函數，分別是高級定時器初始化和通用定時器初始化，中斷配置部分也獨立出來，用了很貼近庫函數的寫法，值得借鑒。

TIM1用於pitch pwm timer，TIM8用於roll pwm timer，TIM4和TIM5用於yaw pwm timer，在中斷配置的時候，yaw pwm timer配置的是TIM5，而在配置計數初值的時候，配置的是TIM4。

TIM8->CNT = timer4timer5deadTimeDelay + 5 + PWM_PERIOD * 2 / 3;

TIM1->CNT = timer4timer5deadTimeDelay + 3 + PWM_PERIOD / 3;

TIM4->CNT = timer4timer5deadTimeDelay;

通過配置TIM4、TIM5成相反極性，加上計數初值，在中斷中變更比較值的時候，給其中一個加上死區時間，模擬成了互補PWM

 

4. 延時

 延時使用了DWT的計數，DWT的初始配置在時鍾配置的時候已經完成，us延時函數如下，值得學習

void delayMicroseconds(uint32_t us)

{
    uint32_t elapsed = 0;

    uint32_t lastCount = *DWT_CYCCNT;

    for (;;)

    {

        register uint32_t current_count = *DWT_CYCCNT;

        uint32_t elapsed_us;
 

        // measure the time elapsed since the last time we checked

        elapsed += current_count - lastCount;

        lastCount = current_count;

        // convert to microseconds

        elapsed_us = elapsed / usTicks;
 

        if (elapsed_us >= us)

            break;
 

        // reduce the delay by the elapsed time

        us -= elapsed_us;

 
        // keep fractional microseconds for the next iteration

        elapsed %= usTicks;

    }

}

 

5. 打印內部還有3s的延時，加上之前之后的延時，至少有23s的延時，該部分延時是為了讓傳感器穩定下來

 

6. RC初始化部分配置外部中斷3、4、5和TIM3

 

7. 時間函數初始化部分配置TIM6，向上計數，在最大中斷的時候溢出，用來對主函數中任務的執行間隔時間進行計時，該計時用於積分運算

 

8. 一階濾波初始化

// TAU = Filter Time Constant

// T   = Filter Sample Time


// A   = 2 * TAU / T


// Low Pass:

// GX1 = 1 / (1 + A)

// GX2 = 1 / (1 + A)

// GX3 = (1 - A) / (1 + A)

 
// High Pass:

// GX1 =  A / (1 + A)

// GX2 = -A / (1 + A)

// GX3 = (1 - A) / (1 + A)

初始化的參數有ACCEL_X_500HZ_LOWPASS、ACCEL_Y_500HZ_LOWPASS、ACCEL_Z_500HZ_LOWPASS、ROLL_RATE_POINTING_50HZ_LOWPASS、PITCH_RATE_POINTING_50HZ_LOWPASS、YAW_RATE_POINTING_50HZ_LOWPASS、ROLL_ATT_POINTING_50HZ_LOWPASS、PITCH_ATT_POINTING_50HZ_LOWPASS、YAW_ATT_POINTING_50HZ_LOWPASS等，初始賦值都是下面模式：

a = 2.0f * eepromConfig.accelX500HzLowPassTau * 500.0f;


firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx1 = 1.0f / (1.0f + a);

firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx2 = 1.0f / (1.0f + a);

firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx3 = (1.0f - a) / (1.0f + a);

firstOrderFilters[ACCEL_X_500HZ_LOWPASS].previousInput  = 0.0f;

firstOrderFilters[ACCEL_X_500HZ_LOWPASS].previousOutput = 0.0f;

 

濾波函數如下：

output = filterParameters->gx1 * input +

filterParameters->gx2 * filterParameters->previousInput -

filterParameters->gx3 * filterParameters->previousOutput;

 

9. PID初始化，注意該部分初始化了rc：

 rc = 1.0f / ( TWO_PI * F_CUT );

 

10. 初始化正弦表，該部分初始化了一個1024個點的正弦表，對每個點求正弦值后再轉化為Q15格式

void initSinArray(void)

{

    int i;

    for (i = 0; i < SINARRAYSIZE; i++)

    {

        float x = i * M_TWOPI / SINARRAYSIZE;

        sinDataI16[i] = (short int)round(sinf(x) * SINARRAYSCALE);

    }

}

 

11. 初始化IMU方向矩陣：根據IMU的安裝位置確定方向矩陣

 

12. MPU6050初始化，通過重寫I2C write函數（三個參數：地址、寄存器、數據），發送指令對6050進行初始化 ，初始化的步驟如下：

①設備復位

②延時150ms，等待穩定

③設置時鍾源

④關閉待機模式，使能加速度和陀螺儀

⑤加速度、陀螺儀采樣頻率設定

⑥濾波設定

⑦加速度量程設定

⑧陀螺儀量程設定

 

在這個初始化函數里，配置完MPU6050后，調用了一個計算MPU6050數據的函數，該函數對加速度和陀螺儀的5000次數據求平均（讀取出加速度和陀螺儀數據，然后與方向矩陣相乘，再用轉化后的數據減去溫度偏差），最后再對加速度進行標准化，這里面有幾個注意的地方：

① 讀取數據的時候，MPU6050先發送高位再發送低位，讀取的時候，數據保存到一個數組緩沖區，然后利用聯合體共享內存的特性將高低位合並成一個數據

② 讀取出來的數據與方向矩陣（在前面初始化的時候已經確定）相乘轉化為旋轉后的數據

③ 計算MPU6050偏差：偏差 = 溫度漂移速率*溫度值+偏差截距

溫度漂移速率和偏差截距在MPU6050校准函數（該函數在串口校准命令下被執行）中計算，該函數先采集2000次MPU6050的數據求均值，等待10分鍾，然后再次采集2000次MPU6050的數據求均值，最后計算溫度漂移速率與偏差截距：

溫度漂移速率 = （第二次均值 - 第一次均值）/（第二次溫度均值 - 第一次溫度均值）

偏差截距 = 第二次均值 - 溫度漂移速率 * 第二次溫度均值

④ 對5000次數據求平均中，每次都需要轉化后的數據減去偏差

 

三． 原點初始化

求取150次角度均值，角度求取使用反正切計算

 

四． 主函數里500Hz執行部分是雲台控制核心部分

1. 讀取TIM6的計數值，重置TIM6，計算兩次運行的間隔時間，用於后面的角度積分

 

2. 讀取陀螺儀和加速度計數值，進行標定（分別減去偏差值）

 

3. 獲取原點位置

該部分先用進行標定后的加速度值進行反正切運算求解出角度，之后用此角度值與先前計算出的角度進行一階滯后濾波，再用互補濾波融合陀螺儀積分出來的角度和加速度滯后濾波后的角度，該角度用於串口通訊輸出

 

4. 對加速度數據進行一階濾波

 

5. 姿態更新（2ms進行一次）

該部分讀取加速度計和陀螺儀的值，進行標定（減去零點偏移值），對加速度值再進行一階濾波，之后傳輸給航姿更新模塊進行姿態解算，姿態解算過程詳見“姿態解算解析”

 

6. 電機驅動

該部分用解算出的姿態角與遙控解析出的角度進行PID運算（2ms進行一次），PID運算的結果傳遞給電機驅動模塊，電機驅動模塊詳見開環雲台電機驅動，電機占空比更新頻率與本部分同頻，也是2ms一次

 

五． 串口通訊部分

該部分使用USB虛擬串口，通過接收串口命令做出不同處理，將相關聯動作的命令用大小寫分別定義，減少了命令復雜度，可對應命令寫上位機處理

 

六． 遙控命令處理

該部分接收三個通道的信號（分別對應三軸），然后與一個閾值做比較，如果小於閾值則認為沒有控制量，如果大於閾值，則根據信號量的大小計算控制量，之后將控制量乘以一個比例因子，使得與限制量在一個量級，再之后判斷控制方向和約束可控范圍，根據判斷選擇是否進行角度補償，最后再進行一階濾波