一. 主程序分析

主程序结构清晰,流程如图所示,下面将对每个部分做详细分析

二. 系统初始化

系统初始化部分的流程如上图所示,下面对每部分做具体分析

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

六. 遥控命令处理

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

开源三轴云台EVVGC(simple BGC)分析的更多相关文章

  1. STorM32 BGC三轴云台控制板电机驱动电路设计(驱动芯片DRV8313)

    1  序言 相信对云台有兴趣的小伙伴对STorM32 BGC这块云台控制板并不陌生,虽说这块控制板的软件已经不再开源,但是在GitHub上依旧可以找到两三个版本的代码,而硬件呢我们也可以从Olliw( ...

  2. STorM32 BGC三轴增稳云台驱动下载

    STorM32 BGC是一种硬件开源.软件闭源的三轴稳定云台控制项目.云台在我们生活中是越来越常见,我们手机拍照用的手持云台,无人机上挂载摄像机的机载隔振云台.我们在电影<流浪地球>里面那 ...

  3. [算法][三轴、六轴、九轴传感器算法分析] 1、分享一个三轴加速计matlab动态可视化脚本

    一.有啥用 这里用的是LIS3DH三轴加速计,输出为X.Y.Z轴的加速度,通过串口连接电脑,电脑里运行matlab脚本通过串口实时获取数据并做可视化显示. 这里虽然是针对LIS3DH的,其实稍作修改即 ...

  4. 精通Web Analytics 2.0 (5) 第三章:点击流分析的奇妙世界:指标

    精通Web Analytics 2.0 : 用户中心科学与在线统计艺术 第三章:点击流分析的奇妙世界:指标 新的Web Analytics 2.0心态:搞定它.新的闪亮系列工具:是的.准备好了吗?当然 ...

  5. Arduino I2C + 三轴加速度计LIS3DH

    LIS3DH是ST公司生产的MEMS三轴加速度计芯片,实现运动传感的功能.主要特性有: 宽工作电压范围:1.71 ~ 3.6V 功耗:低功耗模式2μA:正常工作模式.ODR = 50Hz时功耗11μA ...

  6. 第三部分 MediaPlayer的主要实现分析

    第三部分 MediaPlayer的主要实现分析 3.1 JAVA程序部分    在packages/apps/Music/src/com/android/music/目录的MediaPlaybackS ...

  7. Facebook 开源三款图像识别人工智能软件

    Facebook今天开源了三款人工智能图像分割(Image Segmentation)软件,分别是DeepMask.SharpMask和MultiPathNet,三款工具相互配合完成一个完整的图像识别 ...

  8. 三次握手wireshark抓包分析,成功握手和失败握手

    启动 点击start出现下面的对话框 wireshark是捕获机器上的 某一块网卡的网络包,当机器上有多块网卡的时候,需要选择一个网卡进行捕获操作. 选择网卡 >主页面上,直接点击选中后star ...

  9. 单片机实验: 三轴磁场模块 GY-271

    最近买了一块三轴磁场模块进行实验 名称:HMC5883L模块(三轴磁场模块) 型号:GY-271 使用芯片:HMC5883L 供电电源:3-5v 通信方式:IIC通信协议 测量范围:±1.3-8 高斯 ...

随机推荐

  1. Note - Powerful Number

    Powerful Number   对于 \(n\in\mathbb N_+\),若不存在素数 \(p\) 使得 \(p\mid n~\land~p^2\not\mid n\),则称 \(n\) 为 ...

  2. AI 神经网络学习

    神经网络学习 1.输出与输入的关系(感知基): $$ y=\begin{Bmatrix} 1 & {\overrightarrow{x}\cdot \overrightarrow{w}+b&g ...

  3. C++奇异递归模板模式

    虚函数的问题 虚函数的主要问题是性能开销比较大,一个虚函数调用可能需要花费数倍于非虚函数调用的时间,尤其是当非虚函数被声明为inline时(注意,虚函数不能被内联). CRTP介绍 CRTP的全称是C ...

  4. zabbix-agentd;客户端开启多个端口。

    学习标杆:https://access.redhat.com/documentation/zh-cn/red_hat_enterprise_linux/8/html/configuring_basic ...

  5. Bypass BeaconEye - Beacon 堆混淆

    这是[信安成长计划]的第 9 篇文章 关注微信公众号[信安成长计划] 0x00 目录 0x01 CS4.5 Sleep_Mask 0x02 HeapEncrypt 0x03 效果 0x04 参考文章 ...

  6. AWVS漏洞扫描教程之扫描方式

    实验目的 掌握AWVS的基本用法. 实验原理 Acunetix Web Vulnerability Scanner是一个网站及服务器漏洞扫描软件. 实验内容 对Web站点进行安全测试. 实验环境描述 ...

  7. 力扣第二题 大数相加 ,链表在python到底该怎么写?

    但问题在于链表的表示  如何创建一个L3呢 如何用next将他们连接起来呢? 原来是采用 制作链表的形式 l3_pointer.next = ListNode(l1_pointer.val + l2_ ...

  8. 华为HCIP实验--OSPF单区域

    场景:你是公司的网络管理员.现在公司的网络中有三台ARG3路由器,通过以太网实现相互的连通.在以太网这样的广播式多路访问网络上,可能存在安全隐患,所有你选择采用OSPF区域认证的方法来避免恶意的路由攻 ...

  9. C#特性(属性)Attribute

    先明确一个概念: 元数据..NET中元数据是指程序集中的命名空间.类.方法.属性等信息.这些信息是可以通过Reflection读取出来的. 再来看个例子: #define BUG //#define ...

  10. oracle 12c RAC 重启

    转至:https://blog.csdn.net/weixin_40283570/article/details/81511072 关闭顺序 :关闭PDB----->关闭数据库------> ...