在Mac上使用Dronekit與SITL做飛行程式的模擬除錯
背景
無人機的專案快要中期答辯了,為了在地面站(電腦)控制無人機,我們選擇DroneKit來進行程式碼的書寫。DroneKit是一個專門用於控制無人機的Python庫,使用這個API我們可以更方便的使用MAVLink來給無人機發送指令。
在我的Mac上面,我使用parallels虛擬機器裝載Windows 10和Ubuntu。因為SITL是在Linux下開發的,並且我們將使用樹莓派作為機載電腦,所以Ubuntu上面安裝的是SITL、MAVProxy和DroneKit。為了方便編輯程式碼,我在Ubuntu和macOS上都裝了Python。Windows系統只用於載入視覺化的地面站MissionPlanner。
步驟
執行SITL和MAVProxy:開啟Ubuntu,按住control+option+t,新建一個終端,輸入:
cd ~/ardupilot/ArduCopter/ ./ArduCopter.elf --home 30.75,103.93,580,0 --model quad
home後面的引數分別是模擬機出生時的(緯度,經度,海拔,朝向),因為是四旋翼所以model為quad。
再新建一個終端,輸入:
mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14551 --out 10.211.55.6:14550
這裡面有兩個out介面,第一個127.0.0.1:14551是Ubuntu的內部地址,這個地址將用作程式碼介面,第二個10.211.55.6:14550是Windows的IP地址。要檢視Windows的IP地址,開啟Windows,新建一個cmd視窗,輸入:
ipconfig
找到對應的IPv4地址填入上方相應位置即可。
開啟Windows中的MissionPlanner,右上角連線方式選擇UDP,點選connect連線,埠填寫14550。這裡的埠和上述第二個介面的埠保持一致。
這個時候模擬器和地面站都已經準備好了,就等待程式的運行了。
示例程式
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
© Copyright 2015-2016, 3D Robotics.
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto.
"""
from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
# 通過本地的14551埠,使用UDP連線到SITL模擬器,注意埠與上文的第一個out保持一致
connection_string = '127.0.0.1:14551'
print('Connecting to vehicle on: %s' % connection_string)
# connect函式將會返回一個Vehicle型別的物件,即此處的vehicle
# 即可認為是無人機的主體,通過vehicle物件,我們可以直接控制無人機
vehicle = connect(connection_string, wait_ready=True)
# 定義arm_and_takeoff函式,使無人機解鎖並起飛到目標高度
# 引數aTargetAltitude即為目標高度,單位為米
def arm_and_takeoff(aTargetAltitude):
# 進行起飛前檢查
print("Basic pre-arm checks")
# vehicle.is_armable會檢查飛控是否啟動完成、有無GPS fix、卡曼濾波器
# 是否初始化完畢。若以上檢查通過,則會返回True
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
# 解鎖無人機(電機將開始旋轉)
print("Arming motors")
# 將無人機的飛航模式切換成"GUIDED"(一般建議在GUIDED模式下控制無人機)
vehicle.mode = VehicleMode("GUIDED")
# 通過設定vehicle.armed狀態變數為True,解鎖無人機
vehicle.armed = True
# 在無人機起飛之前,確認電機已經解鎖
while not vehicle.armed:
print(" Waiting for arming...")
time.sleep(1)
# 傳送起飛指令
print("Taking off!")
# simple_takeoff將傳送指令,使無人機起飛並上升到目標高度
vehicle.simple_takeoff(aTargetAltitude)
# 在無人機上升到目標高度之前,阻塞程式
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# 當高度上升到目標高度的0.95倍時,即認為達到了目標高度,退出迴圈
# vehicle.location.global_relative_frame.alt為相對於home點的高度
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
# 等待1s
time.sleep(1)
# 呼叫上面宣告的arm_and_takeoff函式,目標高度10m
arm_and_takeoff(10)
# 設定在運動時,預設的空速為3m/s
print("Set default/target airspeed to 3")
# vehicle.airspeed變數可讀可寫,且讀、寫時的含義不同。
# 讀取時,為無人機的當前空速;寫入時,設定無人機在執行航點任務時的預設速度
vehicle.airspeed = 3
# 傳送指令,讓無人機前往第一個航點
print("Going towards first point for 30 seconds ...")
# LocationGlobalRelative是一個類,它由經緯度(WGS84)和相對於home點的高度組成
# 這條語句將建立一個位於南緯35.361354,東經149.165218,相對home點高20m的位置
point1 = LocationGlobalRelative(31.361354, 104.165218, 20)
# simple_goto函式將位置傳送給無人機,生成一個目標航點
vehicle.simple_goto(point1)
# simple_goto函式只發送指令,不判斷有沒有到達目標航點
# 它可以被其他後續指令打斷,此處延時30s,即讓無人機朝向point1飛行30s
time.sleep(30)
# 傳送指令,讓無人機前往第二個航點
print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...")
# 與之前類似,這條語句建立了另一個相對home高20m的點
point2 = LocationGlobalRelative(31.363244, 104.168801, 20)
# simple_goto將目標航點發送給無人機,groundspeed=10設定飛行時的地速為10m/s
vehicle.simple_goto(point2, groundspeed=10)
# 與之前一樣,延時30s
time.sleep(30)
# 傳送"返航"指令
print("Returning to Launch")
# 返航,只需將無人機的飛航模式切換成"RTL(Return to Launch)"
# 無人機會自動返回home點的正上方,之後自動降落
vehicle.mode = VehicleMode("RTL")
# 退出之前,清除vehicle物件
print("Close vehicle object")
vehicle.close()
儲存該示例程式為test.py,到相應目錄。
測試程式
在SITL和MAVProxy正在執行、MissionPlanner連線正常的情況下,在Ubuntu中新建一個終端,cd到示例程式所在的目錄,輸入:
python test.py
執行後,可以看到MAVProxy終端返回的各種資料,MisssionPlanner中則可以更直觀的觀察飛機的軌跡、航向等等。
自家程式的效果圖: