描述

介紹

無人機有多個傳感器，可以讓它飛行。但他們都無法識別近距離的障礙物或與地面的實際距離。這就是無人機需要測距傳感器的原因。它們提供到途中最近物體的準確距離讀數。此外，這些對于允許路徑規劃和自主導航的映射目的很有用。

多年來，我一直在為我的大學項目使用 Crazyflie 并使用庫存固件。在找到用 Ada 實現的 Certyflie 固件后，由于它的簡單性，我開始對其進行測試。沒有大量的文件，很容易在短時間內熟悉固件并實現自己的函數和庫。在這里，我計劃解釋以下主題。

• 粗略介紹 Certyflie 固件的主要功能。
• 集成 ToF 傳感器以測量高度
• 實施自主起飛序列
• 使用 ToF 傳感器實現高度保持功能。
• 介紹如何使用 ToF 傳感器平臺來映射環境。

讓我們開始為這些設置背景。

設置背景

這些都是對我有用的東西，在開始使用 Ada 對 CF 進行編程時不會遇到太多麻煩。

• 操作系統 - Windows 8.1
• GNAT版-2018手臂精靈
• 固件 - Certyflie (ravenscar-cf-stable)

要設置編譯器路徑，請打開命令提示符，導航到克隆存儲庫的根目錄并鍵入以下命令。我們需要設置 GNAT bin 文件夾的路徑。根據您的安裝目錄更改路徑。

path C:\GNAT\2018\bin;%path%

要將固件上傳到 CF，我們需要 DFU-util。簡單地說，我們可以使用CLI 安裝程序將其安裝在 windows 上。安裝后，您可以dfu-util -l在命令提示符下鍵入以檢查安裝。它應該返回已安裝的版本。

Certyflie 存儲庫的自述文件部分提供了有關上傳編譯文件的明確說明。由于 Windows 沒有 sudo 命令，請確保在最后一個命令中刪除該部分。

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

添加 Z 游俠甲板

CF 平臺帶有幾個可拆卸的傳感器板，以擴展其功能。Z ranger 甲板有一個飛行時間傳感器，它是一個距離測量傳感器，可幫助無人機保持與地面的恒定高度。

Z ranger 配備 VL53L0X 傳感器，最大感應距離為 2m。此傳感器的庫包含在 Certyflie\Ada_Drivers_Library\components\src\range_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 后，它會初始化所有傳感器和組件。此過程在crazyflie_system.adb內部System_Init過程中可用。為了將我們的傳感器添加到這個初始化過程中，我們首先導入 VL53L0X、STM32.board 和 STM32.I2C 庫。然后我們在初始化過程中添加以下部分。

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傳感器是否工作正常。如果傳感器已成功初始化并開始工作，您會注意到紫光。

PS - 這里我們使用光流甲板，而不是 Z 游俠甲板。因此，如果您的 Z 游俠套牌看起來不同，請不要擔心。pmw3901 傳感器由于飛行無人機時發生的一些嚴重碰撞而損壞。

添加到日志

CF 有一個python 庫來執行各種任務。使用原始固件，我們可以通過CF 無線電模塊使用筆記本電腦與無人機進行通信。檢索數據日志是該庫最重要的功能之一。它可以訪問傳感器讀數、無人機狀態、電池電量和許多其他參數。幸運的是，Certyflie 固件的數據日志數量有限，并且可以與 CF python 庫一起使用。

在示例文件夾中打開。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 傳感器測量值添加到日志中。這樣我們就有機會看到當我們改變無人機的高度時值是如何變化的。

首先，我們需要在可用時從傳感器獲取測量值。初始化后，系統Stabilizer_Update_Attitude運行stabilizer.adb. 這會更新無人機的狀態參數。我們在這個函數中添加了我們的高度測量部分。接收距離以毫米為單位。這里我們將其轉換為 m。

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 固件已經具有高度保持功能。它使用預定義的推力作為基礎值。你可以在里面找到這個值commander.ads。我假設這個值幾乎等于沒有額外傳感器板的 CF 的重量。

ALT_HOLD_THRUST_F : constant := 32_767.0;

由于現在我們有了一種以 1mm 的分辨率測量距地面距離的方法，因此我們可以使用 z 測量來實現單獨的高度保持功能。為此，我們需要找到一種方法來使用我們自己的函數來設置推力值。

為穩定器Stabilizer_Control_Loop功能提供推力、滾動、俯仰和偏航值，以運行電機使無人機飛行。如果仔細觀察，我們可以確定無人機是通過兩種方法激活的。

• 從飛行員命令
• 當檢測到自由落體時

Pilot 命令使用 CrazyFlie 移動應用程序發送。您可以使用移動應用程序中的虛擬操縱桿連接藍牙并駕駛無人機。這些命令被無人機捕獲為 CRTP 數據包，并對消息進行解碼以獲取相關參數。

使用 IMU 測量的 Z 加速度值檢測自由落體。一旦檢測到自由落體，無人機就會開始產生推力以從墜毀中恢復并運行一個循環以減少推力以降低高度。起始推力和減量值可以在 free_fall.ads 中找到。您可以減少遞減值以獲得平穩著陸。

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

通過模仿這些函數中的任何一個，我們可以傳遞推力值來實現我們自己的高度保持函數。但我更喜歡模仿飛行員的命令來實現。

首先，我在Stabilizer_Control_Loop. 然后我在命令文件中實現了一個名為“Autonomous_Sequence”的單獨函數，該函數打開電機并運行 PID 算法以將高度保持在所需的水平。添加以下部分以確保無人機在運行此功能之前處于水平位置。

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 時，我決定升級電機以獲得更高的有效負載能力，以連接更多傳感器并包括更大的電池。在這個實驗中，期望的高度是 0.8m。（Kp - 2000，Kd - 0，Ki - 500）。繩子系在兩把椅子上以獲得升高的位置。

VL53L0X 和 VL53L1X

VL53L1X 的最大感應范圍為 4m，而我們在本項目中使用的 VL53L0X 的最大感應范圍為 2m。如上圖所示，VL53L1X 的鏡頭比其他傳感器大。

Z Ranger 甲板 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 引腳相同的程序。

Ada 庫帶有幾個 IO 擴展器庫。但不幸的是，它沒有配置PCA9534芯片的庫。目前，我正在為這個芯片實現一個庫。附件庫的 Ada 庫文件夾中提供了一個 beta 庫，但它需要更多的工作來實現所有可用的功能。同樣，我計劃在實施取得??進展時更新存儲庫。

在這個實現之后，我們可以使用所有的距離測量來實現一個避障功能。

?