今天小編給大家帶來的是新加坡的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 庫文件夾中，但它需要更多的工作來實現所有可用的功能。同樣，我計劃在實現取得進展時更新存儲庫。

實現此結果后，我們可以使用所有距離測量值實現避障功能。

審核編輯：劉清