ArduPilot-TOFSense
1 前言
自2023/7/10日起ArduPilot飞控固件开始支持 深圳空循环科技有限公司的TOFSense系列产品。从Plane/Copter/Rover 4.5 开始，Ardupilot 固件添加了对 TOFSense协议的支持，用于避障和高度保持。用户如果使用低于该版本的固件需要自行将相关文件进行移植与适当修改，才能支持TOFSense，TOFSense传感器驱动文件位置如下，移植可以根据使用模块的型号与模式参考下述分支链接的文件改动来进行移植。
TOFSense UART驱动分支链接
TOFSense CAN驱动分支链接
TOFSense-F IIC驱动分支链接
TOFSense-M LUA脚本驱动分支链接
APMGithub源码链接，M系列以lua脚本存在可以找官方技术支持获取
PS：以上链接为Github链接无法打开可以尝试更换网络或者等待一段时间再访问
2 TOFSense系 列
使用Pixhawk 4飞控板作为演示
2.1 UART
2.1.1 TOFSense 参数设置
使用UART模式进行通信，保持默认出厂参数即可，以下图设置为例
2.1.2 接线
用户需提前查看飞控UART接口线序，并自行准备一根6 to 4 pin的转接线，一般飞控会赠送，但是线序和端子可能并不匹配TOFSense需要手动更改一下线序，TOFSense标准接口为4P GH1.25，端子和线序为
| TOFSense | 飞控Pixhawk |
|---|---|
| VCC | 5V |
| GND | GND |
| RX | TX |
| TX | RX |
下图以Pixhawk 4为例
注意：飞控板接口使用的线序与TOFSense系列的线序并不相同，用户需按照飞控板的数据手册提前自行准备好转接线，不要直接接上TOFSense可能会将TOFSense烧坏
2.1.3 飞控参数设定
打开Mission Planner，连接飞控，点击配置与调试，点击左侧全部参数表，在右侧搜索框中搜索下述参数名称并更改。
SERIALx_PROTOCOL = 9
SERIALx_BAUDRATE = 921
RNGFND1_TYPE = 37
// x代表所选串口号
// 例如使用串口4获取数据则进行下述设置
SERIAL4_PROTOCOL = 9
SERIAL4_BAUD = 921
RNGFND1_TYPE = 37
完成上述设置后点击右侧写入参数，然后重启飞控，Mission Planner重新连接飞控后，点击左上角的飞控数据，在快速页面双击后选择rangefinder1即可看到模块数据，或者点击状态找到rangefinder1也可以看到模块的数据。
2.2 CAN（支持级联）
支持TOFSense/P/PS等型号
2.2.1 TOFSense 参数设置
TOFSense出厂默认模式为UART模式，CAN模式需要先使用NAssistant上位机对模块的参数进行一下更改，可以参考对应型号的用户手册，下述示例为3个TOFSense级联，ID分别设置为1、2、3
2.2.2 接线
注意：飞控板CAN使用的线序与TOFSense系列的线序并不相同，所以TOFSense自带的双头GH1.25的数据线并不能直接插上使用，用户需按照飞控板的数据手册提前自行准备好转接线，冒然接上可能会导致TOFSense烧坏等问题
下图为测试使用pixhawk 4 CAN接口定义
| TOFSense | 飞控Pixhawk |
|---|---|
| VCC | 5V |
| GND | GND |
| CAN_H | CAN_H |
| CAN_L | CAN_L |
级联示意图
2.2.3 飞控参数设定
打开Mission Planner，连接飞控，点击配置与调试，点击左侧全部参数表，在右侧搜索框中搜索下述参数名称并更改。
以下以CAN1作为示例设置参数：
//CAN的配置
CAN_P1_DRIVER = 1
CAN_P1_BITRATE = 1000000
CAN_D1_PROTOCOL = 13
级联参数配置如下
RNGFND1/2/3/4/5_TYPE = 38
RNGFND1/2/3/4/5_RECV_ID = TOFSense模块ID
级联3个示例
RNGFND1_TYPE = 38
RNGFND1_RECV_ID = 1
RNGFND2_TYPE = 38
RNGFND2_RECV_ID = 2
RNGFND3_TYPE = 38
RNGFND3_RECV_ID = 3
PS：找不到CAN_P1_BITRATE先写入CAN_P1_DRIVER = 1重启飞控后就可以找到
完成上述设置后点击右侧写入参数，然后重启飞控，设置无误可以看到rangefinder1/2/3正常刷新
3 TOFSense-F系列
3.1 UART
使用UART模式进行通信，保持默认出厂参数即可，以下图设置为例
其他操作与TOFSense设置操作一致，参考2.1 UART章节
3.2 IIC （支持级联）
3.2.1 TOFSense-F IIC参数设置
TOFSense-F出厂默认模式为UART模式，IIC模式需要先使用NAssistant上位机对模块的参数进行一下更改，可以参考对应型号的用户手册，下述示例为7个TOFSense-F IIC模式级联，ID分别设置为0~6，各个模块ID需要记住，后续在设置飞控参数时会用到。
下图以ID 0参数设置，其余模块除ID外剩余参数均一致
3.2.2 接线
注意：飞控板IIC接口线序与TOFSense系列的线序并不相同，所以TOFSense自带的双头GH1.25的数据线并不能直接插上使用，用户需按照飞控板的数据手册提前自行准备好转接线，冒然可能会将TOFSense烧坏
下图为测试使用pixhawk 4 IIC接口定义
| TOFSense-F | 飞控Pixhawk |
|---|---|
| VCC | 5V |
| GND | GND |
| SDA | SDA |
| SCL | SCL |
级联接线示意图，TOFSense-F只有一个GH1.25接口，所以级联需要自行转接，可以参考如下示例接线图
3.2.3 飞控参数设定以及添加脚本文件
打开Mission Planner，连接飞控，点击配置与调试，点击左侧全部参数表，在右侧搜索框中搜索下述参数名称并更改。
RNGFND1/2/3/4/5_TYPE = 40
RNGFND1/2/3/4/5_ADDR = TOFSense模块ID + 8
由于TOFSense-FIIC地址由0x08开始，所以RNGFNDx_ADDR需要模块id+0x08
// 7个TOFSense-F IIC级联参数设置如下
RNGFND1_TYPE = 40
RNGFND1_ADDR = 8
RNGFND2_TYPE = 40
RNGFND2_ADDR = 9
RNGFND3_TYPE = 40
RNGFND3_ADDR = 10
RNGFND4_TYPE = 40
RNGFND4_ADDR = 11
RNGFND5_TYPE = 40
RNGFND5_ADDR = 12
RNGFND6_TYPE = 40
RNGFND6_ADDR = 13
RNGFND7_TYPE = 40
RNGFND7_ADDR = 14
完成上述设置后点击右侧写入参数，然后重启飞控，操作无误的话可以看到rangefinder1~7正常刷新
PS：如果发现Mission Planner只有3个rangefinder，那么你需要更新一下Mission Planner，更新至1.3.81版本之后
4 TOFSense-M系列
TOFSense-M系列是比较特殊的一款模块，以LUA脚本形式进行开发，所以不局限于Ardupilot的固件版本，只需要稍微配置以及添加脚本文件即可。
4.1 UART
因为LUA脚本限制，TOFSense-M的波特率必须设置为230400才能正常通信
4.1.1 接线
用户需提前查看飞控UART接口线序，并自行准备一根6 to 4 pin的转接线，一般飞控会赠送一根但是线序并不匹配TOFSense-M需要手动更改一下线序，线序为
| TOFSense-M | 飞控Pixhawk |
|---|---|
| VCC | 5V |
| GND | GND |
| RX | TX |
| TX | RX |
下图以Pixhawk 4为例
4.1.2 飞控参数设定以及添加脚本文件
建议用户先对LUA脚本的使用有个大致的了解，可以参考Ardupolit官方写的LUA脚本教程
第一次添加时先使能脚本
SCR_ENABLE = 1
SCR_HEAP_SIZE = 150000
写入参数重启飞控，在空循环官网下载或找技术支持要对应脚本文件，TOFSense-M_Serial.lua然后通过MP ftp上传脚本文件到"scripts"文件夹
也可以使用Mission Planner来上传文件，点击配置/调试，点击左边出现的MAVFtp，找到APM/scripts，将脚本文件拖到右边空白处即可完成上传，添加成功后如下图显示
PS：打开APM下面没有scripts这个文件夹怎么办
之后依次设置
SERIAL4_PROTOCOL = 28
SERIAL4_BAUD = 230
下面两个设置二选一
1、以1D的数据输出
此项设置是将TOFSense-M以1D的传统传感器进行输出，输出值为rangefider1，输出逻辑为输出每帧64个像素点距离中最小值
RNGFND1_TYPE = 36
TOFSENSE_S1_PRX = 0
2、以3D的形式输出
此项设置是将TOFSense-M以3D的接近传感器进行输出
PRX1_TYPE = 15
TOFSENSE_S1_PRX = 1
在Mission Planner按下ctrl+F出现temp页面，找到Proximity即可看到模块数据。Ardupolit目前没有针对接近数据的三维可视化。所以目前看到的只是二维数据，但实际上 ArduPilot 是以三维方式处理所有数据。
4.2 CAN
4.2.1 接线
注意：飞控板CAN使用的线序与TOFSense系列的线序并不相同，所以TOFSense自带的双头GH1.25的数据线并不能直接插上使用，用户需按照飞控板的数据手册提前自行准备好转接线，不要冒然接上TOFSense可能会将TOFSense烧坏
| TOFSense | 飞控Pixhawk |
|---|---|
| VCC | 5V |
| GND | GND |
| CAN_H | CAN_H |
| CAN_L | CAN_L |
4.2.2 飞控参数设定以及添加脚本文件
建议用户先对LUA脚本的使用有个大致的了解，可以参考Ardupolit官方写的LUA教程
第一次添加时先使能脚本
SCR_ENABLE = 1
SCR_HEAP_SIZE = 150000
写入参数重启飞控，在空循环官网下载对应脚本文件，TOFSense-M_CAN.lua然后通过MP ftp上传脚本文件到"scripts"文件夹
也可以使用Mission Planner来上传文件，点击配置/调试，点击左边出现的MAVFtp，找到APM/scripts，将脚本文件拖到右边空白处即可完成上传，添加成功后如下图显示
PS：打开APM下面没有scripts这个文件夹怎么办
如果主控使用的CAN1连接TOFSense
CAN_D1_PROTOCOL = 10
CAN_P1_DRIVER = 1
如果主控使用的CAN2连接TOFSense
CAN_D2_PROTOCOL = 10
CAN_P2_DRIVER = 2
下面两个设置二选一
1、以1D的数据输出
此项设置是将TOFSense-M以1D的传统传感器进行输出，输出值为rangefider1，输出逻辑为输出每帧64个像素点距离中最小值
RNGFND1_TYPE = 36
TOFSENSE_PRX = 0
2、以3D的形式输出
此项设置是将TOFSense-M以3D的接近传感器进行输出
PRX1_TYPE = 15
TOFSENSE_PRX = 1
在Mission Planner按下ctrl+F出现temp页面，找到Proximity即可看到模块数据。Ardupolit目前没有针对接近数据的三维可视化。所以目前看到的只是二维数据，但实际上 ArduPilot 是以三维方式处理所有数据。
