使用通用MCU实现无人机飞行任务的快速二次开发

---TIDronePilot外部控制offboard模式介绍

无名小哥 2024年1月1日

传统飞控二次开发方法和主要存在的问题简介

通过对前面几讲中《零基础竞赛无人机积木式编程指南》系列开发教程的学习可知，在以往TI电赛真题的学习训练方案中飞行任务代码开发主要集中在Substask_Demo.c和Developer_Mode.c两个程序文件，其中在Substask_Demo.c内负责对具体飞行任务中每个阶段的无人机的飞行动作、航点位置、目标追踪、巡航速度、目标姿态、执行机构驱动(如蜂鸣器、激光笔、舵机、电机)等进行流程化的设计，Developer_Mode.c内用于实现自动起飞、降落、不同的飞行任务切换与执行。

尽管飞控代码内的相关的API函数接口已经实现得相当完备，但上述开发过程的对于初次接触整个飞控代码这一系统工程的学习者来讲，想要直接上手去二次开发仍然还有一定的难度，特别是在对飞控系统代码框架（硬件驱动层、传感器驱动、传感器滤波、姿态解算、惯性导航、机器视觉、基本飞行控制实现、API函数、导航控制、SDK自主任务等）没有整体的把握的情况下。

仅有C语言+单片机常用基础芯片资源使用知识的初学者按照提供的真题案例和二次开发教程“依葫芦画瓢”去实现特定任务往往容易会出现一些常识性错误，有些错误会直接导致整个飞控系统的“崩盘”。下面以实现无人机在前进的过程中搜寻到色块后，对色块进行跟踪这一任务为例子，将新手易错的问题整理如下:

不理解单片机通过定时器实现控制器需要周期性执行这一基本要求的重要性，如下图中在5ms周期性任务中引入了延时函数的飞行任务中，开发者OS是希望延时函数上方的速度控制执行10S，认为加了延时之后会速度控制函数就会一直执行。实则是程序会运行一次速度控制函数后，程序会在此处空转等待，后续的高度控制、姿态控制、PWM输出等得不到及时的执行，只有在10S等待空窗期后才会继续执行后续的控制，在10S等待时间内，飞控不会对自身的位置、速度、姿态进行即时高频（200Hz相对0.1Hz来讲）的调整与修正，无人机处于间歇性“失控”状态，飞行器的控制周期从5ms变成了10S，用此段代码去控制你的无人机，惨烈的炸机也就变得不可避免。上述问题的根源是延时函数破坏了控制器的周期性执行，连最基本的姿态控制都得不到保障，动摇了飞控系统的“国本”。
对飞控代码的执行流程与代码产生实际作用理解错乱，不会对任务按流程进行拆分，将部分重复作用的代码“杂乱堆砌”在一起，造成逻辑混乱，实际作用不明。如在下方代码中色块对准函数内部实现有光流速度控制函数，当视觉模块识别到了色块时，case 1内部的速度控制函数和色块控制内部的速度控制函数会顺序执行，即同一个控制周期5ms内，速度控制函数执行了两遍，第一次运行的控制器输出会被后续第二次运行的结果覆盖掉，似乎第一次可以视为“无用”代码，看似不影响最终的控制效果。但是事实是我们需要考虑到PID控制器的运算过程，同一个控制器一个周期内执行两遍，相当于积分I做了两次运算，微分项不起任何作用。因为两次计算过程中，当反馈和期望不变的情况下，第一次的运算过程的偏差和第二次运算的偏差相等，并且第一次的偏差赋值给了上次的偏差，即微分项会恒等于0，即不管微分项参数D为多少，最终计算出来的微分项结果恒等于0。在PID三个参数都存在的情况下，某个控制器重复执行亦会导致灾难性的BUG。
认为控制代码只需要执行一次就能达到期望的控制效果，如果从A飞到B，AB两点的距离为100cm，试想无人机需要在一个控制周期5ms内就能实现A到B，假设无人机做匀加速直线运动，零初始状态下无人机的加速度要到多少才能完成这一目标，无人机的实际推重比是否能达到这个要求？如果按照这个加速度进行加速无人机1S后是否会脱离地球轨道？
另外由于飞控自身硬件资源有限，飞控自身外界的外设占用比较多，可供用户它用的串口、PWM、IO资源不再富裕，使用起来使用捉襟见肘，常常需要外部MCU进行扩展，既然都引入了外部MCU到无人机平台中了，很自然的会想到用自己熟悉的MCU飞无人机飞行任务进行开发。
综上所述，针对新手初学者来讲，面对略显庞杂的飞控系统代码，在进行二次开发时，若写出某些天坑级的BUG会导致无人机系统的整体崩溃，基本的姿态自稳都得不到保障，就会出现灾难级的炸机事故。因此为了使得二次开发更加简单、开发更加安全，有必要将和用户二次开发相关的API函数、导航控制、SDK自主任务控制部分代码的实现，单独用一个通用控制器去实现，我们只需要在飞控端将已有的API函数接口进行简单整合，在SDK模式中新增加外部控制offboard模式就可以，这部分工作量很少，很多都是现成的我们在后面的教程中进行介绍。
TIVA飞控的硬件组成与系统框架
TIVA飞控硬件系统包括飞控主板与外设两部分组成，其中飞控主板上板载加速度计陀螺仪（带温控电路）、气压计传感器，带独立按键和五向按键，配备OLED显示屏进行人机交互，带蜂鸣器和RGB灯提示，板载的扩展接口包括8路串口、16路PWM、8个预留IO口、2组I2C口等。扩展接口用于外接机载端SLAM定位、offboard外部控制、光流、对地激光测距、GPS、机器视觉（如OPENMV/K210/树莓派OPENCV）、激光雷达点云数据等。
TIVA飞控软件系统包括芯片底层与应用相关驱动、传感器数据采集、传感器滤波、姿态解算、惯性导航、基础飞行控制、自主飞行API函数、SDK开发者模式等。
运用通用单片机开发板实现对飞控外部控制
在传统的开发模式中主要是在飞控代码内部对SDK开发模式中编写飞行任务代码去实现特定任务，这些开发流程在之前按的教程中有详细的阐明。本教程需要解决的问题是如何将飞控软件框架中的自主飞行相关API函数和SDK开发者模式的应用层开发代码放在外部的通用的MCU中去实现，比如盘古TI MCU系统板(已完成)、STM32F103系列核心板(已完成)、STC32G12K128开发板(完成度90%)等。由于外部通用MCU中需要实现的代码量很少，对单片机处理资源和性能要求并不高，因此用户完全可以用任何一款自己熟悉的单片机，高效率得心应手的去开发，参照上述已实现的三款核心板方案，去自己实现飞控的外部控制这部分设计。为了方便后面表述，我们将带有offboard外部控制的飞控称为TIDronePilot(简称TPT/下位机/飞控端)，将外部通用MCU开发板称为TIDronePlanner(简称TPR/上位机/应用端)。
TIDronePilot中offboard外部控制相关函数的实现
TPT的实现工作量很小，就是在保留原来TIVA飞控所有功能的前提（仍然可以用传统方案开发）下，主要新增加了一下三点：

TPT发送飞控内部估计出的三维位置、速度、加速度、角速度、姿态四元数、融合标志等信息给外部控制端，TPR端解析到上述反馈数据后用于对无人机实现位置、速度等控制，并将最终的控制量串口打包发送给飞控端。

TPT解析来自TPR端的姿态和竖直方向速度控制信息，作为offboard外部控制模式下对应项的期望值。

在SDK模式中额外新增加了offboard模式，在此模式下飞控端的姿态期望、竖直方向速度期望来源于外部控制器的给定（TPT串口解析来之TPR的控制指令），同时也可以通过遥控器手动介入接管无人机的控制权限，可以实现不切换遥控器开关挡位的情况下对无人机的直接控制。需要注意的是飞控端切入SDK后offboard_start_flag会置1，飞控端会将此标志为发送给外部控制端用于触发外部控制端执行其内部的SDK任务。

至此用于实现offboard外部控制飞控端的主要工作就全部实现了，单从控制部分抽象来看offboard模式下飞控端“退化”成只执行基本的飞行控制部分，主要包括姿态自稳、部分定高(竖直速度、加速度)功能，飞控内部并没有运行对无人机的位置、速度进行直接的控制的代码，相当于飞控在offboard外部控制模式下变成了一个“指哪打哪”、“一切行动听指挥”严格响应外部控制端TPR命令的被控对象。飞控负责保持自身姿态和竖直方向速度的稳定，其它应用层开发全部放在外部控制端TPR端去实现，新的开发模式下用户可以不用在飞控TPT端修改代码，把无人机作为一个整体被控对象去开发，自然也就不会出现第一节中天坑级BUG导致灾难性炸机的情况，在TPR端开发应用层代码不会影响飞控TPT端内部的控制，任何情况下遥控器都可以直接对无人机的控制进行接管。

TIDronePlanner中应用开发层相关函数的实现

TIDronePlanner作为飞控offboard外部控制模式下的上层控制器，其主要作用是实现原飞控中用于自主飞行相关API函数、SDK开发模式的功能，由于TPR应用端内部需要实现无人机水平方向的位置、速度控制，竖直方向的高度位置控制，自然需要飞控端TPT给应用端TPR发送自身的位置、速度、姿态等无人机飞行状态的反馈信息，TPR接收到反馈信息后用于无人机的位置、速度闭环控制，最终将TPR控制器的输出封装成相关API接口变量，通过串口通讯打包发给TPT飞控端，从而实现了整个飞控位置、速度、加速度、姿态角度、姿态角速度的系统的完整闭环控制。

在通用MCU系统板中实现TIDronePlanner的工作量同样很少，除了对应单片机资源如串口通讯、PWM输出、按键、显示屏、定时器调度驱动实现外，剩下的工作只剩下解析飞控端发送过来的状态反馈信息、移植原飞控端位置控制、速度控制、SDK开发模式、API接口函数、串口打包发送API接口变量。

解析来自飞控端的状态反馈数据，按照飞控发送时对应数据协议帧来解析。

实现位置控制、速度控制、SDK开发模式、API接口函数，由于这部分在飞控内部均有实现，直接移植过来就可以，此处不作过多介绍，在之前的教程中有对相关函数实现进行讲解。

这里需要注意的是虽然Auto_Flight_Ctrl函数在定时器内周期性执行，但SDK任务执行与否取决于offboard_start_flag变量的值，当用遥控器/ADC按键操作飞控端切入offboard模式后，此标志位会被置1，Auto_Flight_Ctrl中的自主飞行任务才会执行。

串口打包发送API接口变量

TIDronePlanner中实现部分还包括外设如舵机/电机、激光笔、蜂鸣器等设备的驱动，辅助传感器如激光雷达点云、机器视觉等数据的接入，具体根据实际飞行任务要求来，由于TIDronePlanner的实现仅需要占用一个串口资源就可以实现其应用层，因此通用MCU核心板的其它预留出来的资源就可以供二次开发使用，此举能有效解决传统开发方案中，资源不够用的情况。

基于TIDronePilot和TIDronePlanner分层式开发Q&A

上位机和下位机通过串口通讯的方式进行数据交互，数据的实时性如何，如何保证二者通讯时数据的可靠性？

这里我们看下用于offboard外部控制的通讯串口波特率为2250000bps，传输带个字节数据包括1个起始位、1个停止位、8个数据位共占用10个二进制位，因此传输单个字节时间开销为1/225000秒，即约为4.4us，可知传输100个字节时，时间开销为约为0.44ms，实际当前传输带宽为68个字节，约为0.3ms。
串口通讯时为了确保串口不丢帧，需要考虑优先级高于当前串口中断的任务的最大执行时间+串口中断函数执行时间，务必小于串口接收单个字节所需的时间，才能确保不丢帧。
1、多个串口通讯时，串口通讯波特率可以降低一点。
2、合计设计优先级，当存在不同波特率通讯时，通讯波特率高的串口中断优先级要高于波特率低的。
3、存在优先级高于串口中断的其它中断任务时，其它中断任务的总的最大时间开销也要考虑。

在上位机端开发应用层代码的情况下，是不是可以完全不用管飞控端、机载计算端的代码实现？

新增offboard外部控制模式的主要目的是简化开发过程，让新手能更容易上手在飞控应用层做二次开发，无需管飞控整个飞控系统代码。因此用户可以完全不应管飞控各系统模块的内部具体的实现过程，新手用户把飞控当作“盲盒”也能实现二次开发，就像之前的教程中把机载端当作一个能提供室内高精度位姿数据的“GPS传感器”一样，只需要知道如何安装、接线、设置、操控就可以。

文档中是以室内激光雷达SLAM定位为例的，如果我需要在室外实现相关应用层的二次开发，还能按照上述方式来开发吗？另外外部通用MCU系统板能用树莓派等机载端去实现吗？

能，只需要修改飞控端发送的位置、速度数据融合来源就可以，比如在室外GPS定位下，飞控在给外部应用层发反馈状态数据时，将内部GPS融合得到的位置、速度数据替换掉原来SLAM定位融合的数据，原来外部控制器的开发过程不需要做任何调整，同理UWB定位模式下也也是这个处理思路。
对于通用MCU的要求比较小，用树莓派ROS端去做开发当然可以，同时如果所使用的单片机波特率到不了2250000bps，可以适当调小通讯波特率，但建议不要低于921600。

TIDronePilot和TIDronePlanner配合使用时的具体操作参见配套的视频教程...