crazyflie-CRTP解析

发表于 更新于 分类于 阅读次数：

概述

本文主要分析CRTP协议的结构，对该协议结构的理解，有助于后续的数据扩展和对软件架构的理解。为了和Crazyflie通信，Crazyflie 飞控中提出一种高层次的协议叫做 CRTP (Crazy RealTime Protocol)。这种简单的协议使用一些可以收发数据的双向目标端口，但是大都时候通信由主机发起。

物理载体

当前CRTP同时支持 Crazyradio 和 USB(只针对Crazyflie 2.0)模式。

物理载体 支持的飞控
Crazyradio (PA) Crazyflie 1.0/2.0
USB Crazyflie 2.0

协议头

每个数据包拥有1字节的数据头和31字节有效数据载荷，数据头布局如下： 

1
2
3
4
   7    6    5    4    3    2    1    0
+----+----+----+----+----+----+----+----+
|       Port        |  Link   |  Chan.  |
+----+----+----+----+----+----+----+----+

其中：

  • Port: 用来区别与消息相关的功能和任务
  • Link: 为以后使用预留
  • Channel: 用来识别子任务和子功能

端口分配

当前端口分配如下：

端口 目标 用途
0 Console 读取使用consoleprintf打印到Crazyflie控制台上的控制台信息
2 Parameters 获取或设置Crazyflie参数.Crazyflie源码中宏定义参数
3 Commander 发送控制 滚转/俯仰/偏航/油门调节的设置点
4 Memory access 访问类似于单线或I2C总线的非易失性存储器 (只支持Crazyflie 2.0)
5 Data logging 设置包含需要在特殊周期发回Crazyflie的变量的日记块.
6 Localization 与定位相关的数据包
7 Generic Setpoint 允许发送设置点和控制模式
13 Platform 用于其它平台控制，例如调试和关机
14 Client-side debugging 调试界面并只存在于Crazyflie Python API，Crazyflie飞控本身不具备
15 Link layer 用于控制和询问通信连接

Console端口说明

该端口作为一种文本控制台方式，可以使用consoleprintf 函数从Crazyflie飞控端到主机端打印文本信息。

通信协议

1
2
3
4
5
Answer (Crazyflie to host):
        +---------//-----------+
        | PRINTED CONSOLE TEXT |
        +---------//-----------+
Length          0-31

如果任何如下条件满足，飞行器端的缓存区的内容将被发送：

  • 输出缓存区满(31字节)
  • 一个"新行"字符已经被发送(或
  • 已发出刷新命令

代码分析

参考源文件 console.c，函数调用关系如下：

consoleInit函数说明

该函数用于初始化相关变量，并创建二值型信号量synch用于任务间，任务和中断间同步。

流程图

consoleTest函数说明

该函数返回初始化结果，初始化成功即测试通过。

consolePuts函数说明

该函数用于发送字符串。

流程图

该函数主要通过调用consolePutchar函数，实现字符串的发送。

consolePutchar函数说明

该函数用于发送单字节字符。

参考module/interface/console.h文件,通过utils/src/eprintf.c文件中的eprintf函数，将consolePutchar函数映射到通用打印函数，使用宏定义consolePrintf表示。其中VA_ARGS是可变参数宏。

1
#define consolePrintf(FMT, ...) eprintf(consolePutchar, FMT, ## __VA_ARGS__)

在utils/interface/debug.h文件中

1
#define DEBUG_PRINT(fmt, ...) consolePrintf(DEBUG_FMT(fmt), ##__VA_ARGS__)

使用DEBUG_PRINT宏定义映射consolePrintf函数，通过无线模块或者USB接口发送，并在控制台显示。

流程图

consolePutcharFromISR函数说明

该函数用于在中断中运行，进行字符的发送工作。

流程图

consoleSendMessage函数说明

该函数用于将消息缓存区的的数据messageToPrint，通过CRTP函数发送出去。

addBufferFullMarker

该函数用于当发送队列快满时，在队列缓存区最后加上缓存区满的标志()。

findMarkerStart函数说明

该函数用于查找标志的起始位置。

consoleFlush函数说明

该函数用于发送清空缓存区的内容。

Parameters端口说明

参数系统使得飞行器的所有可获得和可设置参数可访问。飞行器拥有一个可修改得参数表，在这个表中，每个参数都和一个ID和一个组名相关联。三个ID用于访问TOC，参数如下：

Port Channel Function
2 0 TOC 访问
2 1 参数读取
2 2 参数写入
2 3 其它命令

TOC访问

这些信息允许访问参数表的内容。信息的第一个字节是信息ID，信息ID定义如下：

命令 数值 注解
CMD_GET_ITEM 0 原始版本，获取TOC元素
CMD_GET_INFO 1 原始版本，获取CRC信息
CMD_GET_ITEM_V2 2 V2版本，获取TOC元素
CMD_GET_INFO_V2 3 V2版本，获取CRC信息

上游数据ID和命令单独发送，下游数据包拥有如下形式：

1
2
3
4
5
6
7
Bytes     1       1          1        空终止字符串
        +---+------------+------+----------+--------------+
        | 0 |  Param ID  | Type |  Group   |     Name     |
        +---+------------+------+---+------+--------------+
        | 1 | Num. Param |  CRC32   |
        +---+------------+----------+
Bytes     1       1           4

参数被PC端有序请求直到最后。当达到最后参数时,ID为0'最后的TOC元素'，复位命令允许复位TOC指针，因此下一个发送TOC元素将会是第一个。“获取 TOC CRC”命令也会返回参数数量。

CRC32是飞行器TOC的哈希值。旨在PC应用程序中实现TOC的缓存以避免每次飞行器连接后获取完整的TOC。

参数类型用一个字节描述如下：

类型码 c语言类型 Python解包
0x08 uint8_t '<B '
0x09 uint16_t '<H'
0x0A uint32_t '<L'
0x0B uint64_t '<Q'
0x00 int8_t '<b'
0x01 int16_t '<h'
0x02 int32_t '<i'
0x03 int64_t '<q'
0x05 FP16 ''
0x06 float '<f'
0x07 double '<d'

参数读取

PC客户端请求内容如下：

字节 请求字段 内容
0 ID 需要读取参数的ID

Crazyflie飞控端应答内容如下：

字节 应答字段 内容
0 ID 参数ID值
1- ... 数值 参数值。TOC中描述了大小和形式

参数读取请求是在通道1上的一个简单包，Crazyflie飞控应答相应的参数值。

参数写入

PC客户端请求写入内容如下：

字节 请求字段 内容
0 ID 需要写入的参数ID值
1- ... value 写入值，大小和形式在TOC中有描述

Crazyflie飞控端应答内容如下：

字节 应答字段 内容
0 ID 参数ID
1- ... 数值 参数值，大小和形式在TOC中有描述

写入请求是一个通道2上的简单包。Crazyflie飞控发回参数值作为应答。

其它命令

如下其它命令被使用：

命令内容
0x00 按名称设置
按名称设置

请求内容

字节 请求字段 内容
0 按名称设置 0x00
1~ n 组 组名
n ~ (n+1) NULL 0
(n+1) ~ (n+m+1) 名称 参数名称
(n+m+1)~(n+m+2) NULL 0
(n+m+2)~(n+m+3) 类型 参数类型
(n+m+3) ~ ... 数值 参数数值，大小和形式由类型描述

应答内容

字节 应答字段 内容
0 按名称设置 0x00
1~ n 组 组名
n ~ (n+1) NULL 0
(n+1) ~ (n+m+1) 名称 参数名称
(n+m+2) NULL 0
(n+m+3) 错误 如果参数被成功写入返回0.其它编码

组名和参数名称是ASCII字符串的形式，对应的大小分别为n和m。

Commander端口说明

命令端口被用来发送从PC客户端到Crazyflie飞控的控制指令，用来调节滚转、俯仰、偏航和油门等控制信号。随着通信链的建立这些数据包能被发送出去，并且直到下一包数据接收到之前该数值都是有效的。

通信协议

1
2
3
4
        +-------+-------+-------+-------+
        | ROLL  | PITCH |  YAW  |THRUST |
        +-------+-------+-------+-------+
Length      4       4       4       2      bytes
名称 字节 大小 类型 注释
滚转角 0-3 4 float 滚转设置点
俯仰角 4-7 4 float 俯仰设置点
偏航角 8-11 4 float 偏航设置点
油门 12-13 2 uint16_t 油门设置点

Memory access端口说明

存储器访问对Crazyflie Nano Quadcopter无用，当前只在Crazyflie 2.0中应用。使用存储器访问给出如下可能性：

  • 获得哪些存储单元可用的信息；

  • 读取/写入/擦除 存储器；

当前如下存储器被支持：

  • Crazyflie 2.0的板载EEPROM；

  • Crazyflie 2.0扩展板上的单线存储器；

    有更多的信息关于EEPROM如何构建和单总线存储器如何工作和构建可用。

逻辑流程图

对于客户端获取信息和读写存储器是可选择的，但是Crazyflie Python 客户端总是在连接状态下载关于存储器的信息。

通信协议

存储器端口使用3个不同的通道：

端口 通道 功能
4 0 获取关于存储器数量和类型的信息并同时擦除
4 1 读取存储器
4 2 写入存储器
通道0：信息/设置

这个通道被用来获取存在的存储器数量、关于存储器的信息和大规模擦除内存的可能性。每个包的第一个字节是命令类型：

命令内容 命令 操作
1 GET_NBR_OF_MEMS 获取存储器的数量
2 GET_MEM_INFO 获取关于存储器的信息
3 SET_MEM_ERASE 块擦除存储器
GET_NBR_OF_MEMS

这个被用来获取存在的存储器数量。

  • 请求从PC主机到Crazyflie飞控：
字节 字段 数值 长度 注解
0 获取存储器数量 0x01 1 命令字节
  • 响应从Crazyflie飞控到PC主机：
字节 字段 数值 长度 注解
0 获取存储器数量 0x01 1 命令字节
1 存储器数量 1 (所有类型)存在存储器数量

例如当Crazyflie飞控上存在三个存储器时：

1
2
Host-to-Crazyflie: <port/chan> 0x01
Crazyflie-to-Host: <port/chan> 0x01 0x03
GET_MEM_INFO

该命令被用来获取存储器的ID信息。存储器的ID是顺序的，并且从0到1，其数值小于从GET_NUMBER_OF_MEMS返回的存储器数量。

  • 请求从PC主机到Crazyflie飞控：
字节 字段 数值 长度 注解
0 获取存储器信息 0x02 1 命令字节
1 存储器ID 1 存储器ID(0 <= id < 存储器数量)
  • 如果id有效，应答从Crazyflie飞控到PC主机如下：
字节 字段 数值 长度 注解
0 GET_MEM_INFO 0x02 1 命令字节
1 MEM_ID 1 存储器ID
2 MEM_TYPE 1 存储器类型(详见如下)
3 MEM_SIZE 4 存储器的字节数
7 MEM_ADDR 8 存储器地址(只对单总线存储器有效)

关于MEM_ID字段如下：

ID 数值 注解
EEPROM_ID 0x00 关于
LEDMEM_ID 0x01 LED环存储器ID
LOCO_ID 0x02 本地位置节点ID
TRAJ_ID 0x03 轨迹ID
LOCO2_ID 0x04 本地位置节点2的ID
LH_ID 0x05 灯塔基站的ID
TESTER_ID 0x06 存储器测试ID
USD_ID 0x07 待确认
OW_FIRST_ID 0x08 第一个单总线存储器ID

关于MEM_TYPE字段如下：

类型 数值 注解
MEM_TYPE_EEPROM 0x00 电可擦除存储器
MEM_TYPE_OW 0x01 单总线存储器
MEM_TYPE_LED12 0x10 LED环
MEM_TYPE_LOCO 0x11 本地位置节点
MEM_TYPE_TRAJ 0x12 轨迹
MEM_TYPE_LOCO2 0x13 本地位置节点2
MEM_TYPE_LH 0x14 灯塔基站
MEM_TYPE_TESTER 0x15 存储器测试
MEM_TYPE_USD 0x16 待确认
  • 如果id无效，应答从Crazyflie飞控到PC主机如下：
字节 字段 数值 长度 注解
0 GET_MEM_INFO 0x02 1 字节命令
1 MEM_ID 1 存储器id

例如对于单总线存储器的请求内容如下：

MEM_ID=1, MEM_SIZE=112bytes, MEM_ADDR=0x1234567890ABCDEF

1
2
Host-to-Crazyflie: <port/chan> 0x02 0x01
Crazyflie-to-Host: <port/chan> 0x02 0x01 0x01 0x70 0x00 0x00 0x00 0xEF 0xCD 0xAB 0x90 0x78 0x56 0x34 0x12

例如请求的存储器索引信息无效时：

1
2
Host-to-Crazyflie: <port/chan> 0x01 0x10
Crazyflie-to-Host: <port/chan> 0x01 0x10
SET_MEM_ERASE(代码中无描述)

该命令被用来块擦除给定的id存储器。存储器的id是顺序的，并且从0到1，其数值小于从GET_NUMBER_OF_MEMS返回的存储器数量。

  • 请求从PC主机到Crazyflie飞控：
字节 字段 数值 长度 注解
0 SET_MEM_ERASE 0x03 1 命令字节
1 MEM_ID 1 存储器ID(0 <= id < 存储器数量)
  • 应答从Crazyflie飞控到PC主机：
字节 字段 数值 长度 注解
0 SET_MEM_ERASE 0x03 1 命令字节
1 MEM_ID 1 存储器id
2 STATUS 1 命令状态(详见如下)

例如请求块擦除MEM_ID=2的存储器

1
2
Host-to-Crazyflie: <port/chan> 0x03 0x02
Crazyflie-to-Host: <port/chan> 0x03 0x02 0x00
通道1：存储器读取

这个通道只被用来读取存储器，因此信息不包含任何命令字节。

  • 请求从PC主机到Crazyflie飞控：
字节 字段 数值 长度 注解
0 MEM_ID 1 存储器id(0 <= id < 存储器数量)
1 MEM_ADDR 4 要读取的首字节地址
5 LEN 1 要被读取的字节数量
  • 如果存储器的ID有效且地址和长度读取有效，则应答从Crazyflie飞控到PC主机如下：
字节 字段 数值 长度 注解
0 MEM_ID 1 存储器id(0 <= id < 存储器数量)
1 MEM_ADDR 4 要读取的首字节地址
5 STATUS 1 请求状态(详见如下)
6 DATA 1 .. 24? 要读取的数据(只有MEM_ID/MEM_ADDR/LEN有效时)

例如从MEM_ID=0x01 MEM_ADDR=0x0A读取LEN=0x0F字节数据：

1
2
Host-to-Crazyflie: <port/chan> 0x01 0x0A 0x00 0x00 0x00 0x0F
Crazyflie-to-Host: <port/chan> 0x01 0x0A 0x00 0x00 0x00 0x00 0x01 0x09 0x62 0x63 0x4C 0x65 0x64 0x52 0x69 0x6E 0x67 0x02 0x01 0x62 0x55
通道2：存储器写入

该通道只被用来写入存储器，因此消息中不好含任何命令字节。

  • 请求从PC主机到Crazyflie飞控：
字节 字段 数值 长度 注解
0 MEM_ID 1 存储器id(0 <= id < 存储器数量)
1 MEM_ADDR 4 要写入的首字节地址
5 DATA 1 .. 24? 要写入的数据
  • 如果存储器的ID有效并且地址和长度被写入有效，应答从Crazyflie飞控到PC主机：
字节 字段 数值 长度 注解
0 MEM_ID 1 存储器id(0 <= id < 存储器数量)
1 MEM_ADDR 4 要写入的首字节地址
5 STATUS 1 请求状态

DataLog端口说明

log端口分为三个通道：

端口 通道 功能
5 0 访问内容表：用于读取出TOC
5 1 日志控制操作：用于添加/删除/开始/暂停log块
5 2 日志数据操作：用于从Crazyflie飞控端到PC客户端发送log数据

访问内容表(Table of content access)

这个通道用来下载包含所有可记录变量和变量类型的内容表。每组信息的第一个字节对应于命令，所有这个通道的通信都被客户端初始化，并且所有来自飞行器的应答都包含同样的命令字节。

TOC命令字 命令 操作
0 CMD_GET_ITEM 原始版本：从TOC中获取元素
1 CMD_GET_INFO 原始版本：获取TOC和LOG子系统的信息
2 CMD_GET_ITEM_V2 V2版本：从TOC中获取元素
3 CMD_GET_INFO_V2 V2版本：获取TOC和LOG子系统的信息
Get TOC item

CMD_GET_ITEM命令允许从飞行器上检索日志变量名、组名和变量类型。该命令旨在从0到LOG_LEN 的所有ID中被请求。

1
2
3
4
5
6
7
8
9
10
11
12
13
Request (PC to Copter):
        +--------------+----+
        | GET_ITEM (0) | ID |
        +--------------+----+
Length         1         1

Answer (Copter to PC):
        +--------------+----+
        | GET_ITEM (0) | ID |                                        If index out of range
        +--------------+----+------+------------+--------------+
        | GET_ITEM (0) | ID | Type |   Group    |     Name     |     If returning Item
        +--------------+----+------+------------+--------------+
Length        1          1     1    < Null terminated strings >

下表说明PC端请求内容如下：

字节 请求字段 内容
0 GET_ITEM 数值0作为GET_ITEM操作
1 ID 被检索元素的ID，变量数量从0到LOG_LEN

下表说明Crazyflie飞控端的应答响应内容：

字节 应答字段 内容
0 GET_ITEM 数值0用于GET_ITEM操作
1 ID 返回元素的ID
2 Type 元素的变量类型，详见变量类型列表
3-... Group 包含元素组名可变的空终止字符串
... Name 包含元素变量名可变的空终止字符串

如果请求的ID高于(TOC_LEN-1)，则元素类型、元素组名和元素变量名不被发送。

Get Info

当连接到飞行器时，首先要请求Get Info命令。该操作能够知道变量数量、日志实施的限制和日志变量的指纹。

原始版本的请求和应答流程如下：

1
2
3
4
5
6
7
8
9
10
11
Request (PC to Copter):
        +--------------+
        | GET_INFO (1) |
        +--------------+
Length         1

Answer (Copter to PC):
        +--------------+---------+---------+-----------------+-------------+
        | GET_INFO (1) | LOG_LEN | LOG_CRC |  LOG_MAX_PACKET | LOG_MAX_OPS |
        +--------------+---------+---------+-----------------+-------------+
Length        1             1         4             1               1

下表详述PC端的请求内容：

字节 请求字段 内容
0 GET_INFO 数值1用于GET_INFO操作

下表详述crazyflie飞控应答内容：

字节 应答字段 内容
0 GET_INFO 数值1用于GET_INFO操作
1 LOG_LEN 包含在日志表中的元素的数量
2 LOG_CRC 在日志TOC存储区内容数值的CRC值，这是飞行器版本的指纹
6 LOG_MAX_PACKET 飞行器的最大日志包数量，可以在飞行器中编程
7 LOG_MAX_OPS 飞行器中的可编程操作的最大数量，该操作是一种日志变量检索程序

Log control

日志控制通道允许安装、激活、失效和移除日志包。就像访问TOC通道第一个字节代表的命令，日志控制同样有如下命令：

控制命令字节 命令 操作
0 CREATE_BLOCK 创建一个新的日志块
1 APPEND_BLOCK 在存在的日志块上追加变量
2 DELETE_BLOCK 删除一个日志块
3 START_BLOCK 启用日志块传输
4 STOP_BLOCK 停止日志块传输
5 RESET 删除所有的日志块
6 CREATE_BLOCK_V2 V2版本:创建一个新的日志块
7 APPEND_BLOCK_V2 V2版本:在存在的日志块上追加变量
Create block
1
2
3
4
5
6
7
8
9
10
11
12
Request (PC to Copter):
										  < 更多设置参数按如下结构扩展      >
        +------------------+--------------+--------------+-------------+
        | CREATE_BLOCK (0) | LOG_BLOCK_ID | SET_LOG_TYPE |  SET_LOG_ID |
        +------------------+--------------+--------------+-------------+
Length         1				   1			  1				1

Answer (Copter to PC):
        +------------------+--------------+------------+
        | CREATE_BLOCK (0) | LOG_BLOCK_ID | CREATE_STS |
        +------------------+--------------+------------+
Length            1               1               1

返回状态参考<errno.h>文件内容。

Append variable to block
1
2
3
4
5
6
7
8
9
10
11
12
Request (PC to Copter):
										  < 更多设置参数按如下结构扩展      >
        +------------------+--------------+--------------+-------------+
        | APPEND_BLOCK (1) | LOG_BLOCK_ID | SET_LOG_TYPE |  SET_LOG_ID |
        +------------------+--------------+--------------+-------------+
Length         1				   1			  1				1

Answer (Copter to PC):
        +------------------+--------------+------------+
        | APPEND_BLOCK (1) | LOG_BLOCK_ID | CREATE_STS |
        +------------------+--------------+------------+
Length            1               1               1

返回状态参考<errno.h>文件内容。

Delelte block
1
2
3
4
5
6
7
8
9
10
11
Request (PC to Copter):
        +------------------+--------------+
        | DELETE_BLOCK (2) | LOG_BLOCK_ID |
        +------------------+--------------+
Length         1				   1		

Answer (Copter to PC):
        +------------------+--------------+------------+
        | DELETE_BLOCK (2) | LOG_BLOCK_ID | CREATE_STS |
        +------------------+--------------+------------+
Length            1               1               1

通过请求的LOG_BLOCK_ID，查找相关节点，并删除链表中的该节点。

Start block

通过启动相应的定时器实现日志的发送，并设置相应的周期时间。

1
2
3
4
5
6
7
8
9
10
11
Request (PC to Copter):
        +-----------------+--------------+------------+
        | START_BLOCK (3) | LOG_BLOCK_ID | LOG_PERIOD |
        +-----------------+--------------+------------+
Length         1				   1		     1

Answer (Copter to PC):
        +-----------------+--------------+------------+
        | START_BLOCK (3) | LOG_BLOCK_ID | CREATE_STS |
        +-----------------+--------------+------------+
Length            1               1               1
Stop block

通过停止相应的定时器来停止日志数据的发送。

1
2
3
4
5
6
7
8
9
10
11
Request (PC to Copter):
        +----------------+--------------+
        | STOP_BLOCK (4) | LOG_BLOCK_ID |
        +----------------+--------------+
Length         1				   1		

Answer (Copter to PC):
        +----------------+--------------+------------+
        | STOP_BLOCK (4) | LOG_BLOCK_ID | CREATE_STS |
        +----------------+--------------+------------+
Length          1               1               1
Reset

停止所有日志块的发送、删除所有日志块的内容，释放所有日志块的空间。

1
2
3
4
5
6
7
8
9
10
11
Request (PC to Copter):
        +-----------+
        | RESET (5) |
        +-----------+
Length       1					

Answer (Copter to PC):
        +-----------+--------------+------------+
        | RESET (5) | LOG_BLOCK_ID | CREATE_STS |
        +-----------+--------------+------------+
Length        1             1             1
CREATE BLOCK V2

V2版本的日志块创建。

1
2
3
4
5
6
7
8
9
10
11
12
Request (PC to Copter):
										  < 更多设置参数按如下结构扩展      >
        +---------------------+--------------+--------------+-------------+
        | CREATE_BLOCK_V2 (6) | LOG_BLOCK_ID | SET_LOG_TYPE |  SET_LOG_ID |
        +---------------------+--------------+--------------+-------------+
Length         1				   1			  1				2

Answer (Copter to PC):
        +---------------------+--------------+------------+
        | CREATE_BLOCK_V2 (6) | LOG_BLOCK_ID | CREATE_STS |
        +---------------------+--------------+------------+
Length            1               1               1
APPEND BLOCK V2

V2版本的追加日志内容。

1
2
3
4
5
6
7
8
9
10
11
12
Request (PC to Copter):
										  < 更多设置参数按如下结构扩展      >
        +---------------------+--------------+--------------+-------------+
        | APPEND_BLOCK_V2 (7) | LOG_BLOCK_ID | SET_LOG_TYPE |  SET_LOG_ID |
        +---------------------+--------------+--------------+-------------+
Length         1				   1			  1				2

Answer (Copter to PC):
        +---------------------+--------------+------------+
        | APPEND_BLOCK_V2 (7) | LOG_BLOCK_ID | CREATE_STS |
        +---------------------+--------------+------------+
Length            1               1               1

Log data

日志数据通道用于飞行器在可编程速率下发送日志块数据。

详细的请求内容如下：

1
2
3
4
5
Answer (Copter to PC):
        +----------+------------+---------//----------+
        | BLOCK_ID | TIME_STAMP | LOG VARIABLE VALUES |
        +----------+------------+---------//----------+
Length        1          3           0 to 28
字节 应答字段 内容
0 BLOCK_ID 日志块的ID
1 TIME_STAMP 时标以ms形式，从飞行器启动起算起，并以3字节的小端整形表示
4-... Log variable values 日志包的数据值以小端形式表述

Localization端口说明

该端口组有一系列定位相关的数据包，主要如下几个通道：

端口 通道 命名
6 0 External Position 外部位置
6 1 Generic localization 通用定位
6 2 External Position Packet 外部位置数据包

External Position

该数据包被用来发送由外部系统获取的Crazyflie位置信息。主要用该数据包发送由运动捕获系统获取的位置信息，并通过扩展卡尔曼滤波，允许Crazyflie计算位置估计并控制其状态。

数据包形式如下：

1
2
3
4
5
6
struct CrtpExtPosition
{
  float x; // in m
  float y; // in m
  float z; // in m
} __attribute__((packed));

Generic Localization

该通道用于定位子系统有用的主机数据包，该数据包被创建用来服务于自身位置系统数据包，但可以被用作对更多通用系统像GPS NMEA编码或二进制数据流。数据包格式如下：

字节 数值 注解
0 PACKET_TYPE
1-... Payload 数据包的有效载荷，数据形式由包类型确定

关于PACKET_TYPE定义：

包类型 数值 含义
RANGE_STREAM_FLOAT 0 待确认
RANGE_STREAM_FP16 1 待确认
LPS_SHORT_LPP_PACKET 2 LPS 发送 LPP数据包
EMERGENCY_STOP 3 紧急停止
EMERGENCY_STOP_WATCHDOG 4 接收超时触发紧急停止
COMM_GNSS_NMEA 6 待确认
COMM_GNSS_PROPRIETARY 7 待确认
EXT_POSE 8 外部位置信息包
EXT_POSE_PACKED 9 外部位置信息压缩包

LPS:Loco Positioning System

LPP:Loco Positioning Protocol

LPP Short packet tunnel

该数据用于发送LPP短包到本地位置系统，有效数据以LPP Short Packet的形式发送给系统。

Emergency stop

当收到时，稳定循环系统被设置到紧急停止模式，该模式下停止所有电机。直到飞控状态复位，否则稳定循环模式一直保持在紧急停止状态。

EMERGENCY_STOP_WATCHDOG

飞控启动时超时紧急停止处于失效状态，当第一个数据包接收开始，超时1s就会使能超时紧急停止。该数据包至少每秒被Crazyflie发送和接收一次，否则稳定循环系统就会被设置进入紧急停止状态且所有电机停转。

EXT_POSE

外部系统的位置信息，详细结构如下：

字节 字段 类型 含义
0 EXT_POSE byte 外部位置 信息包
1 - 4 x float x轴坐标信息
5 - 8 y float y轴坐标信息
9 - 12 z float z轴坐标信息
13 - 16 qx float 四元素姿态信息：方位信息的x轴分量
17 - 20 qy float 四元素姿态信息：方位信息的y轴分量
21 - 24 qz float 四元素姿态信息：方位信息的z轴分量
25 - 28 qw float 四元素姿态信息：矢量大小系数
EXT_POSE_PACKED

外部位置系统的数据通过该数据包形式进行压缩发送，节省通道带宽，详细包形式如下：

字节 字段 类型 系数 含义
0 EXT_POSE_PACKED byte 外部位置信息压缩包
1 id uint8 Crazyflie飞控地址的最后8bit
2 - 3 x int16 0.001 x轴坐标信息
4 - 5 y int16 0.001 y轴坐标信息
6 - 7 z int16 0.001 z轴坐标信息
8 - 11 quaternion uint32_t 压缩格式的四元素(压缩算法详见quatcompress.h)
... 另一同样包形式的元素

每个CRTP数据包可以达到两个外部位置信息元素。

Generic Setpoint端口说明

该端口允许发送设定点到飞行器平台。其理念在于能够为每个不同的用例定义设定点数据的包格式。因此这是一个拥有一个通道和一个主要包形式的通用端口。

端口 通道 命名
7 0 通用设定点

通用设定点包的形式如下：

字节 数值 注解
0 ID 设定点数据包类型的ID值
1 - ... Payload 数据形式由数据包类型确定

详细的包类型如下：

ID Type
0 stop
1 Velocity World
2 Z Distance
3 CPPM Emulation
4 Altitude Hold
5 Hover
6 Full State
7 Position

Stop

这个设定点无有效负载，用于停止电机和失效控制循环。需要在Crazyflie飞控着陆的情况下发送。

Velocity World

世界坐标系中的速度设定点伴随着偏航角速度。适用于在本地位置系统的遥控模式。

有效载荷数据的形式如下：

1
2
3
4
5
6
struct velocityPacket_s {
  float vx;        // m in the world frame of reference
  float vy;        // ...
  float vz;        // ...
  float yawrate;  // deg/s
} __attribute__((packed));

Z Distance

设置Crazyflie飞控绝对高度和俯仰/滚转角度，数据有效载荷形似如下：

1
2
3
4
5
6
struct zDistancePacket_s {
  float roll;            // deg
  float pitch;           // ...
  float yawrate;         // deg/s
  float zDistance;        // m in the world frame of reference
} __attribute__((packed));

CPPM Emulation

CRTP数据包拥有一个CPPM(Crazyflie Pulse Position Modulation)仿真通道，该通道数据范围1000-2000，其中间值为1500，支持原始的RPYT(Roll Pitch Yaw Thrust)通道，再加上高达MAX_AUX_RC_CHANNELS 数量的辅组通道。辅组通道是可选的，并且除非给定通道被实际使用，发射器没必要传输所有的数据(numAuxChannels是设定依据)。

有效数据载荷形式：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
#define MAX_AUX_RC_CHANNELS 10

static float s_CppmEmuRollMaxRateDps = 720.0f; // For rate mode
static float s_CppmEmuPitchMaxRateDps = 720.0f; // For rate mode
static float s_CppmEmuRollMaxAngleDeg = 50.0f; // For level mode
static float s_CppmEmuPitchMaxAngleDeg = 50.0f; // For level mode
static float s_CppmEmuYawMaxRateDps = 400.0f; // Used regardless of flight mode

struct cppmEmuPacket_s {
  struct {
      uint8_t numAuxChannels : 4;   // Set to 0 through MAX_AUX_RC_CHANNELS
      uint8_t reserved : 4;
  } hdr;
  uint16_t channelRoll;
  uint16_t channelPitch;
  uint16_t channelYaw;
  uint16_t channelThrust;
  uint16_t channelAux[MAX_AUX_RC_CHANNELS];
} __attribute__((packed));

Altitude Hold

设置Crazyflie飞控的垂直速度和滚转/俯仰角度。

数据有效载荷形式如下：

1
2
3
4
5
6
struct altHoldPacket_s {
  float roll;            // rad
  float pitch;           // ...
  float yawrate;         // deg/s
  float zVelocity;       // m/s in the world frame of reference
} __attribute__((packed));

Hover

设置Crazyflie飞控绝对高度和在刚体坐标系下的速度。

数据有效载荷形似如下：

1
2
3
4
5
6
struct hoverPacket_s {
  float vx;           // m/s in the body frame of reference
  float vy;           // ...
  float yawrate;      // deg/s
  float zDistance;    // m in the world frame of reference
} __attribute__((packed));

Full State

设置所有的控制状态。其数据有效载荷形式如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
struct fullStatePacket_s {
  int16_t x;         // position - mm
  int16_t y;
  int16_t z;
  int16_t vx;        // velocity - mm / sec
  int16_t vy;
  int16_t vz;
  int16_t ax;        // acceleration - mm / sec^2
  int16_t ay;
  int16_t az;
  int32_t quat;      // compressed quaternion, see quatcompress.h
  int16_t rateRoll;  // angular velocity - milliradians / sec
  int16_t ratePitch; //  (NOTE: limits to about 5 full circles per sec.
  int16_t rateYaw;   //   may not be enough for extremely aggressive flight.)
} __attribute__((packed));

Position

设置绝对位置和方向，其数据有效载荷形式如下：

1
2
3
4
5
6
struct positionPacket_s {
  float x;     // Position in m
  float y;
  float z;
  float yaw;   // Orientation in degree
} __attribute__((packed));

连接步骤

CRTP被设计为无状态模式，所以不需要握手过程。任何命令可以在任何时候被发送，但是对于一些日记/参数/存储 命令，为了主机客户端能够发送正确的信息，内容表(TOC)需要被下载。为了能够使用所有的功能，在连接的情况下，执行Python API 将会下载 参数/日记/存储 TOC 。