Ardupilotドローン制作⑤ ～オプティカルフローセンサー・レンジファインダーの設定～

オプティカルフローセンサーとレンジファインダーを搭載している
mateksysのオプティカルフローセンサー:OPTICAL FLOW 3901-L0Xを使い、Ardupilotに設定してみます。
前記事で未検証で不確実であったオプティカルフローセンサーの記述を削除し、本記事に改めて記述します

カスタムファームウェア

前記事で
※Flywoo GOKU GN 745 AIOでは使えないみたいです。アプデ後はどうかな？
と書いていましたが

• →カスタムファームウェアビルダーで機能を付与してビルドすると有効化する模様です。
  • latestでビルドすると失敗することがままあるので、stableの最新バージョンで行うのがオススメ
  • MSPのMSP OpticalFlowにチェックを入れる
  • デフォルトで選択されている機能のチェックは外さないが吉

ビルドしたファームウェアでFCを更新しましょう。
これまで設定していた値は消えないようなので、また一から設定し直しにはならない模様でした。

OPTICAL FLOW 3901-L0Xの接続

オプティカルフローセンサーをFCにつなげる
TX1/RX1にはんだ付けした場合、Mission Plannerでの設定手順は以下

1. Mission Plannerの上部メニュー設定/調整をクリック
2. フルパラメータ ツリーを開く
3. SERIAL1の項目のツリーを開き、SERIAL1_PROTOCOLの値を32にする
4. SERIAL1の項目のツリー内、SERIAL1_BAUDの値を115にする
5. FLOW_TYPE の値を 7 に(MSP)
6. FLOW_FXSCALER の値を -800 に
7. FLOW_FYSCALER の値を -800 に
8. オプティカルフローのマウント位置が機体中心からずれている場合、FLOW_POS_X,FLOW_POS_Y,FLOW_POS_Zの値を調整する。1cmの場合0.01

接続できたら、センサーからの数値が以下に表示されるようになる

• フライト・データ
  • ステータス
    • opt_m_x
    • opt_m_y
    • opt_x
    • opt_y
    • opt_qua

このセンサーボードにはオンボードLidarも搭載されており、これを有効化したい場合は

1. RNGFND1_TYPE = 32 (MSP)を設定
2. RNGFND1_MAX_CM = 120 を設定して、レンジファインダーの最大範囲を 1.2m に設定

これによりレンジファインダーが有効化し、OSDにRNGFを設定すると、レンジファインダーからの高度が表示されるようになる。

接続できたら、センサーからの数値が以下に表示されるようになる

• フライト・データ
  • ステータス
    • rangefinder1

(参考1)
(参考2)

機体調整

LOITERモードでオプティカルフローセンサーを利用する場合

1. EK3_ENABLE = 1 EKF3 を有効にする
   1, EKF3 を使用するために、AHRS_EKF_TYPE = 3を設定する
2. EK3_SRC_OPTIONS = 0 を設定。(FuseAllVelocities を無効)
3. オプティカル フロー センサーに応じてEK3_FLOW_DELAYを設定
4. EK3_SRC1_POSXY = 3 を設定(水平位置をGPSから得るに指定。オプティカル フロー センサーのみを使用する場合は 0 に設定でよい)
5. EK3_SRC1_VELXY = 5 (水平速度をOpticalFlow から得るに指定。)
6. EK3_SRC1_POSZ = 1 (垂直位置を気圧からから得るに指定。レンジファインダーを使う場合は 2 に指定)
7. EK3_SRC1_VELZ = 0 (垂直速度センサーはなしに指定)
8. EK3_SRC1_YAW = 1 (ヨー/方位をコンパスから得るに指定。)

(参考)

FlowHoldモードでオプティカルフローセンサーを利用する場合

フライトモードにFlowHoldを追加し、モード切替できるようにしておく

飛行！

LOITERモード

!!注意
明るく、地面に何かしらがある環境で行うこと。
暗い・地面が均一である場合、オプティカルフローは上手く機能せず、機体がどこかに高速ですっ飛びます
異常を感じたらすぐStabilizeモードに戻せるようにしておくと良いかも

1. 明るく、地面が散らかっている部屋に行きます
2. Stabilizeモードで離陸します
3. LOITERモードに変更します
4. 正常に機能すれば、屋内でもピタホバしてくれます

FlowHoldモード

フライトモードにFlowHoldを追加し、モード切替で実行
飛ばした結果、ここにあるように、確かにLOITERモードでオプティカルフローを使った時より制御が微妙で、機体が震えまくった。
使わない方が良さそう。

LOITERモードの使用センサーの切り替え機能

FlowHoldモードが微妙だったので、LOITERの使用センサーのセットを切り替えられるように設定を行います。
セットは3つ設定することができます

第1セットを元に戻す

1. EK3_SRC1_POSXY = 3 を設定(水平位置をGPSから得るに指定。)
2. EK3_SRC1_VELXY = 3 (水平速度をGPSから得るに指定。)
3. EK3_SRC1_POSZ = 1 (垂直位置を気圧から得るに指定。)
4. EK3_SRC1_VELZ = 3 (垂直速度センサーはGPSから得るに指定)
5. EK3_SRC1_YAW = 1 (ヨー/方位をコンパスから得るに指定。)

第2セットをオプティカルフローセンサーを利用するものにする

1. EK3_SRC2_POSXY = 3 を設定(水平位置をGPSから得るに指定。オプティカル フロー センサーのみを使用する場合は 0 に設定でよい)
2. EK3_SRC2_VELXY = 5 (水平速度をOpticalFlow から得るに指定。)
3. EK3_SRC2_POSZ = 1 (垂直位置を気圧からから得るに指定。レンジファインダーを使う場合は 2 に指定)
4. EK3_SRC2_VELZ = 0 (垂直速度センサーはなしに指定)
5. EK3_SRC2_YAW = 1 (ヨー/方位をコンパスから得るに指定。)

セットを切り替えるプロポのスイッチを設定する(3posスイッチ推奨)。

1. スイッチのRC番号を調べる。(意図したものに設定する)
2. RCn_OPTION = 90 とする(nはプロポスイッチに対応するRC番号)

飛行

プロポのスイッチを変更することで、どのポジションに設定されたかがOSDのメッセージに表示される。

• LOW : 第1セット
• MIDDLE : 第2セット
• HIGH : 第3セット

オプティカルフローは第2セットに設定したので、MIDDLEの状態でLOITERモードに入れるとオプティカルフローを用いて位置を固定する。
LOWにすると第1セットを選択するので、これまで同様GPSを用いて位置を固定して飛行する

参考

実際に飛ばしての気づき

• EK3_SRC2_POSXYは0でよい感。屋内なのでGPS使わない
• EK3_SRC2_POSZは1でよい。レーザー測距Liderだと設定でも2mまでの高度が限界なので、低い位置でしかOptical flowの位置固定飛行ができなくなる

---

• EK3_SRCのセットで機体の制御が変わる
  • SRC1のセット(GPS利用)は割と屋内外で安定している(屋内では余計な値が取れないため悪さしない？)
  • SRC2のセット(オプティカルフロー利用)は結構安定しない。機体があらぬ方向にすっ飛ぶことがまま起こる
• ので、オプティカルフローのセンサー利用は起動してみてダメだったらすぐGPS利用に戻すと事故を防止できる。

1. 通常のGPS系統利用設定で離陸
2. altholdでしばらく空中に留める
3. オプティカルフロー系統に切り替え
4. 挙動を見る。メッセージ消えるまで待ったり
5. 機体が流れず、大丈夫そうならloiterに切り替える
6. ダメそうならオプティカルフローは無理なので、利用センサーのセットをGPS系統に戻せばとりあえず落ちないように扱い切れるはず

成功例

オプティカルフローのキャリブレーション

ここにやり方がありますが、未実施