FanciSwarm™ 高级开发指南
高级开发指南 —— FanciSwarm™ 系统架构
• 总体架构
FanciSwarm™ 系统基于Mcontroller® 系统，由主控层、中间层、系统层和硬件层四部分构成，如下图所示：
• FreeRTOS
FreeRTOS是实时操作系统，负责管理Mcontroller的多线程调度。
FanciSwarm™ 的所有线程调度程序都在工程的/Core/Src/freertos.c文件中。主控层、中间层中的所有函数必须加载到freertos.c的线程调度函数中才能运行。
• Main controller
Main controller是系统的核心控制层，采用C风格的Cpp语言编写。
所有的上层控制函数都在Maincontroller/src文件夹中，可以直接在该文件夹中新增自己的上层控制算法，实现自定义功能。
高级开发指南——FanciSwarm™ 开发资源
• Mcontroller® 板载硬件资源
Mcontroller® 的板载硬件资源包含14种，可在“Mcontroller® 板载硬件资源的使用”一节查看其使用方法。如下图所示：
• 飞控扩展板硬件资源
飞控扩展板的板载硬件资源包含6种，可在“飞控扩展板硬件资源的使用”一节查看使用方法。如下图所示：
• 传感器数据
FanciSwarm™ 的传感器数据包含13类，可在开发文档中查看其获取方法。如下图所示：
• 上层应用
FanciSwarm™ 的上层应用包含9种函数库，可在Maincontroller/src文件夹中查看。如下图所示：
• 主控算法
FanciSwarm™ 的主控算法包含定高飞行（姿态模式）、定点飞行（位置模式）和自主飞行，可在“主控算法demo”一节查看算法demo。
高级开发指南——程序烧录指南
使用STM32CubeProgrammer(请下载V2.15版本)（点击从ST官网下载）对FanciSwarm™ 进行程序烧录，烧录步骤分为以下5步。
1、选择下载方式
通过USB线将Mcontroller® 跨模态飞控与电脑连接，打开STM32CubeProgrammer，选择下载方式为USB，如下图所示：
2、Mcontroller® 进入Bootloader模式
重新拔插Mcontroller® 的USB线，或点击Mcontroller® 复位键，使其进入Bootloader模式，如下图所示：
3、连接STM32CubeProgrammer
在Bootloader模式下连接STM32CubeProgrammer，如下图所示：
4、选择固件并下载
进入下载选项卡，选择固件文件，并开始下载，如下图所示：
5、下载成功
下载成功后，Mcontroller® 会自动重启。此时，STM32CubeProgrammer界面会出现相关提示， 如下图所示：
高级开发指南——工程开发指南
使用STM32CubeIDE(建议下载最新版本)（点击从ST官网下载）对FanciSwarm™ 进行工程开发，Mcontroller-v7-FanciSwarm 固件工程（请下载FanciSwarm™ 固件及依赖包，请勿下错）（点击下载）的导入步骤分为以下7步。
1、选择工作区
打开STM32CubeIDE，选择一个文件夹作为STM32CubeIDE的工作空间，如果没有可以新建一个。如下图所示：
2、添加Mcontroller-v7-FanciSwarm 固件
（1）选择好工作空间后，点击Launch，启动STM32CubeIDE ，并把从幻思创新官网或从GitHub/Gitee上下载好的“Mcontroller-v7-FanciSwarm 固件”压缩文件解压后（文件夹名称：Mcontroller-v7-FanciSwarm-main），添加到工作空间文件夹中。
（2）从幻思创新官网下载Mcontroller-v7-FanciSwarm 依赖包（无需解压，点击下载），并将该依赖包添加到"Mcontroller-v7-FanciSwarm-main"工程目录下，如下图所示。如果该工程目录下存在libMcontroller-v7-FanciSwarm.a，需要将原有的libMcontroller-v7-FanciSwarm.a文件替换为最新版。
3、到达Import页面
关闭首页Information Center，选择 File -> Import，此时会弹出Import页面，如下图所示：
4、开始导入工程
在Import页面中选择Existing Projects into Workspace, 然后点击Next。 如下图所示：
5、选择要导入的工程文件
点击Browse，选择刚刚添加到工作空间中的工程文件夹。勾选要导入的工程，并点击Finish。 如下图所示：
6、设置依赖项
右键点击工程名，选择Properties弹出属性选项卡，让工程能够找到Mcontroller-v7-FanciSwarm 依赖包libMcontroller-v7-FanciSwarm.a。 如下图所示：
7、导入完成
此时已经完成了工程导入，点击 Project -> Build All 即可编译工程。 如下图所示：
高级开发指南——系统常用配置说明
常用配置的宏定义参数位于Clibrary/include/config.h文件中。其中部分参数是只读的，用户不可以修改，另一部分是用户可以自由配置的。
• 只读参数
所有未在注释中提示可以修改的参数均为只读参数，如下图所示：
• 可配置参数
可配置的参数如下所示，注释中提示了可以修改的参数值。
• 串口通信配置
每个串口有以下5种模式：自定义模式（1）、Mavlink模式（2）、GPS模式（3）、激光测距仪模式（4）、光流模式（5）；
USB口有以下3种模式：可配置模式（0）、自定义模式（1）、Mavlink模式（2）。
可配置模式（0）
特别注意，仅COMM_0可以配置为CONFIG_COMM模式。在可配置模式下，用户可以通过USB端口对机载Wi-Fi模块进行配置。
轨迹飞行和集群飞行中配置组网模块（机载Wi-Fi模块）将会用到该模式。
自定义模式（1）
在自定义模式下，系统不对串口/USB收发做任何处理，用户可以通过串口自由收发数据。
Mavlink模式（2）
在Mavlink模式下，系统自动通过串口发送Mavlink心跳包（频率1hz），并查询是否有Mavlink消息包的接收，一旦接收到Mavlink消息包就自动进行消息包的解析。
GPS模式（3）
在GPS模式下，系统在启动后自动通过串口检查GPS接收机的连接状态，如果连接成功，则自动更新GPS定位数据。
激光测距仪模式（4）
在Tfmini激光测距仪模式下，系统自动检测Tfmini激光测距仪的连接状态，如果连接 成功，则自动获取Tfmini激光测距仪测距数据。
光流模式（5）
在光流模式下，系统自动检测光流连接状态，如果连接成功则自动刷新光流数据。
通过修改Clibrary/include/config.h内的COMM_0~COMM_4宏定义即可完成对应端口的模式配置，如下图所示:
高级开发指南——线程创建
• 建立线程
基于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)串口。函数在Clibrary/include/hal.h中，用户可以直接调用。每一种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闪烁。如下图所示：
• 蜂鸣器
函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• 串口
Mcontroller® 有4个串口，其中串口2、串口4已被Fanciswarm™ 使用；串口1（5V,Tx1,Rx1,Gnd）、串口3（5V,Tx3,Rx3,Gnd）拓展到了飞控的扩展板上，可连接其他硬件进行二次开发。
• 串口波特率配置
串口波特率配置函数位于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中，可以直接调用。如下图所示：
• 电机、舵机
1、电机
Mcontroller® 有8路电机信号（即PWM信号，motor1～motor8），其中motor1（M1）、motor2（M2）、motor5（M5）和motor6（M6）已被Fanciswarm™ 的四个电机使用；motor3（M3）、motor4（M4）、motor7（M7）和motor8（M8）拓展到了飞控的扩展板上，可连接其他硬件进行二次开发；
2、舵机
Mcontroller® 有4路舵机信号（即PWM信号，servo1～servo4），均拓展到了飞控的扩展板上，可连接其他硬件进行二次开发。
3、电机、舵机驱动函数
电机、舵机驱动函数函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• GPIO
Mcontroller® 有8个GPIO(gpio1~gpio8)，其中gpio1（G1）、gpio3（G3）、gpio5（G5）和gpio7（G7）拓展到了飞控的扩展板上，可连接其他硬件进行二次开发。
GPIO驱动函数在Clibrary/include/hal.h中，可以直接调用。如下图所示：
• SD卡
1、SD卡日志记录函数
函数在Cpplibrary/include/sdlog/sdlog.h中，可以直接调用，如下图所示：
2、调用demo
调用demo在Maincontroller/src/maincontroller.cpp中，使用时可以增加新的日志，如下图所示。其中，上方为日志数据的名称，下方为日志数据的内容。两者代码格式一致，可以参照例程添加新的日志代码。在添加新的数据时，注意名称和内容在顺序上需要一一对应，并且在写入一段日志后，一定要延时1ms再写入下一段日志。
3、SD卡日志查看
请参照SD卡日志查看指南。
