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

オプティカルフローセンサーとレンジファインダーを搭載している  
[mateksysのオプティカルフローセンサー:OPTICAL FLOW 3901-L0X](http://www.mateksys.com/?portfolio=3901-l0x#tab-id-3)を使い、Ardupilotに設定してみます。  
[前記事](https://qiita.com/keraFPV/items/4bb63e04e3c3bab61016)で未検証で不確実であったオプティカルフローセンサーの記述を削除し、本記事に改めて記述します

## カスタムファームウェア

前記事で  
~~※[Flywoo GOKU GN 745 AIOでは使えないみたいです。アプデ後はどうかな？](https://discuss.ardupilot.org/t/help-i-can-not-find-flow-type-parameter-on-flywoo-f745-aio-board/86146)~~  
と書いていましたが

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

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

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

1. Mission Plannerの上部メニュー`設定/調整`をクリック
1. `フルパラメータ ツリー`を開く
1. `SERIAL1`の項目のツリーを開き、`SERIAL1_PROTOCOL`の値を`32`にする
1. `SERIAL1`の項目のツリー内、`SERIAL1_BAUD`の値を`115`にする
1. `FLOW_TYPE` の値を `7` に(MSP)
1. `FLOW_FXSCALER` の値を `-800` に
1. `FLOW_FYSCALER` の値を `-800` に
1. オプティカルフローのマウント位置が機体中心からずれている場合、`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)を設定
1. `RNGFND1_MAX_CM = 120` を設定して、レンジファインダーの最大範囲を 1.2m に設定

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

接続できたら、センサーからの数値が以下に表示されるようになる
- `フライト・データ`
  - `ステータス`
    - `rangefinder1`

[(参考1)](https://ardupilot.org/copter/docs/common-mateksys-optflow-3901L0X.html)
[(参考2)](https://www.mateksys.com/?portfolio=3901-l0x#tab-id-3)

## 機体調整

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

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

[(参考)](https://ardupilot.org/copter/docs/common-optical-flow-sensor-setup.html)

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

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

## 飛行！

### LOITERモード

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

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

### FlowHoldモード

フライトモードに`FlowHold`を追加し、モード切替で実行
飛ばした結果、[ここ](https://ardupilot.org/copter/docs/common-mateksys-optflow-3901L0X.html#additional-notes)にあるように、確かにLOITERモードでオプティカルフローを使った時より制御が微妙で、機体が震えまくった。  
使わない方が良さそう。

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

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

### 第1セットを元に戻す

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

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

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

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

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

### 飛行

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

- `LOW` : 第1セット
- `MIDDLE` : 第2セット
- `HIGH` : 第3セット

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

[参考](https://ardupilot.org/plane/docs/common-non-gps-to-gps.html#common-non-gps-to-gps)


### 実際に飛ばしての気づき

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

---

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

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

### 成功例

https://x.com/walkunaikera/status/1759910875505574297?s=20

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

[ここ](https://ardupilot.org/plane/docs/common-optical-flow-sensor-setup.html)にやり方がありますが、未実施