MiniFly微型四轴开发学习日志（一）——MiniFly 微型四轴软件原理

文章目录

微型四轴技术参数

NRF51822 程序框架

 

STM32F411 程序框架

 

姿态解算和 PID 算法总体流程

 

---

 

 

微型四轴技术参数

 Minifly微型四轴采用双MCU，即STM32F411与NRF51822双核运行，其技术参数如下。

 

 NRF51822 程序框架

 

如图为NRF51822的代码关系图 。其中button.c为按键驱动代码，uart.c为串口驱动代码，pm.c为电源管理驱动代码，systick.c为系统滴答定时器驱动。NRF51822的工作流程如下：

（1）遥控器通过radiolink.c传送数据包到main.c；

（2）main.c对数据包进行解析，判断该数据包的流向；

（3）如果该数据包是传送给NRF51822的，则直接传送；

（4）如果不是传送给NRF51822，即传送给STM32F411，于是通过uartlink.c传送给STM32F411；

（5）STM32F411接收到数据包之后会通过uartlink.c返回一个数据包到main.c;

  (6)  最后main.c将返回的数据包通过radiolink.返回到遥控器当中。

以上六步形成了一个数据包传送的闭环，可以看出NRF51822在其中起到了通信桥梁的作用。

当四轴处于关机的时候，NRF51822处于待机状态，STM32F411处于断电状态。当按下按键，NRF51822被唤醒，它会判断按下按键的时间：

（1）如果是短按，则启动STM到固件模式；

（2）如果是长按，则NRF51822拉高UAR_RTS_PIN引脚，使能电源芯片给STM供电。（STM32F411会在上电后运行bootloader程序，会判断到该引脚是否为高电平，是即留在该程序中等待固件升级，否即跳转会（1），当NRF51822运行程序拉高引脚后，会以一定周期时间获取pm.c采集到的电池电压值并转发给STM)

如此，NRF51822起到了管理管理电源的作用。

STM32F411 程序框架

（1）radiolinkTask ： 无线通信任务。该任务主要负责接收从 NRF51822 发送（串口方式）过来的数据，然后对数据进行打包和校验，打包成 ATKP 格式并校验无误后发送到 atkpRxAnlTask 的接收队列里，

同 时回传一帧数据给 NRF51822 。

 

（2）usblinkRxTask ： USB 通信接收任务。该任务主要负责接收上位机发下来（ USB 虚拟串 口方式）数据，然后对数据进行打包和校验，打包成 ATKP 格式并校验无误后发送到 atkpRxAnlTask 接收队列里。

 

（3）atkpRxAnlTask ： ATKP 数据包接收处理任务。该任务主要是处理遥控器和上位机发下 来的数据包，解析到的控制指令则发送到 stabilizerTask 中去。

 

（4）stabilizerTask ： 四轴平衡控制任务。该任务运行的内容比较多，也是比较关键的内容。 包括传感器数据读取，数据融合，获取控制数据，空翻检测，异常检测， PID 控制， PWM 输出控制等。

 

（5）wifilinkTask ： 手机控制任务。该任务主要是接收 WiFi 摄像头模块的串口数据，然后按 照 WiFi 摄像头模块通讯协议解析成对应的控制指令，并将控制指令发送到 stabilizerTask 。

 

（6）atkpTxTask ： ATKP 数据包发送任务。该任务主要是获取 stabilizerTask 中的传感器数据、 姿态数据、电机 PWM 输出数据等数据以定周期发送给 radiolinkTask 和 usblinkTxTask ，由这 两个任务分别发送

到遥控器和上位机。

 

（7）usblinkTxTask ： USB 通信发送任务。该任务主要负责发送 atkpTxTask 发送过来的数据 包，这些数据包主要是传感器数据、姿态数据等。

 

总结就是：

（1）将NRF51822发送的串口数据打包为ATKP形式并校验，发送到（3）→（2）将上位机发送的USB数据打包为ATKP形式并校验发送到（3 ）→（3）解析前二者的数据包发送到（4）→（5）将WIFI摄像头模块的串口数据按协议发送给（4）→（4）将（3）（5）数据结合进行四轴平衡任务→（6）获取（4）当中的相关数据发送给（1）（7）→（1）将数据发送回NRF51822（串口）→ （7）将数据发送会上位机（USB）

 

姿态解算和 PID 算法总体流程

（1）关于姿态解算，采用的是互补滤波算法。每隔T=0.2ms（500Hz），MCU通过IIC读取加速计数据以及读取陀螺仪数据，并对加速计数据进行IIR低通滤波，对陀螺仪数据进行偏置调整，结合之后得到了姿态数据roll/pitch/yaw，即角度测量值。

（2）角度环PID，同样T=0.2ms，其角度期望值来自WIFI或者遥控RC，测量值来自（1），将二者相减之后得到偏差角度，作为角度换PID控制器输入，进行PID校正之后得到期望角速度。

（3）角速度环PID，同样T=0.2ms，期望角速度来自（2），角速度测量值即（1）中陀螺仪数据偏置调整之后的数值。二者相减之后得到偏差角速度，作为角速度环PID控制器输入，校正后得输出姿态控制量。

（4）Z轴高度PID控制器，其T=0.4ms。首先，在minifly四轴当中由手动和定高两个模式，当处于手动模式下，设定油门值=实际油门值。而当处于定高模式，油门控制就会切换到Z轴速度模式。此时，将读取高精度气压计BMP280数据，并对其进行IIR滤波、限幅均值滤波；并从姿态测量值当中获取去除重力后的Z轴加速度。二者融合得到高度估计值。另一方面，高度期望值即油门转换为Z轴设定速度值的积分，二者相减之后得到高度偏差值，作为Z轴高度PID控制器输入，校正后得到油门控制的变化量，再加上油门基准值即实际油门值输出。

（5）输出的实际油门值以及姿态控制量结合进行算法运算之后得到四个参数，以此用于调节四轴电机的PWM，进行姿态矫正。