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

#はじめに

このページは，

[ドローン操作システムを作ろう](https://qiita.com/hsgucci/items/86eedb5555b4234ee0e7)

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


#概要

[前回の記事](https://qiita.com/hsgucci/items/27fa33d7c7de505839da)で，Dronekit-Pythonを使い，
シミュレーション上のドローンとの通信・接続はできました．

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

#準備するもの

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

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

　　一応APM2.8でも試しました．
　　Pixhack[^1][^2]やPixhawk4[^3]は試していませんが，まず大丈夫だと思います．
![pixhawks.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/d48e00f4-88ae-9a21-a93a-73a2acec141b.png)


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

　　今はLychee[^5]を超試してみたい〜！

[^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

- ファームウェア： 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`で追加ですね．

```bash: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の高速ですね．

以下をコピーするか，
あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter03/hello_usb.py)　を右クリックして[名前を付けて保存]してください．

```python: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("終了．")    # 終了メッセージ
```

##プログラムの実行

ファイルを実行します．

```bash
$python hello_usb.py
```

##実行結果

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

```bash:実行結果
--------------------------
 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](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/f7905edd-50ff-0b43-5e6a-a7757c46e53e.png)

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

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

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

[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter03/hello_telem.py)　を右クリックして[名前を付けて保存]してください．

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

```python:hello_telem.py
省略

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

省略
```

実行します．

```bash
$python hello_telem.py
```

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

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

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

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

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

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

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

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

[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter03/kbhit.py) にも置いておきます．

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

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

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

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

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

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

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

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

例えば，

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

などが考えられますね．


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

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

以下をコピーするか，
あるいは　[ここ](https://raw.githubusercontent.com/DCoJA/OpenUVTM/master/chapter03/change_mode.py)　を右クリックして[名前を付けて保存]してください．

```py: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("終了．")    # 終了メッセージ
```

ファイルを実行します．

```bash
$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に戻ります．