Mcontroller-v7 控制器
Mcontroller 系统架构
• 总体架构
Mcontroller 系统由主控层、中间层、系统层和硬件层四部分构成，如下图所示：
• FreeRTOS
FreeRTOS是实时操作系统，负责管理Mcontroller的多线程调度。
Mcontroller的所有线程调度程序都在工程的/Core/Src/freertos.c文件中。主控层、中间层中的所有函数必须加载到freertos.c的线程调度函数中才能运行。
• Main controller
Main controller是系统的核心控制层，采用C风格的Cpp语言编写。
所有的上层控制函数都在Maincontroller/src文件夹中，可以直接在该文件夹中新增自己的上层控制算法，实现自定义功能。
• 软件系统框图
Mcontroller 首次使用指南——Mcontroller 说明书
Mcontroller 说明书包括Mcontroller接口说明、关于引脚标号的进一步说明、Mcontroller的校准以及Wi-Fi模块的连接说明。
关于Mcontroller更多的使用细节以及拿到Mcontroller后的使用流程，请参照下一节 Mcontroller 使用步骤。
1、Mcontroller接口说明
Mcontroller的接口说明如下图所示，其中黄铜色的小正方形表示控制器接口的插针位置，插针旁边的标识符号与插针一一对应，表示插针的含义。
2、关于引脚标号的进一步说明
结合上图，对Mcontroller引脚标号的进一步说明如下所示。特别注意，Mcontroller的所有同名引脚是连通的，Mcontroller的对外供电电流最大为1A，因此全部接插在Mcontroller上的设备额定电流总和不应超过1A。
3、Mcontroller指示灯说明
Mcontroller板载指示灯共8个，分别为LED0~LED7，如下图所示：
如果用户希望自定义LED灯的闪烁，请参照开发文档——板载硬件资源的使用——LED灯的开发。
Mcontroller 首次使用指南——Mcontroller 使用步骤
Mcontroller 使用步骤分为以下15步，从 Mcontroller供电 开始，到 着手开发 结束。
现在，仔细阅读下述的使用步骤，准备开启一场 Mcontroller 的开发之旅吧！
(一) Mcontroller 供电
Mcontroller 供电电压范围为5v-7.5v，可采用USB供电和插针供电。当成功供电后，Mcontroller的电源指示灯(红灯)会点亮。
1、USB供电
USB供电线直接接插在Micro-USB接口处。
2、插针引脚供电
Mcontroller电源输入口与APM、PX4等开源飞控的电源模块兼容，并且支持更宽的电压输入范围。
电源模块通过电源转接线(包装附赠)接插在Mcontroller 电源输入口引脚处，如下图所示：
(二) 下载 Mcontroller App
(三) 连接Wi-Fi模块
Mcontroller控制器目前支持两种幻思创新Fancinnov官方Wi-Fi模块——Mlink-video WiFi摄像头模组和Mlink-esp WiFi模块，连接步骤点击查看。
(四) 熟悉 App首界面操作
App首界面操作说明点击查看。
实际开发中，机器人类型和电机类型请根据自己的需求仔细选择，若勾选项和实际类型不匹配，将影响Mcontroller的正常工作。
(五) 熟悉 App主界面操作
App主界面操作说明点击查看。
实际开发中，Mcontroller App起着关键作用，请认真查看。
(六) 熟悉 App设置界面操作
App设置界面操作说明点击查看。
实际开发中，Mcontroller App起着关键作用，请认真查看。
(七) 水平校准
Mcontroller控制器在使用前需要借助Mcontroller App进行水平校准。
【关于加速度计校准】Mcontroller控制器在出厂前已完成加速计校准，一般情况下，用户无需再对加速度计进行校准。但是，当Mcontroller控制器受到猛烈撞击，可能导致Mcontroller 飞控的加速度计受损或者失调，进而引起姿态控制不准确，飞行不稳，这种情况下需要对加速度计进行校准。点击查看加速度计校准
【关于磁力计校准】Mcontroller控制器在出厂前已完成磁力计校准，一般情况下，用户无需再对磁力计进行校准。但是，当Mcontroller控制器受到强电磁干扰时，导致磁力计读数发生变化，影响其精度和稳定性，进而引起飞行不稳、定位误差较大，这种情况下需要对磁力计进行校准。另外，由于不同地区的地磁场不同，如果对磁力计工作精度要求更高，对飞机的定位精度要求更高，也可以进行磁力计校准。点击查看磁力计校准
特别注意，磁力计校准不良可能导致多旋翼悬停时在特定区域盘旋或者自旋并快速飞向某一个方向，因此校准时请务必按照步骤一步步进行操作，确保磁力计校准成功。磁力计一旦进行校准，如若校准不成功，严禁Mcontroller控制器起飞！
(八) 遥控器连接与校准（若不连遥控器请忽略此步）
遥控器以常用的乐迪AT9S为例，接收机以常用的乐迪R9DS和R6DSM为例。
1、遥控器通道说明
以左手油门为例，通道1——通道4的说明如下图所示，遥控器面板上控制通道5——通道8的按钮，可参照遥控器使用说明进行设置和调整。
2、接收机连接
乐迪R9DS接收机与Mcontroller的连接如下图所示。注意，需要将接收机的信号工作模式设置为SBUS信号模式,具体的操作步骤请参照乐迪官方说明。
乐迪R6DSM接收机与Mcontroller的连接如下图所示。注意，需要将接收机的信号工作模式设置为SBUS信号模式,具体的操作步骤请参照乐迪官方说明。
3、遥控器校准
在用遥控器进行遥控操作前，需要借助Mcontroller App对遥控器进行校准。 注意，在进行遥控器校准前，需要配置channel5的数值，使Mcontroller进入自稳模式，请点击查看关于Mcontroller模式切换的说明。Mcontroller进入自稳模式后，才可以对遥控器进行校准，请点击查看具体的校准步骤。
4、遥控器操控说明
以乐迪AT9S遥控器（左手油门、油门不自动回中）操控四旋翼无人机为例。
（1）操控前。在Mcontroller App首界面中选择四旋翼模式（点击查看操作说明），选择完成后，Mcontroller会记住该选择。
特别注意，在App和遥控器同时连接的情况下，还需要在App摇杆设置中关闭摇杆（点击查看操作说明），这样Mcontroller才会听从遥控器的控制。
同理，如果使用Mcontroller开发无人车，则选择无人车模式。
（2）起飞。
第一步，使用遥控器对Mcontroller进行硬件解锁和软件解锁，操作说明请参照（九）；
第二步，缓慢向上推动油门摇杆，飞机起飞后，将摇杆回中，完成起飞。
特别说明，油门死区默认为0.4-0.6（油门摇杆推到最下方为0，回中为0.5，推到最上方为1）。在定高模式和定位模式下（请点击查看关于Mcontroller模式切换的说明），无人机在油门死区内处于悬停状态；在自稳模式下，不存在油门死区，油门是多少对应到无人机的实际数值就为多少。
（3）飞行中。
在操控无人机上、下、前、后、左、右运动以及转向时，每次推动摇杆后，需要及时将摇杆回中。若不回中，无人机会持续向摇杆控制的方向运动。
（4）降落。
缓慢向下推动油门摇杆直到无人机着陆。无人机着陆后，长按Channel6通道的按钮即可锁定Mcontroller。
(九) Mcontroller 解锁与锁定
特别注意：
（1）Mcontroller解锁分为两层。先是硬件解锁（外层），再是手势解锁（内层）;
（2）硬件解锁后，不会启动电机；手势解锁完成后，电机将启动，请小心操作。
Mcontroller控制器的解锁和锁定取决于Channel6通道的数值。
Mcontroller 解锁与锁定方式有两种，一种是通过Mcontroller App ，另一种是通过遥控器。
1、Mcontroller App 解锁与锁定
2、遥控器解锁与锁定
遥控器解锁步骤分以下三步：
第一步，硬件解锁。操作口诀——“点击解锁"。点击Channel6通道的按钮，此时电机锁定指示灯(控制器右侧LED红灯)熄灭，电机解锁指示灯(控制器右侧LED绿灯)点亮；
第二步，手势解锁。以乐迪AT9S遥控器左手油门为例，同时将油门摇杆推到最下方，偏航摇杆推到右方，持续3s左右。
第三步，解锁完成。当Mcontroller 左侧LED绿灯按照一定频率闪烁，并伴随着由蜂鸣器发出的声音时，控制器解锁完成。
遥控器锁定步骤只有一步：
操作口诀——“长按锁定"。长按Channel6通道的按钮即可锁定Mcontroller。当控制器右侧LED绿灯熄灭、LED红灯点亮，控制器锁定完成。
(十) 电机连接（若使用无刷电机还需要连接电调，请参照(十一)）
特别注意以下两点：
（1）机头或者车头要和Mcontroller 控制器的头(复位键所在位置)保持一致；
（2）请再三核对电机转向和螺旋桨安装是否正确。
1、四旋翼无人机
对于四旋翼无人机，电机与Mcontroller的连接说明如下图所示：
2、八旋翼无人机
对于八旋翼无人机，电机与Mcontroller的连接说明如下图所示：
3、四轮无人车
对于四轮无人车，电机与Mcontroller的连接说明如下图所示：
(十一) 电调连接与校准（若不使用无刷电机请忽略此步）
特别注意，此步骤完成后，若想对无人机进行试飞，请参照（十四）进行调参。请勿在不调参的情况下，直接起飞无人机。
初步调参完成后，方可对无人机进行试飞。试飞环境请选择空旷区域，请小心操作，避免发生危险。
1、电调连接
电调与无刷电机连接后，再通过电调转接线(包装附赠)将电调与Mcontroller 电机引脚(M1-M8)连接，连接方式和（十）相同，如下图所示。
以其中一个电调为例，电调的白线和黑线分别连接到电调转接线3P胶壳的白线和黑线，电调转接线2P胶壳的白线和黑线分别连接到Mcontroller的M1引脚和M1引脚旁边的Gnd引脚。其他电调的连接同理。
2、电调校准
点击视频，查看电调校准指南：
(十二) Mcontroller 日志查看
Mcontroller可借助SD卡很方便地对日志进行记录，日志查看步骤点击查看。
(十三) 机器人搭建
动手搭建你的机器人吧！
特别说明，在用Mcontroller 构建无人机时，务必在Mcontroller 下面垫减震板以过滤震动。减震板如下图所示：
(十四) 调试参数（若不调参请忽略此步）
无人机在起飞前需要借助Mcontroller App对Mcontroller进行调参, 具体的调参步骤请点击查看 。
以F450机架为例,需要将App参数列表，按照下图所示的数值进行配置。
第一张参数列表截图标注的姿态外环参数、姿态内环参数、位置外环参数和位置内环参数，可参照下方控制系统框图进行理解：
第二张参数列表截图标注的高度环参数、垂直速度环参数和垂直加速度环参数可参照下方控制系统框图进行理解：
App参数列表中的其他参数的意义请参照参数名称。
(十五) 开启 Mcontroller 开发之旅
现在，借助下述的开发文档，开启一场 Mcontroller 的开发之旅吧！
Mcontroller 开发资源
• 板载硬件资源
Mcontroller的板载硬件资源包含15种，可在开发文档中查看其使用方法。如下图所示：
• 传感器数据
Mcontroller的传感器数据包含13类，可在开发文档中查看其获取方法。如下图所示：
• 上层应用
Mcontroller的上层应用包含9种函数库，可在Maincontroller/src文件夹中查看。如下图所示：
• 主控算法
Mcontroller的主控算法涉及自稳飞行、定高飞行、定点飞行、自主飞行和无人车模式，可在开发文档中查看算法的实现demo。
开发文档——Mcontroller模式切换
1、Mcontroller模式切换概述
Mcontroller模式切换示意图如下所示，其中主模式（空中模式和无人车模式）的切换通过Mcontroller App首界面的机器人类型选择完成。
图中的通道数值0-1对应着1000-2000，例如0.9对应着1900；自定义模式为用户预留模式，用户可以自己开发自定义子模式的功能代码。
当模式切换成功时，会发出“滴”的一声；
当模式切换失败时，会发出“嘟嘟嘟…”的声音，并且持续鸣响，这时需要检查：(1)电机是否锁定，(2)油门是否太高。
特别注意，在不连接任何遥控设备时，Mcontroller开机自动进入定位模式；如果采用Mcontroller App作为遥控器连接Mcontroller,可以自动进入空中模式，通过配置遥控通道channel5以在自稳模式、定高模式和定位模式之间切换。
特别注意，App控制的优先级高于遥控器，在遥控器和Mcontroller App同时连接的情况下，可以通过Mcontroller App 中的摇杆设置功能实现app控制与遥控器控制的互相切换，详细操作步骤请点击查看。
2、Mcontroller模式切换编程指南
打开Maincontroller/src/common.cpp文件，找到mode_update函数。首先在注释为init（）的位置添加自定义子模式的初始化代码，初始化代码只会运行一次。如下图所示：
接着在注释为update（）的位置添加自定义子模式的主循环代码，循环频率为400hz。如下图所示：
开发文档——程序烧录指南
使用STM32CubeProgrammer(请下载V2.15版本)（点击从ST官网下载）对Mcontroller进行程序烧录，烧录步骤分为以下5步。
1、选择下载方式
通过USB线将控制器与电脑连接，打开STM32CubeProgrammer，选择下载方式为USB，如下图所示：
2、Mcontroller进入Bootloader模式
重新拔插Mcontroller的USB线，或点击Mcontroller复位键，使Mcontroller进入Bootloader模式，如下图所示：
3、连接STM32CubeProgrammer
在Bootloader模式下连接STM32CubeProgrammer，如下图所示：
4、选择固件并下载
进入下载选项卡，选择固件文件，并开始下载，如下图所示：
5、下载成功
下载成功后，Mcontroller会自动重启。此时，STM32CubeProgrammer界面会出现相关提示， 如下图所示：
开发文档——工程开发指南
使用STM32CubeIDE(建议下载最新版本)（点击从ST官网下载）对Mcontroller进行工程开发，Mcontroller-v7 固件工程（点击下载）的导入步骤分为以下7步。
1、选择工作区
打开STM32CubeIDE，选择一个文件夹作为STM32CubeIDE的工作空间，如果没有可以新建一个。如下图所示：
2、添加Mcontroller-v7 固件
（1）选择好工作空间后，点击Launch，启动STM32CubeIDE ，并把从幻思创新官网或从GitHub/Gitee上下载好的“Mcontroller-v7 固件”压缩文件解压后（文件夹名称：Mcontroller-v7-firmware-main），添加到工作空间文件夹中。
（2）从幻思创新官网下载Mcontroller-v7 依赖包（无需解压，点击下载），并将该依赖包添加到"Mcontroller-v7-firmware-main"工程目录下，如下图所示。如果该工程目录下存在libMcontroller-v7-firmware.a，需要将原有的libMcontroller-v7-firmware.a文件替换为最新版。
3、到达Import页面
关闭首页Information Center，选择 File -> Import，此时会弹出Import页面，如下图所示：
4、开始导入工程
在Import页面中选择Existing Projects into Workspace, 然后点击Next。 如下图所示：
5、选择要导入的工程文件
点击Browse，选择刚刚添加到工作空间中的工程文件夹。勾选要导入的工程，并点击Finish。 如下图所示：
6、设置依赖项
右键点击工程名，选择Properties弹出属性选项卡，让工程能够找到Mcontroller-v7-firmware 依赖包libMcontroller-v7-firmware.a。 如下图所示：
7、导入完成
此时已经完成了工程导入，点击 Project -> Build All 即可编译工程。 如下图所示：
开发文档——系统常用配置说明
常用配置的宏定义参数位于Clibrary/include/config.h文件中。其中部分参数是只读的，用户不可以修改，另一部分是用户可以自由配置的。
• 只读参数
所有未在注释中提示可以修改的参数均为只读参数，如下图所示：
• 可配置参数
可配置参数包括LED灯的控制、内置磁罗盘的启用、外接GPS模块的启用和外接UWB模块的启用，配置文件位于Clibrary/include/config.h文件中。
1、是否启动系统控制LED灯
在默认状态下，以下宏定义设置为1，所有的LED灯由系统控制。如果希望自定义Led灯的闪烁，请将宏定义设置为0。
<!-- 配置LED --> #define FMU_LED_CONTROLL_ENABLE 1
2、是否启用内置磁罗盘
在默认状态下，以下宏定义设置为1，启用内置磁罗盘。如果不启用磁罗盘，请将宏定义设置为0。
<!-- 配置磁罗盘 --> #define USE_MAG 1
3、是否启用外接GPS模块
在默认状态下，以下宏定义设置为0，不启用GPS模块。如果启用GPS模块，请将宏定义设置为1。
<!-- 配置磁罗盘 --> #define USE_GPS 0
4、是否启用外接UWB模块
在默认状态下，以下宏定义设置为1，启用UWB模块。如果不启用UWB模块，请将宏定义设置为0。
<!-- 配置磁罗盘 --> #define USE_UWB 1
• 串口通信配置
每个串口有以下4种系统默认模式：自定义模式、Mavlink模式、GPS模式和Tfmini激光测距仪模式；USB口只能配置为自定义模式和Mavlink模式，原因是USB口无法匹配至GPS接收机和激光测距仪。
1、自定义模式
在自定义模式下，系统不对串口收发做任何处理，用户可以通过串口自由收发数据。
2、Mavlink模式
在Mavlink模式下，系统自动通过串口发送Mavlink心跳包（频率1hz），并查询是否有Mavlink消息包的接收，一旦接收到Mavlink消息包就自动进行消息包的解析。
3、GPS模式
在GPS模式下，系统在启动后自动通过串口检查GPS接收机的连接状态，如果连接成功，则自动更新GPS定位数据。
4、Tfmini激光测距仪模式
在Tfmini激光测距仪模式下，系统自动检测Tfmini激光测距仪的连接状态，如果连接 成功，则自动获取Tfmini激光测距仪测距数据。
通过修改/Core/Src/freertos.c中InitTask内的config_comm()函数即可完成对应串口的模式配置，如下图所示:
开发文档——线程创建
• 建立线程
基于Mcontroller，建立新线程需要5步，可以参考/Core/Src/freertos.c文件中的例程，注意这5步分散在文件的三个位置。如下图所示：
• 查询线程内存
1、查询线程内存的必要性
线程内存包括两部分，(1)为单线程分配的栈空间; (2)所有线程共用的堆空间。
如果内存分配过大会造成资源浪费，分配过小会导致内存泄漏、程序崩溃。一般分配内存的方式是首先分配一个较大的内存，然后在程序运行时查询内存使用情况，再把内存调整到一个合理大小。
2、查询函数
查询栈空间剩余情况：vTaskGetInfo(); 查询堆空间剩余情况：xPortGetFreeHeapSize()。
3、例程
可参考/Core/Src/freertos.c文件中的例程代码。如下图所示：
• 线程的定时调度
在工程中，常常需要让线程每隔一段时间运行一次，这就是定时调度。Mcontroller支持以下三种定时调度方式，例程可以参考工程的/Core/Src/freertos.c文件中的线程主函数。
1、相对延时
osDelay()，单位：ms。在当前时刻延时设定的毫秒后，再运行。
2、绝对延时
vTaskDelayUntil()，单位：ms。每隔设定的毫秒自动运行一次。
3、系统定时
Mcontroller有四个系统定时器分别为400hz,200hz,100hz和50hz，对应于四个线程Loop400hzTask()、Loop200hzTask()、Loop100hzTask()和Loop50hzTask()。实际使用时，把需要周期运行的函数添加到这四个线程中即可。
开发文档——程序调试
• printf()调试
printf()函数在c语言中比较常见，经常用来在电脑上输出一些变量数值用于调试代码bug，但是嵌入式系统没有显示器，无法直接用printf()打印测试数据。为此，我们为Mcontroller开发了两种调试方式，通过将Mcontroller与电脑usb口连接，在电脑的串口调试助手上打印出调试信息。
• printf输出方式
Mcontroller支持两种printf输出方式：(1)USB口; (2)串口。每一种printf输出方式又分为printf()和printf_dir()，我们推荐使用printf()。如下图所示:
• printf()/printf_dir()
printf()的方式是先把数据发送给缓冲区，再从缓冲区集中传递给底层硬件；printf_dir()是不通过缓冲区直接传递给底层硬件。这两种方式都可以发送大型数据而不会引起线程堵塞，但是通过缓冲区的方式可以连续调用，而不通过缓冲区的方式不能连续调用，它的两次调用之间需要间隔几毫秒。因此，我们推荐使用printf()。
开发文档——上层应用函数库的使用
• 低通滤波器的使用
滤波器的头文件为LowPassFilter.h以及LowPassFilter2p.h。demo文件为Maincontroller/demo/demo_filter.cpp。
1、定义滤波器对象
这里以float类型进行说明，如下图所示。用户可以根据不同需求自主选择int类型，long类型，Vector2f或者Vector3f类型。
2、初始化
在初始化函数中,调用set_cutoff_frequency(float sample_freq, float cutoff_freq)函数，用以设置滤波器的采样频率(Hz)以及截止频率(Hz)，初始化函数只运行一次。其中采样频率的设置，决定后续demo函数的循环运行频率。如下图所示：
3、调用demo函数
下图是demo函数，通过调用apply()函数，实现滤波。具体操作为：输入采样值，得到滤波后的输出值。该demo函数需要循环运行，且循环频率与init函数中设置的采样频率一致。即需要在freertos.c中，在对应的循环频率函数下调用该demo函数即可。
• EKF库的使用
关于EKF（拓展卡尔曼滤波）的函数等主要位于Cpplibrary/include/ekf文件夹中，包括ekf_baro.h，ekf_gnss.h，ekf_odometry.h，ekf_rangefinder.h。用户可以根据不同需求自主选择对应头文件内的函数使用。
1、定义ekf对象
定义EKF_Baro对象，_dt表示运行周期，一般取0.0025；Q表示观测数据的方差；R1表示预测数据的位置的方差；R2表示预测数据速度的方差。
定义EKF_Rangefinder对象，_dt表示运行周期，一般取0.0025；Q表示观测数据的方差；R1表示预测数据的位置的方差；R2表示预测数据速度的方差。
定义EKF_Odometry对象，_dt表示运行周期，一般取0.0025；Q1表示观测数据的位置的x轴方差；Q2表示观测数据的位置的y轴方差；R1表示预测数据的位置的x轴方差；R2表示预测数据的速度的x轴方差；R3表示预测数据的位置的y轴方差；R4表示预测数据的速度的y轴方差。
定义EKF_GNSS对象，_dt表示运行周期，一般取0.0025；Q1表示观测数据的位置的x轴方差；Q2表示观测数据的速度的x轴方差；Q3表示观测数据的位置的y轴方差；Q4表示观测数据的速度的y轴方差，观测数据就是GPS的测量值。R1表示预测数据的位置的x轴方差；R2表示预测数据的速度的x轴方差；R3表示预测数据的位置的y轴方差；R4表示预测数据的速度的y轴方差，预测数据跟加速度测量值相关。更小的Q表示更相信GPS，更小的R表示更相信imu。
上述ekf对象的定义，如下图所示：
2、参数调整策略
可以从demo给定的参数开始调整，调整的时候一般十倍十倍调整，越小的参数会导致ekf滤波结果变化比较满比较平滑，这会导致动态响应不好，表现的是滤波数据有比较大的滞后，如果出现这个问题就把这些参数调大。特别注意，Odometry只观测了位置，而GNSS既观测了位置又观测了速度，实际使用时可以根据不同需求灵活调用。
3、调用函数
具体调用函数如下图所示，调用每个对象中的update()函数，来实时更新数据。其中，每个update()函数中的第一个参数均为bool类型，实际为一种标志位。在update()中该标志位会被置成False，但在外部更新测量值的函数中，若接收到更新后的测量值，该标志位会被重新置成True。以这种方式来判断是否收到测量值。
• PID库的使用
1、头文件
使用PID相关函数时需引用“maincontroller.h”库文件，其中包含了pid.h等与pid使用相关的头文件。如下所示：
<!-- 引用库文件 --> #include "maincontroller.h"
2、定义PID对象
定义PID对象，如下所示。其中参数分别代表PID控制中的p参数，i参数，d参数，积分结果最大值，输入error的低通滤波截止频率(Hz)，以及PID运行周期(s)。
PID *pid = new PID（kp, ki, kd, imax, filt_hz, dt）;
3、单环PID使用
函数参数包括目标值以及当前值，目标值减去当前值得到误差值，将误差值传入set_input_filter_all()函数。该函数通过定义的pid对象来调用，返回计算得到的pid数值。可以通过定义的pid对象调用get_p()，get_i()，get_d()，来分别得到返回的计算值；也可以通过get_pid()函数一步返回计算值。如下图所示：
4、串级PID使用（三环）
以三环串级PID为例，函数参数包括目标值，以及三个PID环的当前值。从最外环开始，目标值减去最外环当前值，得到最外环误差值；通过定义好的p类对象调用get_p()函数，并将最外环误差值传入，得到最外环输出，即中间环的目标值。重复上述过程，得到最内环的目标值。最内环的误差值传入通过pid对象调用的set_input_filter_all()函数，返回计算值get_pid()，即最终的输出。
5、二维PID使用
二维PID的使用需要定义二维PID对象，即PID_2D，如下图所示。其中参数分别代表二维PID控制中的p_x参数，p_y参数，i_x参数，i_y参数，d_x参数，d_y参数，积分结果最大值，输入error的低通滤波截止频率(Hz)，微分项滤波频率(Hz)，以及PID运行周期(s)。
调用demo如下图所示，由于是二维PID，需要用到Vector2f类型，代表二维向量，包含x，y两个坐标。传入目标值以及当前值之后，分别相减得到误差值，传入set_input()函数，返回get_pid()值，即可得到结果。
开发文档——板载硬件资源的使用
• LED灯
板载LED灯共8个，其中可控的有7个，分别为Led1~Led7。在默认状态下，所有的LED灯由系统控制，无需用户自定义。如果用户希望自定义Led灯的闪烁，可以在Clibrary/include/config.h中更改宏定义如下：
<!-- 配置LED --> #define FMU_LED_CONTROLL_ENABLE 0
此时所有LED灯不再受系统控制，用户可以调用Clibrary/include/hal.h中的以下函数自由控制LED闪烁。如下图所示, 其中左侧为LED灯控制函数，右侧为Mcontroller控制器上的LED灯示意图。
• 蜂鸣器
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• USB&串口配置
配置串口模式在Clibrary/include/config.h文件中。如下图所示：
配置串口波特率函数位于Clibrary/include/hal.h文件中。如下图所示：
• USB&串口使用
函数在Clibrary/include/hal.h中，可以直接调用。
1、基于USB&串口的字符串打印函数
打印函数一般用于调试。如下图所示：
2、基于USB&串口的数据发送函数
数据发送方式分为两种，一种是实时发送方式，该发送方式的优点是立即发送、实时性强，缺点是高频调用可能丢失数据。如下图所示：
另一种是非实时发送方式，该发送方式的优点是高频调用不会丢失数据，缺点是非实时发送。如下图所示：
3、基于USB&串口的数据接收函数
特别注意!只有通过config_comm()函数把对应comm口配置为DEV_COMM模式才能启用下列函数。
config_comm() 函数位于/Core/Src/freertos.c中InitTask线程内。如下图所示：
• Mavlink消息包
1、发送Mavlink消息包
通过串口和usb口发送Mavlink消息包，函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
2、接收并解析Mavlink消息包
通过串口和usb口接收并解析Mavlink消息包，函数在Maincontroller/src/maincontroller.cpp中，可参考工程已有代码增加新的消息包解析代码。如下图所示：
• I2C
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• SPI
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• FRAM
1、fram驱动函数
函数在Clibrary/include/hal.h中，如下图所示。使用时无需调用该函数，而是调用它的上层函数。
2、fram上层函数
上层函数在Cpplibrary/include/flash/flash.h中，用于在板载fram中存储参数，可以直接调用，如下图所示：
3、调用demo
调用demo在Maincontroller/src/maincontroller.cpp中。所有的参数都定义在Maincontroller/inc/common.h中的parameter结构体中，用户可以参考源码添加新的参数。如下所示：
extern parameter *param
• ADC
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• 遥控器信号
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• 电机、舵机
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• GPIO
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• SD卡
1、SD卡日志记录函数
函数在Cpplibrary/include/sdlog/sdlog.h中，可以直接调用，如下图所示：
2、调用demo
调用demo在Maincontroller/src/maincontroller.cpp中，使用时可以增加新的日志，如下图所示。其中，上方为日志数据的名称，下方为日志数据的内容。两者代码格式一致，可以参照例程添加新的日志代码。在添加新的数据时，注意名称和内容在顺序上需要一一对应，并且在写入一段日志后，一定要延时1ms再写入下一段日志。
3、SD卡日志查看
请参照SD卡日志查看指南。
开发文档——传感器数据的获取
• 姿态角和传感器数据
姿态角、传感器数据的获取函数声明在Maincontroller/inc/common.h中，如下图所示：
• 四元数、欧拉角和旋转矩阵
四元数、欧拉角、旋转矩阵的转换源码在Maincontroller/src/maincontroller.cpp中。如下图所示:
开发文档——自定义参数的添加
1、在参数结构体中添加自定义参数
打开Maincontroller/inc/common.h文件，仿照Demo，在最下方parameter结构体中添加自定义参数。其中，参数类型包括：int、float、Vector和pid等常用类型。如下图所示：
2、初始化自定义参数
首先打开Maincontroller/src/Maincontroller.cpp文件，找到update_dataflash函数，如下图所示：
接着在函数中添加dataflash->set_param_***，和dataflash->get_param_***。其中***取决于添加的参数类型。
添加位置就仿照源代码，写在源代码后面即可。如下图所示：
3、通过Mavlink发送和接收自定义参数
首先打开Maincontroller/src/Maincontroller.cpp文件，找到parse_mavlink_data 函数。如下图所示：
然后，在parse_mavlink_data 函数中找到case MAV_CMD_DO_SET_PARAMETER: ;
接着，在if(is_equal(cmd.param1, 0.5f)){ … } 函数中实现重置参数功能。如下图所示：
最后，在case MAV_CMD_DO_SET_PARAMETER: 的末尾添加接收参数功能。如下图所示：
4、刷新app的参数列表
首先在Maincontroller/src/Maincontroller.cpp中找到 send_mavlink_param_list 函数，添加刷新参数列表功能代码。如下图所示：
接着在函数末尾，添加刷新自定义参数的功能代码，注意此处的param1~7和接收参数功能中的一样。如下图所示：
开发文档——SD卡日志查看指南
Mcontroller可借助SD卡很方便地对日志进行记录， 日志查看步骤分为以下9步。特别注意，日志记录在Mcontroller电机解锁时自动开始，在Mcontroller 电机锁定时自动结束。 Mcontroller从电机解锁到重新锁定期间为一个完整的日志生命周期，再次解锁将会开始下一个日志的记录。如果Mcontroller电机解锁后，没有锁定电机，而是直接断电或者强行拔出SD卡，那么将会导致最新一次的日志数据存储失败。
1、打开SD卡
在电脑中打开SD卡，可以看到SD卡中有多个txt文档，如下图所示：
2、打开index.txt
双击打开最下方的index.txt文件，可以看到最新记录的日志文件名。如下图所示：
3、选择打开方式
双击打开最新记录的日志文件，可以看到最新记录的日志数据。但是仅仅看到日志数据是没有意义的，我们还需要分析数据。因此日志文件需要鼠标右键点击，选择打开方式为Excel或者WPS Office。如下图所示：
4、使用Excel打开日志文件
如果用Excel打开日志文件，可以看到如下图所示界面，此时所有数据都存在于一列中。
5、对数据进行分列
单击“A”列的标号，即可选中第一列，在数据选项卡点击“分列”。 如下图所示：
6、分隔符号选取
点击分列后会弹出选项卡如下图所示，选择“分隔符号”->下一步->勾选“空格”->下一步->完成。如下图所示：
7、数据分列完成
完成分列后，日志数据如下图所示，每项数据都整齐排在不同列中。
8、生成数据曲线
此时选中任意一列，这里以高度数据（pos_z）为例，在“插入”选项卡中选择“折线图”，即可生成数据曲线进行分析。如下图所示：
9、日志数据名称说明
关于日志中每一列数据的含义，请点击下方按钮查看。
点击查看
开发文档——主控算法demo
主控算法Demo包含四种，可在Maincontroller/src文件夹中查看。如下图所示：
开发文档——Mcontroller ROS工程指南
• Mcontroller ROS工程概述
Mcontroller ROS工程包含以下两种用法:
1、运行于机载电脑
ROS工程运行于机载电脑上，机载电脑与Mcontroller通过usb线进行数据交互，Mcontroller传输状态数据给机载电脑，机载电脑传输定位数据和指令数据给Mcontroller。这种用法一般应用于基于slam的自主导航场景中。
2、运行于远程主机
ros工程运行于远程主机上，远程主机与Mcontroller通过互联网进行数据交互，Mcontroller传输状态数据给远程主机，远程主机传输定位数据或指令数据给Mcontroller。这种用法一般应用于基于室内定位系统的无人机自主飞行场景或集群编队飞行场景。
特别说明
（1）Mcontroller ROS工程为了兼容这两种工作模式，采用了分布式的设计架构，每一台与ROS工程通信的Mcontroller对应一个唯一的 ROS节点，即如果有N台Mcontroller同时与ROS通信那么就运行N个节点，N可以为任意整数。
（2）Mcontroller ROS工程（点击查看GitHub工程 或者 点击查看Gitee工程）中可以创建无穷多个通信节点。例如，通信节点001对应于ROS工程中的fcu_bridge_001.cpp文件。依次类推，通信节点002对应于ROS工程中的fcu_bridge_002.cpp文件。ROS工程中已经创建了多个通信节点，如果开发者希望创建更多节点则需要仿照工程源码创建新的通信节点文件。
• 环境配置
这里以 ubuntu18.04+ROS Melodic 为例讲解环境配置。
1、安装ROS
在ubuntu系统下安装ROS，相关教程请点击查看。
2、安装虚拟串口功能包
打开终端，输入以下命令。
特别说明，此处的ros版本为melodic，如果不是该版本，需要将下述的melodic替换成相应的版本代号。
sudo apt-get install ros-melodic-serial
3、安装eigen库
输入以下命令:
sudo apt-get install libeigen3-dev
• 工程编译
1、创建工作空间文件夹
工作空间可以简单理解为工程目录，在系统主目录下创建名为ros-ws的文件夹，可通过终端操作：
mkdir ros-ws
2、新建src文件夹
在ros-ws中新建src文件夹用于存放工程代码。可通过终端操作：
cd ros-ws
mkdir src
3、clone 工程
把工程clone到src文件夹中。
仓库网址：
GitHub工程请点击查看
Gitee工程请点击查看
4、运行catkin_make指令
在工作空间文件夹ros-ws下运行catkin_make指令执行工程项目编译，注意不是在src内。
cd ros-ws
catkin_make
5、编译成功
输入catkin_make指令后会自动执行一串操作，最后看到build files have been xxxx，说明编译成功了。
• 参数配置
Mcontroller ROS工程中的每一个通信节点都需要对通信方式进行单独配置。以通信节点001为例讲解配置过程。
1、打开fcu_bridge_001.cpp文件
首先打开fcu_bridge_001.cpp文件，可以看到文件顶部的参数配置变量，如下图所示：
2、参数配置
从上图可以看出：
BUF_SIZE——通信缓存区的字节大小，默认为1KB，开发者可自定义；
BAUDRATE——USB虚拟串口的波特率，默认为115200，开发者可自定义；
DRONE_PORT——网络通信的端口，默认为333，开发者可自定义；
DRONE_IP——Mcontroller网络通信的IP，默认为“192.168.0.201”，开发者可自定义；
USB_PORT——USB端口的文件描述符，默认为“/dev/ttyACM0”，开发者可自定义；
mav_chan——当前通信节点的通信方式，MAVLINK_COMM_0表示USB虚拟串口通信，MAVLINK_COMM_1表示网口通信，默认为网口通信，开发者可自定义。
• 工程运行
1、程序注册
在终端输入以下命令：
source devel/setup.bash
2、配置USB通信权限
如果用到USB通信需要配置权限，在终端输入以下命令：
sudo chmod 777 /dev/ttyACM0
3、运行
在终端输入以下命令：
roslaunch fcu_core fcu_core.launch
• 操作指令与二次开发
针对基于室内定位系统的无人机自主飞行场景或集群编队飞行场景，我们开发了一套简单易用的指令集，方便开发者通过键盘一键下发指令数据。
工程运行后，直接在Ubuntu终端中输入：指令字符+回车，即可完成对应指令的下发。
指令说明如下：
a+回车：解锁
d+回车：锁定
t+回车：起飞
l+回车：降落
r+回车：开始运行轨迹
s+回车：停止运行轨迹
1+回车：运动到位置点1
2+回车：运动到位置点2
3+回车：运动到位置点3
4+回车：运动到位置点4
特别说明
（1）所有的指令字符都在fcu_command.cpp文件中定义；
（2）所有的位置点与轨迹指令都在fcu_mission.cpp文件中定义；
（3）开发者如果需要设计自己的轨迹和目标点可以仿照工程源码进行二次开发。