五、无人机知识笔记（中级：控制原理与程序实现）
笔记来源于：沈阳无距科技-工业级无人机的中国名片（编程外星人）
目录
一、飞控整体架构
一、数据存储（Storage）
二、外部通讯（External Connectivity）
三、驱动程序（Drivers）
四、飞行控制（Flight Control）
六、消息总线（Message Bus）
二、编译选项与模块功能说明
三、uORB原理与使用
四、系统级驱动程序开发
五、应用级驱动程序开发
六、多进程、多线程与工作队列原理
七、创建并运行多进程程序
八、创建并运行多线程程序
九、工作队列
十、遥控器协议S-BUS、PWM与PPM协议
十一、命令响应与状态切换
一、飞控整体架构
PX4的设计从大角度划分为五个部分：
- 数据存储（Storage）
- 外部通讯（External Connectivity）
- 驱动程序（Drivers）
- 飞行控制（Flight Control）
- 消息总线（Message Bus）
一、数据存储（Storage）
数据存储分为3个部分：
- 飞行任务存储部分（dataman）：dataman给上层应用提供了可以随时存储指定飞行任务的通用函数，上层应用可以通过调用dm_write和dm_read函数来读写飞行任务，通常用于读写飞行航迹点的坐标。保存的目标通常在sdcard中。
- 飞机参数存储部分（param）：飞控程序中定义了大量的飞行参数，例如，与串级PID控制参数就有二十多个，用于其它功能的参数还有很多，程序中提供了param_get和param_set函数可以将这些参数存放到flash和sdcard中。
- 飞行日志存储部分（logger）：飞控程序中提供了两种飞行日志的记录方式（二选一），早期的日志记录方式为sdlog2，可以将需要记录的数据用字符方式存储到sdcard中的csv格式文件中，飞行结束后可以通过其它的软件程序来查看。近期的飞控程序推荐使用logger来记录飞行日志，它可以将飞控程序中的uORB记录下来并保存。
二、外部通讯（External Connectivity）
飞控程序的外部通讯采用的是Mavlink协议，Mavlink是无人机外部通讯的一个轻量级通讯协议。
与外部通讯即是与计算机上的地面站程序来进行通讯。我们可以使用Mavlink官方提供的协议生成工具来定制我们自己需要的协议内容，并可以通过需要生成不同编程语言所使用的源文件，例如C/C++、Java、Python等。这样的好处是同样的一个通讯协议可以被飞控程序、PC计算机、手机APP程序和其它程序同时使用。这样就有效的避免了不同运行平台上，不同类型程序运行的差异，而采用相同的协议通讯。
三、驱动程序（Drivers）
驱动程序是飞控程序中采集传感器数据的源头，常用的传感器驱动程序有：
- 相机驱动：通常可以控制相机的拍照功能，并可以对其指定拍照参数，例如：光圈、快门、感光度、焦距、白平衡、曝光补偿等等。这些控制功能主要取决于相机的功能。对于最初级拍照相机来说，飞控程序只需要对其发送“拍照”命令即可。
- GPS驱动：全球定位系统（Global Positioning System）的驱动程序。在前面系列中我们已经讲述过，在室外的自动飞行过程中无人机需要执行定点飞行模式，然而定点飞行模式的前提就是要有GPS数据对IMU的积分数据进行修正，所以GPS是自动飞行功能的基础，它通常有ashtech、mtk和u-blox等。驱动程序中会根据当前插入的GPS协议进行自动切换并解析。
- 遥控器驱动：目前常见的遥控器通讯协议为SBUS和PPM，就实用性来说SBUS通讯协议可能更高一点，但其从电路上需要接入一个反向器（Pixhawk上已经加入了反向器），通常采用串口来传输SBUS协议，串口波特率100k，其支持16个遥控器通道，而每一个通道的数值采用11个bit位来表示即11*16=176字节。
- 云台驱动：云台通常安装在多旋翼机架正下方，在云台上挂载相机或其它设备。云台通常有自己的软件程序，可以很好的达到自平衡的状态。也就是说，当飞机产生俯仰或滚转角度时，云台会自动修正其三轴（简易云台通常只有两个转动轴）舵机的角度从而自动恢复平衡。而飞控程序也可以通过向云台发送相关指令控制其当前俯仰、滚转、航向角度。
- 空速计、数传、光流、距离传感器等：这些传感器驱动并不是在所有的飞机上都要使用，通常根据实际需要来使用其中的一个或几个。空速计可以测量当前空气流动速度；数传支持无线传输设备的数据发送与接收；光流定位系统驱动可以检测当前移动速度与位置；距离传感器可以检测传感器前方物体与其之间的距离，例如：超声波、雷达、双目摄像头、激光测距仪等等。
- IMU传感器驱动：IMU传感器是飞控驱动程序中最重要的部分，其通过初始化、校准、读取加速计、陀螺仪、磁罗盘这些传感器，为上层状态融合和导航系统提供最基本的数据支撑。加速计是检测飞机当前三轴的加速度，陀螺仪可以检测飞机的旋转角速度，而为了消除陀螺仪的航向积分误差，需要使用磁罗盘对其做航向修正。IMU的数据采集的频率通常都高于其它的驱动程序，通常是250Hz。
四、飞行控制（Flight Control）
- 传感器数据集成（Sensors Hub）：sensors模块收取了所有传感器的数据，包括加速计、陀螺仪、磁罗盘、空速计、高度计、电量计等等传感器的读数，并对这些数据进行初步过滤与处理。之后将处理后的数据向ekf2（姿态位置估计）模块发送。
- 姿态位置估计（Position Attitude Estimator）：ekf2中主要对传感器的数据进行融合，并采用卡尔曼滤波对其做进一步的处理，进而对飞机当前的姿态和位置做出估计。
- 状态机（State Machine）：commander处理飞机当前状态（正常、异常）和用户命令。在多种状态下切换。例如：飞控锁定/解锁、飞机模式切换、安全保护启动与关闭、启动传感器或遥控器校准功能等等。
- 自动飞行（Autonomous Flight）：navigator配合dataman中存储的任务信息进行自动飞行功能，给定飞机当前的飞行目标点（将位置期望传递给位置控制模块），并以指定高度和航向飞行，到达目标点执行特定任务（例如：拍照、鸣笛等），并继续向下一点飞行。
- 位置控制（Position Controller）：串级PID反馈控制，外环PID将当前期望位置输出为期望速度，内环PID根据期望速度计算出期望姿态，输出给姿态控制模块。
- 姿态控制（Attitude Rate）：串级PID反馈控制，外环PID将当前期望状态输出为期望角速度，内环PID根据期望角速度计算出期望控制量，输出给控制输出模块。
- 控制输出（Output Driver）：得到姿态控制最终输出的控制量，根据机型不同机型（四轴、六轴、八轴等）Mixer混控文件，输出实际多路控制PWM信号，输出到电调中，实现电机的控制。
六、消息总线（Message Bus）
飞控程序中内部消息传递采用的是异步消息机制uORB。它的设计理念很有趣，它可以实现不同模块中的数据快速通讯，并且以异步通讯为基本原则，也就是说在通讯过程中发送者只负责发送数据，而并不关心数据由谁接收，也不关心接收者是否能将所有的数据都接收到；而对于接收者来说并不关心数据是由谁发送的，也不关心在接收过程中是否将所有数据都接收到。
二、编译选项与模块功能说明
先来看看整体目录结构，然后我们会针对主要的功能模块说一些说明（由于篇幅限制本节所列出的目录与文件都是源代码中一少部分，实际上很多目录与文件夹都被省略了）：
我自己也试着把文件的目录生成出来，但是还是太多了，所以还是用了网站的这个目录来学习
三、uORB原理与使用
uORB在数据发布与接收过程中并不保证发送者的所有数据都可以被接收者收到，而只保证接收者在想要接收时能收到最新的数据。而发送与接收的分离可以使飞程中各个模块相互独立，互不干扰。实际上一个uORB可以由多个发送者发布，也可以被多个接收者接收。也就是说他们之间是多对多的关系。
下面我们来看一个具体的例子：
1.在msg文件夹中创建一个叫做extctl_sp.msg的文件，表示我们创建了一个新的uORB，它的名字叫作extctl_sp，其内容如下：
这是作者在做“外部控制”模式下所用到的一个uORB，在本节中读者可以不用了解其中变量的具体含义，只知道我们需要在extctl_sp.msg中定义一些指定属性即可。不过需要注意其定义的语法与C/C++类似但有些不同，参考PX4中现有的msg文件内容即可。
2.在msg文件夹中的CMakeLists.txt中添加刚刚我们添加的extctl_sp.msg，表示对其做编译处理：
即程序在编译时会根据extctl_sp.msg生成extctl_sp.h和extctl_sp.cpp文件，也就是我们在程序中所用到的结构定义：
3.发布者发布uORB时需要做两步操作：
(1)公告/多重公告uORB；
(2)发布uORB。
我们来看下面的的发布者的例子：
4.接收者接收uORB时需要做三步操作：
(1)订阅uORB；
(2)判断uORB数据是否有更新。
(3)复制uORB数据内容到本地内存。
我们来看下面的的者接收的例子
这就是uORB的使用方法，注意，发布和订阅只需要执行一次即可，而发布、检查更新和接收可以执行多次。下面我们来看看uORB中比较关键的几个函数的定义：
关于这些函数的用法、参数及反回值不做过多的说明，这些函数的参数和反回值源代码中有详细的说明，还是比较容易理解和使用的。
四、系统级驱动程序开发
这部分涉及c/c++，以及linux系统相关的知识，我现在还有很大的欠缺，先放着先......
连接在这里：无人机中级篇：第四讲：系统级驱动程序开发
五、应用级驱动程序开发
这部分涉及c/c++，以及linux系统相关的知识，我现在还有很大的欠缺，先放着先......哈哈哈哈哈......
连接在这里：无人机中级篇：第五讲：应用级驱动程序开发
六、多进程、多线程与工作队列原理
- 单核单线程处理器处理多线程：时间片轮转方式（现代操作系统有着很多任务调度策略。例如：多级反馈工作队列、实时优先级调度、非实时可变优先级调度等等）这个学大机基的时候学过诶。
- 进程：操作系统执行计算机程序的基本单元。注意：是操作系统执行程序的基本单元，而不是调度的基本单元。每一个进程都有一个独立的任务控制块PCB（Processing Control Block）和任务ID（task id），而且它们在操作系统是“并行”的。实际上对于操作系统来说，每一个进程都是一个调度的对象，操作系统会按照一定的调度策略使它们能够合理的执行。而对于不同进程来说，它们的内存数据段资源是不同的。
- 线程：允许在一个进程中可以创建多个可并行的调度单元。“线程”机制可以使一个进程在同一时刻中可以执行多个任务。
- 多个进程在操作系统中运行的过程通常被称为“多进程”，一个进程中的多个线程运行的过程通常被称为“多线程。
- 工作队列：在操作系统创建进程和线程时，需要为其分配PCB进而方便对其做调度，但是都要消耗掉一些额外的内存空间。对于嵌入式的操作系统来说，内存资源非常有限，所以为了节省内存的消耗，操作系统通常都会为用户提供一种叫做工作队列的机制。会按顺序执行所有需要执行的函数，功能函数排列的顺序是按用户加入队列时所指定的时间间隔顺序，通常是按操作系统调度的TICK作为单位。
七、创建并运行多进程程序
无人机中级篇：第七讲：创建并运行多进程程序
八、创建并运行多线程程序
无人机中级篇：第八讲：创建并运行多线程程序
九、工作队列
无人机中级篇：第九讲：工作队列
十、遥控器协议S-BUS、PWM与PPM协议
- S-BUS其实是一种串口通信协议，采用100000的波特率，数据位点8bits，停止位点2bits，偶效验，即8E2的串口通信。但是S-BUS采用的是反向电平传输，也就是说，在S-BUS的发送端高低电平是反向的，协议中的所有高电平都被转换成低电平，协议中的所有低电平都被转换成高电平。所以在S-BUS的接收端需要增加一个高低电平反向器来进行电平反转。实际上，我们使用的Pixhawk飞控板上已经集成了这个反向器，所以对于使用Pixhawk的用户来说，可以忽略掉S-BUS的反向机制，但是对于其它没有集成S-BUS反向器的硬件平台上，就需要使用者增加一个反向器来处理数据，否则将无法读取协议数据。
- PWM：传统的遥控器接收机是采用多路PWM的方式进行输出的，遥控器中有多少个通道，接收机中就有多少路PWM输出。我们需要使用飞控程序来读取接收机输出的PWM信号值。在STM32系列中，我们可以为GPIO引脚设置成PWM采集模式，并通过采集寄存器来读取当前PWM的周期和PWM占空比。
-
PPM编码是PWM的扩展协议，可以在一个管脚中传输多路PWM信号。PPM的频率通常是50Hz，周期长度20ms，每一个周期中可以存放最多10路PWM信号，每一路PWM的周期为2ms。
十一、命令响应与状态切换
在PX4中负责状态切换的模块为commander，在src/modules/commander文件夹下有很多文件，他们分别负责不同的功能，例如：主状态和导航状态切换、传感器校准、命令响应等等。我们主要来了解commander.cpp中命令响应与状态切换的相关内容。
在int commander_main(int argc, char *argv[])函数中定义了所有可执行的命令选项，我们可以根据其useage来对这个功能做一些简单的了解：
usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}
- 启动/停止进程：start/stop/status这3个命令主要功能是在nsh脚本里将commander线程启动/停止/查看状态。当执行commander start后，系统调用Commnader::run()函数，并为此函数创建一个叫做commander的进程，同时Commnader::run()函数中创建了一个叫做commander_low_prio的低优先级线程。在执行commander stop会终止commander进程和commander_low_prio线程。
- 其中显示飞控的运动类型（type）、安全开关状态（safety）、电源状态（power state valid）、起飞点坐标（home position）全局坐标系和本地坐标系、数传状态（datalink）、主状态（main state）、导航状态（nav state）、锁定状态（arming）等等。
- 传感器校准：calibrate选项可以对传感器进行校准，校准选项有4个，分别为accel、gyro、mag、level、airspeed。可以分别执行下面命令来对加速计、陀螺仪、磁罗盘、水平仪和空速计做校准。
commander calibrate accel
commander calibrate gyro
commander calibrate mag
commander calibrate level
commander calibrate airspeed
-
行前检查：check选项可以针对飞控当前的状态来进行行前检查，并显示当前飞控的状态是否达到飞行的条件
-
锁定/解锁与起降命令：arm/disarm/takeoff/land/transition这5个选项可以让飞控通过commander命令来使飞控直接完成相应功能：
commander arm：飞控解锁
commander disarm：飞控锁定
commander takeoff：起飞
commander land：着陆
commander transition：模态切换（倾转旋翼使用）
-
主状态模式：mode选项中有13个选项，这13个选项就是飞控的13个飞行模式，如下：
manual：MAIN_STATE_MANUAL：手动模式
altctl：MAIN_STATE_ALTCTL：定高模式
posctl：MAIN_STATE_POSCTL：位置模式
auto:mission：MAIN_STATE_AUTO_MISSION：任务模式
auto:loiter：MAIN_STATE_AUTO_LOITER：悬停模式
auto:rtl：MAIN_STATE_AUTO_RTL：返航模式
acro：MAIN_STATE_ACRO：特级模式
offboard：:MAIN_STATE_OFFBOARD：离线模式
stabilized：MAIN_STATE_STAB：增稳模式
rattitude：MAIN_STATE_RATTITUDE：姿态特级模式
auto:takeoff：MAIN_STATE_AUTO_TAKEOFF：自动起飞
auto:land：MAIN_STATE_AUTO_LAND：自动降落
flowtarget：MAIN_STATE_FLOW_TARGET：跟随模式
-
Commander::run()函数中定义了一个while循环结构，这个循环结构在commander启动后就一直在运行，直到commander停止后才结束。