开源三轴云台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虚拟串口，通过接收串口命令做出不同处理，将相关联动作的命令用大小写分别定义，减少了命令复杂度，可对应命令写上位机处理

六． 遥控命令处理

该部分接收三个通道的信号（分别对应三轴），然后与一个阈值做比较，如果小于阈值则认为没有控制量，如果大于阈值，则根据信号量的大小计算控制量，之后将控制量乘以一个比例因子，使得与限制量在一个量级，再之后判断控制方向和约束可控范围，根据判断选择是否进行角度补偿，最后再进行一阶滤波