Go to Qiita Advent Calendar 2024 Top
LoginSignup
15
19

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@hsgucci

dronekit-python を使ってみる(実機編)

Last updated at Posted at 2019-04-25

#はじめに

このページは，

ドローン操作システムを作ろう

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要

前回の記事で，Dronekit-Pythonを使い，
シミュレーション上のドローンとの通信・接続はできました．

次は，実際にフライトコントローラを接続して試してみます．
最初はUSBで，次はテレメトリ接続で試します．

#準備するもの

  • 前回使用したLinux PC
    　（dronekitをインストールしたRaspberry Piでも可能です）

  • フライトコントローラ： Pixhawk系　
    　　GPSやパワーモジュールも付けてあると，より良い

　　一応APM2.8でも試しました．
　　Pixhack12やPixhawk43は試していませんが，まず大丈夫だと思います．
pixhawks.png

　　Navio+やNavio24はUSB接続やシリアル接続ではないので，
　　connection_stringsが変わります．

　　今はLychee5を超試してみたい〜！

  • ファームウェア： ArduPilot系
    　コプター(ArduCopter)でもローバー(ArduRover)でも，どれでも良いです．
    　PX4系でも大丈夫だと思いますが，まだ試していません．

  • USBケーブル： マイクロUSBのケーブル
    　普段，Pixhawkの設定に使っている物でかまいません．

  • テレメトリ装置：　telem1や2に接続して使う物
    　普段使っているテレメトリ装置でかまいません．

　例：
　　・イームズ製：　ワイヤレステレメトリーユニット 2.4GHz 送受信機セット
　　　　　https://store.shopping.yahoo.co.jp/elab-store/ld013.html

　　・DJI製：　Datalink 900 PRO
　　　　　https://www.dji.com/jp/datalink-pro/info
　　　　　※ボーレートは115200bpsに変えてください．

　　・DIY： XBee S2Cなどで自作する
　　　例　XBee S2C (２台購入)
　　　　　　　https://www.switch-science.com/catalog/list/786/

　　　　　パソコン側基板：　XBee USB アダプター rev.2
　　　　　　　https://www.switch-science.com/catalog/3495/
　　　　　ドローン側基板：　XBee 5Vインターフェースアダプタ
　　　　　　　https://www.switch-science.com/catalog/1216/

　　・違法：Amazon等で購入できる433MHzや915MHzのテレメトリ装置
　　　　　https://www.amazon.co.jp/dp/B01A8E8XWO
　　　　　※海外製のPixhawkを購入すると一緒に付いてくることがありますが，
　　　　　　当該周波数は日本国内では違法になりますので，使用してはいけません．
　　　　　　3DRが作ったオリジナルの3DR Radioはともかくとして，
　　　　　　コピー品は，繋がらない・通信がすぐ切れるなどトラブルが多いので，
　　　　　　怖くて使う気になりません．テレメトリ切れるとかもう．．．

#事前準備

USB接続やテレメトリ接続では，シリアルポートを使用します．
Linuxで具体的には「/dev/ttyなんちゃら」というヤツです．

例：
　・/dev/ttyACM0
　　　　APMやPixhawkをUSBで接続するとACMになる．Arduinoと同じ．
　・/dev/ttyUSB0
　　　　FTDIやSiLabsのUSBシリアル変換ICだとUSBになる．テレメトリはこっち．

シリアルポートは，昔はモデムの接続に使われていましたから，
アクセス権はdialoutグループが持っています．（ダイヤルするんですね）

そこで，Ubuntu Linuxに今ログインしているユーザを
daialoutにも参加させましょう．-aで追加ですね．

dialoutのアクセス権限を追加する
$sudo usermod -a -G dialout ログインユーザ名

sudo chmod a+rw /dev/ttyなんちゃら と打って，
全員にRead/Write権限を付けても良いですが，
USBデバイスを抜いてしまうと元の状態に戻ります．
chmodは一時的な処置に過ぎません．
dialoutグループを追加するほうが楽です．

この作業はLinuxでArduinoの開発するときと同じですね．

#USB接続で試す

では，Mission PlannerやQGroundControlを使うときと同様に，
ＰＣにUSB接続して試してみましょう．

##プログラム

dronekit-sitlで使ったhello_jp.pyを少し改変しただけのプログラムです．

connection_string が "/dev/ttyACM0,115200" になっています．
USBのときは115200bpsの高速ですね．

以下をコピーするか，
あるいは　ここ　を右クリックして[名前を付けて保存]してください．

hello_usb.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "dronekitスタート" )    # 開始メッセージ

# 必要なライブラリをインポート
from dronekit import connect    # フライトコントローラやシミュレータへ接続するのがdronekit内にあるconnect
import time                     # ウェイト関数time.sleepを使うために必要

# 接続文字列の作成
connection_string = "/dev/ttyACM0,115200"       # USB接続だとttyACM、ボーレートは115.2k

# フライトコントローラ(FC)へ接続
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#Ctrl+cが押されるまでループ
try:
    while True:
        # vehicleオブジェクト内のステータスを表示
        print("--------------------------" )
        print(" GPS: %s" % vehicle.gps_0 )          # GPSがないとゼロのまま
        print(" Battery: %s" % vehicle.battery )    # パワーモジュールがないとゼロのまま
        print(" Last Heartbeat: %s" % vehicle.last_heartbeat )
        print(" Is Armable?: %s" % vehicle.is_armable )     # ARM可能か？
        print(" System status: %s" % vehicle.system_status.state )
        print(" Mode: %s" % vehicle.mode.name )

        time.sleep(1)

except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
    print( "SIGINTを検知" )

# フライトコントローラとの接続を閉じる
vehicle.close()


print("終了．")    # 終了メッセージ

##プログラムの実行

ファイルを実行します．

$python hello_usb.py

##実行結果

実行結果の例は以下の表示がCtrl+cを押すまで繰り返されます．

実行結果
--------------------------
 GPS: GPSInfo:fix=1,num_sat=0
 Battery: Battery:voltage=0.0,current=None,level=None
 Last Heartbeat: 0.674198211
 Is Armable?: False
 System status: STANDBY
 Mode: STABILIZE

GPSやパワーモジュールが有効でないと，数値はゼロになります．
また，各種キャリブレーション(加速度やコンパス)が完了していないと，
Is Armable?:も Falseのままです．

GPSを必要とするフライトモードではGPSのFixもArmableの要件です．

#テレメトリ接続で試す

次はフライトコントローラからUSBケーブルを外し，
テレメトリ接続で試してみましょう．
telemetry.png

フライトコントローラの電源は，パワーモジュールから供給すると良いでしょう．
USBで5V供給したい場合は，携帯電話用のモバイルバッテリー等を使います．

##情報を表示する(ダウンリンク側)

先程のUSB接続とは，
connection_string が "/dev/ttyACM0,57600" になっただけの違いです．

ここ　を右クリックして[名前を付けて保存]してください．

通信速度はテレメトリ装置の性能に依存します．基本的には57600bpsです．
※DJI Datalink 900 Pro を使うときは115200bpsにしてください．
　Mission Plannerのフルパラメータリスト/ツリーで書き換えます．

hello_telem.py
省略

# 接続文字列の作成
connection_string = "/dev/ttyUSB0,57600"       # テレメトリ接続だとttyUSB、ボーレートは57.6k

省略

実行します．

$python hello_telem.py

テレメトリでdronekit接続ができました．

##フライトモードの変更(アップリンク側)

データの取得だけでは面白くないので，新しいことをしてみましょう．
**キーボード入力でフライトモードの変更**をやってみます．

###キーボード入力を取る方法

まず，Pythonでキーボード入力を取る方法ですが，
通常のinput関数を使ってしまうと，プログラムが入力待ちで止まってしまいます．
(ブロッキング関数)

C言語で言うところのkbhit関数，
Arduinoで言うところのSerial.available関数
で監視する関数が欲しいですね．

それができるライブラリが以下のリンク先にあるので，
kbhit.pyとして保存しましょう．
http://code.activestate.com/recipes/572182-how-to-implement-kbhit-on-linux/

このkbhit.pyを，自分で書いたプログラム(.py)があるフォルダと同じところに置けば，
機能が利用可能になります．

ここ にも置いておきます．

一応，動作確認をしておきましょう．

$python kbhit.py

実行すると，画面上にひたすらピリオド.が表示されます．
何かのキー(CtrlやShiftなどの機能キーはダメ)を入力すると，プログラムが終了します．

kbhit.pyの使い方は割愛して説明すると以下の3つを満たすように書くことです．

(1) kbhit.pyをインポートする

from kbhit import *

(2) プログラムの冒頭(インポートの後)に，この２行を書く

atexit.register(set_normal_term)
set_curses_term()

(3) 永久ループ内でキー入力があるかどうかチェック（Ｃ言語と同じ書き方）

if kbhit():     # 何かキーが押されるのを待つ
    key = getch()   # 1文字取得

kbhit.pyを使いこなすと，dronekitに限らず，
pythonを使ったロボットプログラミングの開発効率がUPします．

例えば，

・前進・後進・旋回などの移動コマンドに使う
・パラメータ変更などのチューニングに使う
・セーブ・ロードなどのデータ管理をする

などが考えられますね．

###キー入力に応じてフライトモードを変更する

それではkbhit.pyを利用してdronekitと連携させてみましょう．

以下をコピーするか，
あるいは　ここ　を右クリックして[名前を付けて保存]してください．

change_mode.py
#!usr/bin/env python
# -*- coding: utf-8 -*-
print( "dronekitスタート" )    # 開始メッセージ

# 必要なライブラリをインポート
from kbhit import *                 # kbhitを使うために必要(同じフォルダにkbhit.pyを置くこと)
from dronekit import connect        # connectを使いたいのでインポート
from dronekit import VehicleMode    # VehicleModeも使いたいのでインポート
import time                         # ウェイト関数time.sleepを使うために必要

# kbhit()を使うための「おまじない」を最初に２つ書く
atexit.register(set_normal_term)
set_curses_term()

# 接続文字列の作成
connection_string = "/dev/ttyUSB0,57600"       # テレメトリ接続だとttyUSB、ボーレートは57.6k

# フライトコントローラ(FC)へ接続
print( "FCへ接続: %s" % (connection_string) )    # 接続設定文字列を表示
vehicle = connect(connection_string, wait_ready=True)    # 接続

#Ctrl+cが押されるまでループ
try:
    while True:
        if kbhit():     # 何かキーが押されるのを待つ
            key = getch()   # 1文字取得

            # keyの中身に応じて分岐
            if  key=='s':				# stabilize
                mode = 'STABILIZE'
            elif key=='a':				# Alt Hold
                mode = 'ALT_HOLD'
            elif key=='p':				# PosHold
                mode = 'POSHOLD'
            elif key=='l':				# loiter
                mode = 'LOITER'
            elif key=='g':				# guided
                mode = 'GUIDED'
            elif key=='t':				# auto
                mode = 'AUTO'
            elif key=='r':				# RTL
                mode = 'RTL'
            elif key=='d':				# land
                mode = 'LAND'

            vehicle.mode = VehicleMode( mode )  # フライトモードの変更を指示
        
        # ここはif文と同じインデントなので，キーに関係なく1秒に1回実行される
        # 現在のフライトモードを表示
        print("--------------------------" )
        print(" Mode: %s" % vehicle.mode.name )

        time.sleep(1)

except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
    print( "SIGINTを検知" )

# フライトコントローラとの接続を閉じる
vehicle.close()

print("終了．")    # 終了メッセージ

ファイルを実行します．

$python change_mode.py

###実行結果
キーに該当するフライトモードに変更されていることがわかります．

--------------------------
 Mode: STABILIZE
--------------------------
 Mode: STABILIZE
--------------------------
 Mode: STABILIZE
--------------------------
 Mode: ALT_HOLD
--------------------------
 Mode: ALT_HOLD
--------------------------
 Mode: ALT_HOLD
--------------------------
 Mode: LOITER
--------------------------
 Mode: LOITER
--------------------------
 Mode: POSHOLD
--------------------------
 Mode: POSHOLD

###プログラム解説

key = getch() で取ったキーコードに応じて，
フライトモードの文字列を渡しているだけです．

切り替えるフライトモードの文字列はプログラム通りに書いてください．
特に間違えやすいのは'ALT_HOLD'です．
AltHoldとして認知されていますが，'_'アンダーバーが必ず必要です．
（PosHoldの方はいらないのに）

#おわりに

今回は，実際のフライトコントローラに対してdronekitで命令を送ってみました．

正直，これだけでは全然面白くないですね．
一番おもしろいのはやはり移動させることでしょう．

次回はARM/DISARMや離着陸，ウェイポイント移動などの
実際のドローンの運用を解説しようと思います．

実機を移動させるのは少々危険なので，またsitlに戻ります．

  1. CUAVのページ　https://store.cuav.net/index.php ↩

  2. CUAVのページ　https://cuav.taobao.com/ ↩

  3. Holybroのページ　https://shop.holybro.com/pixhawk-4_p1089.html ↩

  4. Emlidのページ　https://emlid.com/navio/ ↩

  5. Droneeのページ　https://dronee.aero/pages/lychee ↩

15
19
18

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
15
19

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?