最近開発を進めている自作VIVE trackerについて、以前の記事でBase Stationからの赤外線信号を受光する回路を構築したことについて紹介しました。
今回は、受光した信号をESP32というマイコンで読み取ったことについてです。
実装物
赤外線信号を読み取るために、物理的な工夫を施したのちに、プログラムを実装しました。
一番初めに、今回の実装物の最終形態について動画を掲載します。
時々、xとyの値が反転しているように見えますが、原因究明はまだできていません。まあ、飛び値を消すことや、平均をとることでまともな値とすることはできそうですね。
赤外線透過フィルタの追加
私が使用しているフォトダイオードは、ピーク波長が900nmであるものの、素子としては波長が400nm程度からでも受光可能です。これでは、照明環境によっては環境ノイズが多すぎて信号が全く読めません。ちょっとしたノイズが立つだけでも、プログラムでの信号読み取りに影響がある（仕様よりHighのパルスが長くなる）ので、解決しなければならない問題です。
赤外線の信号のみを受光したい場合は、照明を消してBase Stationからの光のみが届く状態にする必要がありましたが、これでは真っ暗な環境でしかtrackerを使用することができませんね。
ということで、赤外線透過フィルタをamazonで購入したり、wiiリモコンについているフィルタを応用したりして、照明下でもうまく受光できるかを検証しました。
880 nmの赤外線透過フィルタ
購入したのは ↓ の880nmのフィルタです。
880nmのフィルタを通してスマホのカメラでBase Stationを撮影してみると、Sweep用の右・下の赤外線が見えないことがわかります。心なしかBlink用の赤外線も暗めに映っていますね。
この確認を1秒したかったがために、完全に1500円を無駄にしました。ワロスワロス。
wiiリモコン先端のフィルタ
880nmのフィルタでは、目的の850nmの赤外線までカットしてしまっていたので、身近な赤外線透過フィルタとしてwiiリモコンの先端についているフィルタを使用してみました。
880nmの際と異なり、Sweep用の赤外線が正常にカメラに写っています。目的は達成できましたが、Photo Diodeの分だけwiiリモコンを購入するのは論外なので、あくまで確認用という感じですかね。
840 nmの赤外線透過フィルタ
880nmのフィルタが動作しないことを知り、wiiリモコンのフィルタで正常に読み取れたので、続いて購入したのは840nmのフィルタです。
840nmのフィルタを通してスマホのカメラでBase Stationを撮影してみると、正常にSweep用の赤外線が見えていることがわかります。wiiリモコンのフィルタと比較してゴーストが少ない気がしますが、ただの撮影状況のせいかもしれません。
この実験により840nmのフィルタが使えると判明したため、今後フィルタが必要となった際には、これを適当なサイズにカットして使おうと思います。
プログラムの実装
自作基板に搭載されたESP32で、赤外線信号を簡単に解析してみました。
各種変数の定義
#include <Arduino.h> #include "IoTBoardUtility.h" //自作基板用のライブラリ（ピンの初期設定ぐらいしかしてない） struct PhotoSensor { const uint8_t PIN; int signalStatus; // 1つ目のHIGHを1, 2つ目を3, 2つ目を5の状態とする uint32_t signalStart; // 3パルスの総合的な始まり uint32_t highMicroSec; // highだった時間長 uint32_t lowMicroSec; // lowだった時間長 uint32_t highStart; // highが始まった時間 uint32_t lowStart; // lowが始まった時間 char nextAxis; //x:0, y:1 uint32_t axisTime[2][2]; //xA }; // 今は1つのセンサだけ使用 PhotoSensor photo1 = {32, 0, 0, 0, 0, 0, 0, 0, {{0.0, 0.0},{0.0, 0.0}}}; //PhotoSensor photo2 = {26, 0, 0, 0, 0, 0, 0, 0, {{0.0, 0.0},{0.0, 0.0}}}; //PhotoSensor photo3 = {27, 0, 0, 0, 0, 0, 0, 0, {{0.0, 0.0},{0.0, 0.0}}}; //PhotoSensor photo4 = {33, 0, 0, 0, 0, 0, 0, 0, {{0.0, 0.0},{0.0, 0.0}}}; // LighthouseはA,Bの2つがあるとする。つまり、statusの012はA, 345はB int preambleCount = 0; //preambleを考慮すると 17+1 bitの容量が必要 char cA = 0; char cB = 0; int bitCountA = 0; int bitCountB = 0; bool LH[2] = {false,false}; char OOTXdatas[100] = {}; int OOTXdataCount = 0; int count = 0; // とりあえずglobalな値で動作実験 uint32_t now; int ticks; int pulse; //0~7の値のはず int axis; int dataBit; int skip;
複数のPhotoDiodeを管理することを見越して、PhotoSensor構造体を作っています。信号処理はピン割り込み（電位の変化時）タイミングで、各センサごとに行うため、信号のHighやLowになったタイミングを逐次保存しています。
2つのBase Station同士が発している信号には、OOTX形式のデータが含まれているのですが、それはセンサごとに管理し分ける必要が無いため、1つずつ変数を用意しています。まだOOTXデータの読み取りは未実装につき、使われていませんが。
"// とりあえずglobalな値で動作実験" と書かれた箇所についても、センサごとに管理し分ける必要が無い情報のはずであり、globalな変数として定義しています。
割り込み処理の実装
void IRAM_ATTR photo1interrupt() { count++; now = micros(); if(digitalRead(photo1.PIN) == 1){ //HIGHになったタイミング photo1.highStart = now; photo1.lowMicroSec = (now - photo1.lowStart); }else{ //LOWになったタイミング photo1.lowStart = now; photo1.highMicroSec = (now - photo1.highStart); // 最初のパルスがどれかを判断するために、 // パルスの長さと前回からのパルスの経過時間を用いる if((photo1.highMicroSec > 50 && photo1.highMicroSec <= 135) && photo1.lowMicroSec > 1500){ photo1.signalStatus = 1; photo1.signalStart = photo1.highStart; // 色々と初期化処理を記述する！ LH[0] = false; LH[1] = false; } // Serial.printf("%d, %d\n", photo1.highMicroSec, photo1.signalStatus); // Serial.printf("\nL LowSec = %d, HighSec = %d, Status = %d", photo1.lowMicroSec, photo1.highMicroSec, photo1.signalStatus); if(photo1.signalStatus > 5){ photo1.signalStatus += 1; return; } // micro secではなく、tickで考える ticks = (int)(photo1.highMicroSec * 48.148); // Serial.printf("Ticks = %d, ", ticks); if(ticks > 2500 && ticks <= 9000){ //はじめの2パルス pulse = (int)((ticks-2501) / 500); //0~7の値のはず axis = pulse & 0b00000001; dataBit = (pulse>>1) & 0b00000001; skip = (pulse >> 2) & 0b00000001; // Serial.printf(" **************** Axis = %d, bit = %d, skip = %d \n", axis, dataBit, skip); // Lighthouseからのデータを溜めていく OOTX形式 cA += dataBit * (1 < bitCountA); bitCountA++; if(bitCountA == 8) { bitCountA = 0; OOTXdatas[OOTXdataCount] = cA; cA = 0; OOTXdataCount++; } // Lighthouse A,Bのどちらかのうちskipしない側の、LighthouseをLH配列で覚えておく // 1つ目のHIGHを1, 2つ目を3, 2つ目を5の状態 if(skip==0) LH[(photo1.signalStatus-1)/2] = true; photo1.nextAxis = axis; } else{ //xy軸方向のレーザであるはず //LighthouseA if(LH[0])photo1.axisTime[0][photo1.nextAxis] = now - photo1.signalStart; //LighthouseB if(LH[1])photo1.axisTime[1][photo1.nextAxis] = now - photo1.signalStart; } Serial.printf("%d, %d\n",photo1.axisTime[0][0], photo1.axisTime[0][1]); } photo1.signalStatus += 1; } void setup() { Serial.begin(115200); pinMode(photo1.PIN, INPUT_PULLUP); attachInterrupt(photo1.PIN, photo1interrupt, CHANGE); } void loop() { }
setup( )内で、ピン割り込みの設定を行います。photo1interrupt( )という極限にセンスが無い関数を、HighとLowが変化する際に毎回呼び出すようになっています。
割り込み関数内では、この ↓ まとめ記事を参考に信号処理を行っています。 ume-boshi.hatenablog.jp
Base Stationの値を取得するためには、信号の開始を読み解く必要があります。これはHighになったタイミングからだけではわからずHigh -> Lowになった際の、｛前回のLowの時間が1500us以上 && 65us以上のHighのパルス｝の条件時に判明します。条件に当てはまった際には、signalStartとsignalStatus、BaseStationの状態を初期化しています。11行目あたりの内容ですね。
BaseStationからの信号は1フェーズにつき3つのパルスが来るので、「High -> Low -> High -> ... 」 という変化について、それぞれ「0 -> 1 -> 2 -> ...」という状態にsignalStatusを定義してます。このsignalStatusは5が最大で、それ以降になったときには信号読み取りが正しくできていないので、無視します。
中盤弱から書かれている"tick"の計算ですが、同期信号の意味を読み取っています。これも先ほどの記事に分析方法がかかれているので、ここでは説明を省略します。この同期信号を読み取ることで、そのフェーズで受け取れるSweep信号のaxisがわかるので、なぞの"LH[]"という配列で保存してあげます。
Lowの時間が1500us以上のとき、Sweep信号を読み取れる可能性があり、本プログラムでは下から22行目あたりの"else"で処理しています。とりあえず現在は、信号のフェーズが始まってから信号取得までの経過時間を保存しているだけです。
おわりに
今回やってみて、やはりプログラミングは本当に苦手です。自分が管理できない速さで処理されるとなおさらわかりづらくなりますね。さらに日本語の資料が無いと、なおさらやる気を出して取り掛からなければなりません。もっと賢くなりたいなぁ。。。
それでは、次の自作VIVE tracker計画の記事では、2つのBase Stationから座標推定するところまで実装出来たらにしたいと思います。
