Rover製作07 Ardupirotで距離センサーの設定をする

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| OS | Windows10 |
| MissionPlanner | v1.3.80 |

| 項目 | 値 |
|:-:|:-:|
| Board| Pixhawk(v2.4.8) |
| Ardupilot | Rover V4.2.3 |

# 概要
ardupilotでは距離センサーを外付けすることが出来ます。今回はその設定を行います。

![07_distance.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/fa9d1dcb-ce39-87d1-8a57-a5953b54f2b0.jpeg)

# 予備知識

## ArdupilotのRangeFinder機能
Ardupilotでは10個まで距離センサーを接続できます。今回はanalog接続x1、I2C接続x2の合計3つの距離センサーを接続します。

|センサー番号|種類|型番|IF|用途|
|:--|:--|:--|:--|:--|
|RNGFND1|赤外線|GP2Y0A21YK|analog|下向き（対地距離測定用）|
|RNGFND2|超音波|GY-US42V2|I2C|右後（障害物検知用）|
|RNGFND3|超音波|GY-US42V2|I2C|左後（障害物検知用）|

I2C方式のセンサーは測距距離をそのまま値として返しますが、analog方式の物は電圧から距離に変換が必要です。今回は以下の様な反比例の式を用います。

```math:距離の計算式
距離=\frac{距離係数}{計測電圧[V] - オフセット電圧}
```

## 自己位置での使い方
ROSなどでRoverの自己位置を考えるときは、タイヤが接地しているはずなので自己位置は2次元で考えて、高さは無視または固定とします。しかしArdupilotは元々はDrone用のファームウェアなので自己位置は常に3次元で考慮します。
高さ方向のセンサーがないと、Pixhawkが内蔵している加速度と気圧計の値から高さを推定するために、メートル単位で上下に揺れてしまいます。
この自己位置をROSに送ると都合が悪いために、下方向の対地センサーを入れることで、自己位置の高さを安定させます。

# 前提条件

以下の機器が必要です。

* WindowsPC
    * MissionPlanner導入済み
* Pixhawk
    * ardupilot roverインストール済み
    * WindowsPCのMissionPlannerで接続可能
    * 電源供給可能
* 赤外線距離センサー（GP2Y0A21YK）x1
* 超音波センサー(GY-US42V2)x2
* I2Cエクステンダー
* Arduino Uno


# 手順

## 赤外線距離センサーの接続ケーブルの作成

* 以下のように両端がpicobladeとPHのコネクタのケーブルを作成する

|Pixhawk||GP2Y0A21YK|
|:--|:--|:--|
|Picoblade||PH|
|1 VCC||3 VCC|
|2 ADC IN (ADC3)||1 Vo|
|3 GND||2 GND|
|4 ADC IN  (ADC4)|||
|5 GND|||

![DSC_1242.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/a1ae8c4d-4e49-8e28-8723-d6816ebf1cfa.jpeg)

## 赤外線距離センサーの接続

* PHコネクタを赤外線距離センサーに接続する
* picoblade側をPixhawkの「ADC 3.3V」のコネクタに接続する。

## 赤外線距離センサーのパラメーター設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のパラメーターを設定する。

|パラメーター名|値|説明|
|:--|:--|:--|
|RNGFND1_TYPE|1|analog入力で使う|
|RNGFND1_FUNCTION|2|Hyperbolic(電圧と距離が反比例)|
|RNGFND1_MAX_CM|200|計測最大距離(cm)|
|RNGFND1_MIN_CM|5|計測最小距離(cm)|
|RNGFND1_OFFSET|0.2|オフセット電圧|
|RNGFND1_ORIENT|25|取付方向：下|
|RNGFND1_PIN|14|ADC3ピン|
|RNGFND1_POS_X|-0.2|取付位置(X)|
|RNGFND1_POS_Y|0.1|取付位置(Y)|
|RNGFND1_POS_Z|0.02|取付位置(Z)|
|RNGFND1_RMETRIC|0|出力は供給電圧に依存しない|
|RNGFND1_SCALING|0.2|距離係数|

## 赤外線距離計の動作確認

* 「フライト・データ」メニューを選択する
* 画面下部の「チューニング」の左のチェックボックスにチェックをつける。
* 画面上部に出現する「Tuning」ウィンドウをダブルクリックする。
    * 出てくるダイアログで「sonarrange」だけにチェックをつけて、ほかのチェックを外す。
* グラフのプロットの数字を見て、測距値と対応するか確認する。単位は[m]になっている。

![sonarrange.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/02db7620-5a27-a366-1016-b08933d140f1.png)

## GY-US42V2のアドレス設定作業用ケーブルの作成

| Arduino側 |  | センサー側 |
|:-:|:-:|:-:|
| プラグコンタクト |   | PH |
| - |   | 1 VCC |
| - |   | 2 SCL |
| - |   | 3 SDA |
| - |   | 4 GND |

## GY-US42V2のアドレス設定

* [アドレス変更用スケッチ](https://github.com/project-srs/ArduRover01_design/tree/main/arduino_sketch/gy-us42v2_address_changer)をArduino Unoに書き込む
* 上記のArduinoにGY-US42V2を接続する（1つずつ接続する）。
* 以下の表のようにアドレスを書き込む
    * アドレスは0~127の7bitでの表記で記載している。

|用途|センサー番号|アドレス|
|:--|:--|:--|
|右後用|RNGFND2|112（デフォルト値）|
|左後用|RNGFND3|111|

## 超音波距離センサーの接続

* 2つの超音波距離センサー(右後用、左後用)をPixhawkのI2Cバスに、エクステンダー経由で接続する。

## 超音波距離センサーのパラメーター設定

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のパラメーターを設定する。
 
|パラメーター名|値|説明|
|:--|:--|:--|
|RNGFND2_TYPE|2|MaxbotixI2C互換のI2C接続|
|RNGFND2_ADDR|112|センサーのアドレス|
|RNGFND2_ORIENT|3|取付方向：後方右|

|パラメーター名|値|説明|
|:--|:--|:--|
|RNGFND3_TYPE|2|MaxbotixI2C互換のI2C接続|
|RNGFND3_ADDR|111|センサーのアドレス|
|RNGFND3_ORIENT|5|取付方向：後方左|

## 超音波距離計の動作確認

* 「フライト・データ」メニューを選択する
* 画面下部の「チューニング」の左のチェックボックスにチェックをつける。
* 画面上部に出現する「Tuning」ウィンドウをダブルクリックする。
    * 出てくるダイアログで「rangefinder2」、「rangefinder3」だけにチェックをつけて、ほかのチェックを外す。
* グラフのプロットの数字を見て、測距値と対応するか確認する。単位は[m]になっている。

![range123.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/de8ba5bd-31a6-1ff7-8801-bdbb26b552dc.png)

# 参考
[Ardupilot RangeFinder](https://ardupilot.org/rover/docs/common-rangefinder-landingpage.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)