今天小编给大家带来的是新加坡的Maker Chathuranga Liyanage使用TOF传感器让Crazyflie实现高度自保持的项目。

设备列表

硬件：

Crazyflie2.0

Bitcraze Crazyradio PA

Z-ranger deck

软件：

AdaCore GNAT Community

无人机有多个传感器可以飞行，但是他们都无法识别出近距离的障碍物或与地面的实际距离。这就是无人机需要测距传感器的原因。它们提供到途中最近物体的准确距离读数。此外，这些对于地图绘制目的很有用，允许路径规划和自主导航。

几年来，我一直在使用Crazyflie进行我的大学项目，并使用库存固件。在发现使用 Ada 实现的 Certyflie 固件后，由于它的简单性，我开始对其进行测试。由于没有大量的文件，我很容易在短时间内熟悉固件，并实现自己的函数和库。在这里，我计划解释以下主题。

粗略了解 Certyflie 固件的主要功能。

集成ToF传感器以测量高度

实现自主起飞序列

使用ToF传感器实现高度保持功能。

介绍如何实现ToF传感器甲板以映射环境。

让我们开始为这些设置背景。

设置背景

这些对我来说是有用的，而不会给使用 Ada 开始编程 CF 带来太多麻烦。

操作系统 - Windows 8.1

GNAT 版本 - 2018 arm elf

固件 - Certyflie （ravenscar-cf-stable）

若要设置编译器路径，请打开命令提示符，导航到克隆存储库的根目录，然后键入以下命令。我们需要设置 GNAT bin 文件夹的路径。根据您的安装目录更改路径。

path C:GNAT2018in;%path%

要将固件上传到 CF，我们需要 DFU-util。简单地说，我们可以使用 CLI 安装程序将其安装在 Windows 上。安装后，您可以在 命令提示符下键入以检查安装。它应该返回已安装的版本。dfu-util -l

Certyflie 存储库的 Readme 部分提供了有关上传编译文件的明确说明。由于 Windows 没有 sudo 命令，因此请确保在最后一个命令中删除该部分。

dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D obj/cflie.bin

添加 ZRangerDeck

CF平台带有几个可拆卸的传感器拓展版，以扩展其功能。Z ranger 甲板有一个飞行时间传感器，它是一种距离测量传感器，可帮助无人机保持与地面的恒定高度。

Z ranger 配备 VL53L0X 传感器，最大感应距离为 2m。此传感器的库包含在 CertyflieAda_Drivers_Librarycomponentssrc ange_sensor 内的克隆存储库中。但是，此库的新版本在 https://github.com/AdaCore/Ada_Drivers_Library 中可用。在此实现中，我将现有库替换为新库。

要定义传感器对象并设置 I2C 端口， 请使用以下命令将库添加到 stm32-board.ads：

with VL53L0X;        use VL53L0X;

并添加以下行。

Z_Ranger_Device : VL53L0X_Ranging_Sensor(I2C_EXT_Port'Access, Ravenscar_Time.Delays);

给CF通电后，它会初始化所有传感器和组件。此过程在 内部 过程中可用。为了将我们的传感器添加到这个初始化过程中，我们首先导入VL53L0X、STM32.board和STM32。I2C 库。然后我们在初始化过程中添加以下部分。

crazyflie_system.adbSystem_Init

Initialize_I2C_GPIO (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all));
Configure_I2C (STM32.I2C.I2C_Port (Z_Ranger_Device.Port.all));
Set_Device_Address (Z_Ranger_Device, 16#52#, Status);
Data_Init (Z_Ranger_Device, Status);


Static_Init (Z_Ranger_Device, New_Sample_Ready, Status);
Perform_Ref_Calibration (Z_Ranger_Device, Status);
Set_VCSEL_Pulse_Period_Pre_Range (Z_Ranger_Device, 18, Status);
Set_VCSEL_Pulse_Period_Final_Range (Z_Ranger_Device, 14, Status);
Start_Continuous (Z_Ranger_Device, 0 ,Status);

上传后，您可以打开CF的电源，并用手机摄像头检查ToF传感器，以检查其是否正常工作。如果传感器已成功初始化并开始工作，您会注意到紫灯亮起。

P.S. - 这里我们使用光流甲板，而不是Z ranger甲板。因此，如果您的 Z 游侠套牌看起来不同，请不要担心。pmw3901 传感器由于在飞行无人机时发生了一些严重的坠机事故而损坏。

添加到日志

CF 有一个 python 库来执行各种任务。使用原始固件，我们可以通过CF无线电模块使用笔记本电脑与无人机进行通信。检索数据日志是该库最重要的功能之一。它可以访问传感器读数、无人机状态、电池电量和许多其他参数。幸运的是，Certyflie 固件的数据日志数量有限，并且可以与 CF python 库一起使用。

在 examples 文件夹内打开。此代码在运行时打印实时滚动、俯仰和偏航值。

basiclogSync.py

lg_stab.add_variable('stabilizer.roll', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')    
lg_stab.add_variable('stabilizer.yaw','float')

在第一行中，“stabilizer”是日志组，“roll”是参数，“float”是数据类型，数据类型与CF记录该特定参数的数据类型相同。在 中，您可以找到 所有日志组都已定义的过程。例如，下面显示了如何初始化偏航日志。

stabilizer.adb,Stabilizer_Init

Log.Add_Log_Variable (Group    => "stabilizer",
                               Name     => "yaw",
                               Log_Type => Log.LOG_FLOAT,
                               Variable => Euler_Yaw_Actual'Address,
                               Success  => Dummy)

当您运行 python 代码时，它将连接到 CF 并开始打印值。更改无人机的方向，看看值是如何变化的。

现在，让我们将ToF传感器测量值添加到日志中。这样，我们就有机会看到当我们改变无人机的高度时，该值是如何变化的。

首先，我们需要在传感器可用时从传感器获取测量值。初始化后，系统 在 .这将更新无人机的状态参数。我们在此功能中添加了高度测量部分。接收距离以毫米为单位。在这里，我们将其转换为 m。

Stabilizer_Update_Attitudestabilizer.adb

if Range_Value_Available (Z_Ranger_Device) then


   Z_Height := 0.001 * Float (Read_Range_Millimeters (Z_Ranger_Device));
end if;

我添加了一个名为“Range_Measurements”的单独状态变量组来保留此变量。然后，将以下日志添加到 函数中。

Stabilizer_Init

Log.Add_Log_Variable (Group    => "range",
                               Name     => "z_range",    
                               Log_Type => Log.LOG_FLOAT,
                               Variable => Z_Height'Address,
                               Success  => Dummy);

要通过 PC 查看这些值，可以在 python 脚本中添加以下行。

lg_stab.add_variable('stabilizer.yaw','float')

实现高度保持功能

Certyflie 固件已具有高度保持功能。它使用预定义的推力作为基值。您可以在 中找到此值。我假设这个值几乎等于 CF 的重量，没有额外的传感器甲板。

commander.ads

ALT_HOLD_THRUST_F : constant := 32_767.0;

由于现在我们有了测量分辨率为 1mm 的离地距离的方法，因此我们可以使用 z 测量实现单独的高度保持功能。为此，我们需要找到一种方法来使用我们自己的函数设置推力值。

为稳定器功能提供推力、滚转、俯仰和偏航值， 亿配芯城 以运行电机使无人机飞行。如果仔细观察，我们可以确定无人机是通过两种方法激活的。

Stabilizer_Control_Loop

从飞行员命令

当检测到自由落体时

飞行员命令是使用 CrazyFlie 移动应用程序发送的。您可以使用移动应用程序中的虚拟操纵杆连接蓝牙并驾驶无人机。这些命令被无人机捕获为CRTP数据包，并对消息进行解码以获取相关参数。

使用IMU测量的Z加速度值检测自由落体。一旦检测到自由落体，无人机就会开始产生推力以从坠毁中恢复过来，并运行一个循环以减少推力以降低高度。起始推力和递减值可以在 free_fall.ads 中找到。您可以减少递减值以获得平稳着陆。

MAX_RECOVERY_THRUST       : constant T_Uint16 := 48_000;
RECOVERY_THRUST_DECREMENT : constant T_Uint16 := 100;

通过模仿这些函数中的任何一个，我们可以传递推力值来制作我们自己的高度保持函数。但我更喜欢在我的实现中模仿试点命令。

首先，我注释掉了 Commander_Get_RPY、FF_Get_Recovery_Commands、Commander_Get_Thrust 和 FF_Get_Recovery_Thrust 函数调用。然后，我在指令器文件中实现了一个名为“Autonomous_Sequence”的单独函数，该函数打开电机并运行 PID 算法以将高度保持在所需水平。添加以下部分，以确保无人机在运行此功能之前处于水平位置。

Stabilizer_Control_Loop

if abs (Euler_Pitch_Actual) < 2.0 and abs (Euler_Roll_Desired) < 2.0 then
         Activate_Autopilot := True;
end if;

PID 变量在 commander.ads 中定义。

如果您曾经调整过无人机的 PID，您可能知道这是一个非常麻烦的过程。没有精确的水平位置控制器会使这变得更加困难，因为无人机开始横向漂移。为了避免这种情况，我从左右两侧在无人机上系了两根绳子。这允许无人机在有限的区域内移动，同时我们调整 PID 值。

以下是高度保持功能的初始测试。该函数运行 8 秒。可以通过增加循环计数来增加此时间。所需高度设置为0.1米。

在调整PID时，我决定升级电机以获得更高的有效载荷能力，以连接更多的传感器并包括更大的电池。CF电机驱动器的N沟道MOSFET的最大额定电流为6A，为我们提供了更大的空间来增加电机尺寸。如封面图所示，现有的 0716 空心杯电机被 0720 电机取代。

以下视频显示了使用新电机和进一步调谐的 PID 的高度保持功能。在本实验中，所需的高度为0.8m。（Kp - 2000，Kd - 0，Ki - 500）。绳子被绑在两把椅子上，以获得更高的位置。

VL35LOX和VL35L1X

VL53L1X的最大感应距离为4m，而我们在这个项目中使用的VL53L0X的最大范围为2m。如上图所示，VL53L1X的镜头比其他传感器大。

Z Ranger deck V2 配备了这款新传感器。在浏览了两个数据表后，我注意到VL53L1X具有相同的默认 I2C 地址 （0x29），这也是VL53L0X的地址。此外，大多数重要寄存器在两个传感器中具有相同的地址。因此，Ada 中的 VL53L0X 库可以与VL53L1X一起使用来运行基本功能，这些功能足以满足我们的需求。

使用多个TOF传感器

很明显，我们需要在无人机上安装更多的传感器来覆盖周围环境。但问题是我们如何与具有相同 I2C 地址的多个传感器进行通信。

CF 上的 STM32 芯片主要有 2 个 I2C 端口。但一个端口仅用于内部传感器，例如 IMU。其他端口连接到扩展引脚，以便与传感器卡座通信。因此，我们需要找到一种使用这个 I2C 端口与多个 ToF 传感器进行通信的方法。

两种ToF传感器型号都能够更改其I2C地址。当我们将新地址写入传感器时，它存储在易失性存储器中。因此，我们每次打开设备并写入 I2C 地址时，都需要一个接一个地连接传感器。为了避免这个过程，ToF传感器附带了一个额外的引脚，称为XSHUT。

要打开传感器，我们需要拉起这个引脚。在Z ranger卡座中，该引脚通过电阻器永久连接到Vcc。解决方案是编写一个程序，使除 z ranger 之外的所有其他传感器的 XSHUT 引脚保持低电平，写入不同的 I2C 地址，将一个 XSHUT 引脚设置为高电平，写入不同的 I2C 地址，依此类推。这看起来是一个很好的方法，直到您有足够的GPIO引脚来连接所有ToF传感器。

为了克服GPIO引脚的不足，具有5个ToF传感器的CF Multiranger卡座配备了一个8位GPIO扩展IC，PCA9534 。该芯片通过不同的 I2C 地址与无人机通信。当我们将值写入其注册表时，它会根据给定的值将其 8 个 GPIO 引脚设置为高低。这样，我们就有机会仅通过 I2C 端口控制所有 ToF 传感器。因此，可以毫无困难地使用上述XSHUT引脚的相同过程。

da 库附带了多个 IO 扩展器库。但不幸的是，它没有配置芯片PCA9534库。目前，我正在为这个芯片实现一个库。beta 库位于附加存储库的 Ada 库文件夹中，但它需要更多的工作来实现所有可用的功能。同样，我计划在实现取得进展时更新存储库。

实现此结果后，我们可以使用所有距离测量值实现避障功能。

审核编辑：刘清

• 传感器
• 基于
• TOF
• Crazyflie
• 自旋