dSPACE ControlDeskでIMU WT61C (RS232)を使う

## 1. はじめに
dSPACE MicroLabBoxでIMUを使って加速度や角速度を取得するまでの記事です．
RS232通信機能がついているボードであれば大体やることは同じです．

Amazonで購入可能な入手難易度が低いWITMOTIONのIMUセンサがあります．
接続方法でいくつか商品がありますが，RS232通信を用いるWT61Cを使用してみます．

https://amzn.asia/d/hqWthUi

専用のソフトウェアもあるので値を見る分にはそちらを使えばよいかと．

## 2. 必要なもの
- IMU WT61C
TTL版など種類が豊富なので注意．
- 5-36V電源 (側面のSensor Supplyでも可)
- XHコネクタ 4極
IMUとD-Sub基板との接続ケーブルを自作する場合
- D-Sub 9ピン基板
これとか
<img width="20%" alt="614ul+5wOIL._AC_SL1500_.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2654082/694222e3-3aa6-ad9a-5d43-1b9cfe52afa8.jpeg">

https://amzn.asia/d/8X3w8Ft

- RS232延長ケーブル
必要な場合．ストレートとクロスがあるので注意！
(RXとTXが逆になるくらいなので変更はききます)

ちなみにPCと直接センサを繋ぎたい場合はこれ↓が必要．
- USBシリアル変換コネクタ

https://www.monotaro.com/g/04184844/?t.q=RS232%E3%80%80USB

## 3. 仕様
仕様書はメーカのサイトで入手することができます．

https://www.wit-motion.com/

ページ上部のDownload CenterからGoodle Driveを開いて対象のモデルのフォルダに説明書が格納されています．
WT61Cの場合はこれ．

https://drive.google.com/drive/u/1/folders/1KCphnAn_sttBbPlGB0SXtfHyIveAoPmV

`Manual & Datasheet Documentation`フォルダ内の`WT61C RS232 Manual.pdf`がホームのマニュアルであり，その中の2 User Instructionsというページにより詳細な情報が記載されたPDFへのリンクが貼られています．
RS232でやり取りする上で必要なのは`Communication Protocol`です．

https://drive.google.com/file/d/1xrfK9bAEncgFQYjvT_c6vwSEH0ZhzaUZ/view

## 4. 接続
WT61C RS232 Manual.pdfの`3 MCU Connection`に接続方法が記載されています．
MicroLabBoxのシリアル通信はチャンネルが2つあるので同時に2つのセンサを読めるはず？（要確認）
- Vcc
D-Subには電源ラインがないので外部電源＋に接続します．
- RXD
基板のTXDに接続する (D-Subの3ピン)
2つ目のときはDTR？ (D-Subの4ピン)
- TXD
基板のRXDに接続する (D-Subの2ピン)
2つ目のときはDCD？ (D-Subの1ピン)
- GND
外部電源－およびRS232のGND (D-Subの5ピン)に接続します．

[参考] dSPACE,"RS232デバイスの接続（要ログイン）",
https://www.dspace.com/ja/jpn/home/support/documentation.cfm?helpsetid=MicroLabBoxHardwareInstallationConfigurationGuide&externalid=Topic_81b1df5d-1acc-4c6d-aa39-2a73fce60871_--_&Language=ja-jp&Release=RLS2023-A

## 5. Simulinkモデル
[全体]
- DS1202SER_RX_C1
シリアル通信のデータ受信用ブロック
  - RX Parameters
  `Reception mode`: Skip read operation
  `Number of bytes to be received > Parameter flexibility`: non-tunable
  - Advanced
  2つともチェックを外す．
- DS1202SER_SETUP_C1
シリアル通信の設定ブロック．
  - Unit
  設定したい送受信ブロックとチャンネルを合わせる．
  - UART
  `Transeiver`: RS232
  ` Baud rate`: 115200 (WT61Cのデフォルト)
  `Data bits`: 8 Bit
  `Stop bits`: 1 Bit
  `Parity`: No
  `Copy data RX SW FIFO after reception of` 8 `byte(s) at latest.`
  - FIFO
  `SW FIFO size`: 64 Bytes
  - Advanced
  `Termination mode`: チェック外す

![スクリーンショット 2025-01-07 185716.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2654082/bb24abf6-32ec-2ebf-84d7-4989625da5e0.png)

[Subsystemの中身]
ノイズ処理のために1次遅れフィルタを挿入しています．
![スクリーンショット 2025-01-07 185745.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2654082/35951a19-8926-2b89-8732-8353f9f91d8a.png)

MATLAB Functionの中身
実行速度などは度外視のコード．
プロトコルシートの変換式をそのまま使いました．

シリアル通信が詳しくない方に説明すると，
シリアルのデータがDS1202SER_RX_C1ブロックのRXBytes端子からずっと流れ続けてくるので，..., 0x55, 0x51, ...と連続して来ればその次から加速度のデータがやってくるという仕組みです．
i番目に0x55, i+1番目に0x51が格納されている場合，それ以降のデータが以下のような意味合いを持ちます．
後はデータシートに基づいて意味ある値に変換していくだけです．
|||
|---|---|
|u(i)|0x55|
u(i+1)| 0x51
u(i+2)| X方向の加速度のLow byte
u(i+3)| X方向の加速度のHigh byte
u(i+4)| Y方向の加速度のLow byte
u(i+5)| Y方向の加速度のHigh byte
u(i+6)| Z方向の加速度のLow byte
u(i+7)| Z方向の加速度のHigh byte
u(i+8)| 温度のLow byte
u(i+9)| 温度のHigh byte
u(i+10)| チェックサム

特に設定をいじっていなければ，
加速度11bytes　→　ジャイロ11bytes　→　角度11byte　→　加速度11byte
と順繰りデータが流れていきます．

``` m
function [y1,   y2,   y3,   y4,   y5,   y6,   y7,   y8,   y9]         = fcn(u)
x = zeros(1,9);

% 85(0x55)があったら次の値をチェック．
% その次の値が81(0x51):加速度、82(0x52):角速度、83(0x53):角度

% [for文の実行回数の決め方]
% FIFO Sizeを64 byteにしているのでこのプログラムの入力uは0~63までの要素を持ちます．
% 各センサ値のデータ長は11 byteずつあります．
% ここで，すべてのセンサ値を参照するための一番長くなるケースを考えると
% 0x55の次のバイトから始まと10 byte
% 2つのセンサ値のデータが収まるので11 byte x 2 =  22 byte
% Z軸のHigh byteまで見れればいいので8 byte
% 合計40 byteの中には確実に3つのセンサ値情報が含まれていることになります．
for i = 1:40
    if u(i) == 85
        if u(i+1) == 81
            x(1) = cast(u(i+2),'uint16') + cast(u(i+3),'uint16')*256;
            x(2) = cast(u(i+4),'uint16') + cast(u(i+5),'uint16')*256;
            x(3) = cast(u(i+6),'uint16') + cast(u(i+7),'uint16')*256;
        elseif u(i+1) == 82
            x(4) = cast(u(i+2),'uint16') + cast(u(i+3),'uint16')*256;
            x(5) = cast(u(i+4),'uint16') + cast(u(i+5),'uint16')*256;
            x(6) = cast(u(i+6),'uint16') + cast(u(i+7),'uint16')*256;
        elseif u(i+1) == 83
            x(7) = cast(u(i+2),'uint16') + cast(u(i+3),'uint16')*256;
            x(8) = cast(u(i+4),'uint16') + cast(u(i+5),'uint16')*256;
            x(9) = cast(u(i+6),'uint16') + cast(u(i+7),'uint16')*256;
        end
    end
end

%0-32767はそのままの値、32768は-32768に対応。32769は-32767、65535が-1,65530が-6

% 加速度
g = 9.80665; %重力加速度
if x(1) <= 32767
    y1 = x(1)/32768*16*g;
else
    y1 = (x(1)-65536)/32768*16*g;
end

if x(2) <= 32767
    y2 = x(2)/32768*16*g;
else
    y2 = (x(2)-65536)/32768*16*g;
end

if x(3) <= 32767
    y3 = x(3)/32768*16*g;
else
    y3 = (x(3)-65536)/32768*16*g;
end

% 角速度
if x(4) <= 32767
    y4 = x(4)/32768*2000;
else
    y4 = (x(4)-65536)/32768*2000;
end

if x(5) <= 32767
    y5 = x(5)/32768*2000;
else
    y5 = (x(5)-65536)/32768*2000;
end

if x(6) <= 32767
    y6 = x(6)/32768*2000;
else
    y6 = (x(6)-65536)/32768*2000;
end

% 角度
if x(7) <= 32767
    y7 = x(7)/32768*180;
else
    y7 = (x(7)-65536)/32768*180;
end

if x(8) <= 32767
    y8 = x(8)/32768*180;
else
    y8 = (x(8)-65536)/32768*180;
end

if x(9) <= 32767
    y9 = x(9)/32768*180;
else
    y9 = (x(9)-65536)/32768*180;
end
```

## 6. 実行結果
`scopeブロック`imuを`Variable Array` Instrumentで開くと85や81といった値を確認できます．

![スクリーンショット 2025-01-07 195044.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2654082/23dbb560-35cb-d054-7a05-859628ac697e.png)

## 7. 設定の変更
ボードレートやサンプリング周波数を変更するにはRS232でコマンドを送信しないといけません．
実行中に書き換える必要がない項目はWitMotionが出しているソフトウェアを使って設定するのも手です．
以下のリンクからGoogle Driveに飛んで`WitMotion New Software.zip`をダウンロード->解凍
(ちょくちょく中国語なので使いにくい...)

https://drive.google.com/drive/u/0/folders/1TLutidDBd_tDg5aTXgjvkz63OVt5_8ZZ

実行中にリセットしたい，例えばx, y軸の角度値を0に設定したい場合，
pythonでは以下のように記述します．
``` python
# unlock
serial.write(b'\xFF\xAA\x69\x88\xB5')
time.sleep(0.01)  # sleepがないと無視される(秒数は適当)
# set x, y angles to zero
serial.write(b'\xFF\xAA\x01\x08\x00')
time.sleep(0.01)
# save
serial.write(b'\xFF\xAA\x00\x00\x00')
time.sleep(0.01)
```

Simulinkだとこんな形↓
rs232_send_swが0→1など立ち上がったときにコマンドが送信されます．
Push Button Instrumentとリンクしてコマンド送信ボタンを構築するのが吉．
注意点は，Delayブロックの遅延の長さは[ms]ではなくサンプル数なのでサンプリングレート×サンプル数が実際の遅延時間になるくらいですね．
![スクリーンショット 2025-01-27 204709.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2654082/03a83921-9484-d6ea-7b8c-a2fe2e3eea49.png)

## 8. 注意事項
- 電源OFF
一度センサ用電源を切断した場合，D-Sub端子を一度脱着しないとセンサが認識されなくなるみたいです．
(Go Online中に切断したのが原因？要確認）

## 9. おわりに
自分自身もこのようなセンサでRS232通信を扱うのが初めてなので，とりあえず動くコ
ードであることに注意してください．
次は角度のレンジや各種設定の変更についてまとめます．