ESP32-CAMのWiFi自動再接続

 ESP32-CAM、お安いしバッテリつけてラジコンに乗せてFPVみたいにして遊ぶのも楽しいので、何人かの友人も使っているが(ESP-32S剥がすのも慣れた)WiFiルータを再起動すると自動で再接続してくれないのが不便なのでスケッチを修正してみた。

ベースのスケッチはマルチクライアント対応のものを使って、mDNSとSoftAP自動切り替え、SoftAP時のCaptivePortal、UPnPとかを盛り込んでいた。みんな色んな使い方しているので色々盛り込んだ感じ…
なんか問題あったときのファームウェアアップデートも面倒なのでOTAにも対応(ブラウザ上からbinファイルをアップロードする感じ)

本家のアップデートが止まっているようなので今回はついでにESP32 Arduino Core 2.0.4に対応させてみた。(そのままだと1.0.5か1.0.6じゃないとビルドできない)

バリエーションがあるんだけどいつもesp32-mjpeg-multiclient-espcam-driversの中のesp32-cam-rtosをベースに改良しているので、(いじっているのはinoファイルだけ)これで作業している前提でアップデート手順。

まずはこのフォルダ内の"esp32-cam-rtos.ino"ファイルと"camera_pins.h"の2つのファイルを残して他のファイルを削除。これ以外のファイルはESP32 Camera Driverなので今回はこれをアップデートするため、削除しておく感じ。

ESP32 Camera Driverから最新のカメラドライバをダウンロードしてくる。今回は2.0.1のZIPを使用した。ZIPをそのままエクスプローラで開いて"examples"フォルダと"test"フォルダを削除する。続いてesp32-camera-master/targetの中のesp32s2とesp32s3フォルダを削除。そんでもってこのZIPファイルをフォルダ構成なしで展開する。今回はWinRARを使って高度な設定でパスを無視にチェックを入れて解答することでいけた。

あとは解凍したファイルを"esp32-cam-rtos.ino"ファイルと"camera_pins.h"の2つのファイルと一緒のディレクトリにコピーしてビルドするだけ。これでArduino Core ESP32 2.0.4でもエラーなしでコンパイルすることができるようになった。

しかしArduino Core 2.0.4とCamera Driver 2.0.1の組み合わせだとメモリ不足でうまく起動しなくなってしまったのでxTaskCreatePinnedToCoreでマルチタスクを起動しているところの3箇所目、Start mainstreaming RTOS taskのスタックサイズを 2 * 1024 から 3 * 1024 に変更することでうまく動かすことができた。

とりあえずこれで最新の環境でビルドできるようになったところで、本題のWiFiのアクスポイントが再起動すると自動で再接続してくれない対策。おそらくESP32-CAMにかぎらず常時APに接続して運用しているESP32搭載デバイスには使える手法かも。

方法は何種類かあると思うんだけど、定期的にWiFi.status()でWiFiの接続情報を確認して再接続する方法と、WiFiEventのWiFi切断されたときのイベントを使用して再接続する方法があるらしい。今回はWiFiEventを使用してみた。

Arduino Core 2.0.xと1.0.xでちょっとイベント名が変わったりしているので注意かも。今回は2.0.4用。

まずはWiFi切断イベントがトリガされたときに読み込まれる関数を作成。

void WiFiStationDisconnected(WiFiEvent_t event, WiFiEventInfo_t info){
  //digitalWrite(LED_BUILTIN, LOW);
  Serial.println("Disconnected from WiFi access point");
  Serial.print("WiFi lost connection. Reason: ");
  Serial.println(info.wifi_sta_disconnected.reason);
  Serial.println("Trying to Reconnect");
  WiFi.begin();
  //digitalWrite(LED_BUILTIN, HIGH);
}

これをスケッチの何処かに入れておく。
そんでもってsetupの何処かに

WiFi.onEvent(WiFiStationDisconnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_DISCONNECTED);

を入れる。(1行)この行が読み込まれた瞬間にESP32のWiFi切断イベントが有効になるのでWiFi接続後とかがいいかも。APモードでもWiFiに繋がってないときはトリガされてしまったので、SoftAPで起動するときは有効にしないような感じにしておいた。

一応これでAPを再起動しても何度かイベントが発生してしまうけど問題なくWiFiに再接続することができた。

そういえばESP32-CAM、何台か購入していると基板の厚みが1.6と1.0があったり、RSTがピンヘッダに出ていたり、カメラの縦横が違かったり、カメラの画質も違うんだよねぇ…
カメラの画質に関してはレンズなのかねぇ(ただしレンズも互換性なかったりしている)
技適対応用のESP-WROOM-32Dもディスコンだし、ESP32-WROVER版とかあるとスッキリしていいんだけどねぇ

ESP32でLiDAR LDS-006からデータを取得してみる。

 この前ROSでLDS-006からデータを取得したとき、このLiDARはスペック的に1週5Hzぐらいのデータしか取れなかったのでESP32でも行けるんじゃないかということでやってみることに。

LDS-006は電源は5V何だけど信号ラインは3.3VレベルなのでESP32に直結できる。ESP32に接続して距離データを取得してみることに。

#include <Arduino.h>

#define RXD2 16
#define TXD2 17

// --- グローバル変数 ---
uint16_t lidar360[360];   // 1周分距離データ
uint32_t lastTime = 0;    // 前回受信時刻
uint16_t lastRpm = 0;     // 最後のRPM
bool newDataReady = false; // 1周分揃ったらtrue

QueueHandle_t lidarQueue;

struct LidarInfo {
  uint16_t rpm;
  uint32_t dt; // 前回からの時間差(ms)
};

void taskLidarRead(void *pvParameters) {
  uint8_t packet[22];
  int packetIndex = 0;
  static int count = 0;

  while (1) {
    while (Serial2.available()) {
      uint8_t b = Serial2.read();

      // パケット同期
      if (packetIndex == 0 && b != 0xFA) continue;
      packet[packetIndex++] = b;

      if (packetIndex == 22) {
        if (packet[0] == 0xFA) {
          uint8_t index = packet[1] - 0xA0;
          if (index <= 89) { // 0〜89ブロック
            // 4点ずつ距離データ取得
            for (int i = 0; i < 4; i++) {
              int offset = 4 + i * 4;
              uint16_t dist = ((packet[offset + 1] & 0x3F) << 8) | packet[offset + 0];
              int angle = index * 4 + i;

              if (angle < 360) {
                lidar360[angle] = dist;
              }
            }

            // パケット0でRPMとΔtを計算
            if (index == 0) {
              uint16_t speed = (packet[3] << 8) | packet[2];
              uint16_t rpm = speed / 64;

              uint32_t now = millis();
              uint32_t dt = (lastTime == 0) ? 0 : (now - lastTime);
              lastTime = now;
              lastRpm = rpm;

              LidarInfo info = { rpm, dt };
              xQueueSend(lidarQueue, &info, 0);
            }

            // 1周揃ったらフラグ
            if (index == 89) {
              newDataReady = true;
            }
          }
        }
        packetIndex = 0;
      }
    }
    vTaskDelay(1);
  }
}

void taskPcSend(void *pvParameters) {
  LidarInfo info;
  while (1) {
    if (xQueueReceive(lidarQueue, &info, portMAX_DELAY)) {
      Serial.print("RPM=");
      Serial.print(info.rpm);
      Serial.print("  Δt=");
      Serial.print(info.dt);
      Serial.println(" ms");

      // 1周分距離データが揃ったらカンマ区切りで送信
      if (newDataReady) {
        for (int i = 0; i < 360; i++) {
          Serial.print(lidar360[i]);
          if (i < 359) Serial.print(",");
        }
        Serial.println();
        newDataReady = false;
      }
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);

  delay(100);
  Serial2.print("startlds$"); // LDS-006 起動

  lidarQueue = xQueueCreate(16, sizeof(LidarInfo));
  if (!lidarQueue) {
    Serial.println("Queue create failed!");
    while (1);
  }

  xTaskCreatePinnedToCore(taskLidarRead, "LidarRead", 4096, NULL, 3, NULL, 1);
  xTaskCreatePinnedToCore(taskPcSend, "PcSend", 4096, NULL, 1, NULL, 1);
}

void loop() {
  
}

最初はloopの中で全部やっていたんだけどデータの取りこぼしがあったので別タスクでやってみることにした。200ms周期でデータが取得できているのでぴったり5Hzで１周分のデータが揃う用になっていた。回転数は465rpmから470rpmをキープしている。

LDS-006のデータ更新周期は5Hzってことがわかったのであとは距離を測ってみないと。

あとはどうやってこのデータを処理すればよいのか…

BOSCH BNO055をROSで使ってみた。

 LiDARをROSで使えるようにしたのはいいんだけど、SLAMのサンプルを試してみたらやっぱりIMUもあったほうが面白いんじゃないかということでROSで使えるIMUを探してみた。

MPU-6050(6軸)とかMPU-9250(9軸)はArduinoとかでもよく使われていて、DMPを使えばフィルタして安定しているデータを得られるんだけど、ラズパイとかだとI2Cで直結できたりするかもしれないけど、PCだとちょっと不便。WitMotionからカルマンフィルタ通したデータがワイヤレスやシリアル通信で得られるモジュールも出てるっぽいけど。

そこでカルマンフィルタ付きのIMUを探していたら、BoschのBNO055っていうセンサーがマイコン内蔵で出力がI2CとUARTが選べるという。これならUARTでUSB変換通せば簡単にPCに取り込めそう。更にはROSのドライバもあるようだ。

秋月だと1680円だし、WitMotionのモジュールよりも安い。ケース無いけど。

秋月のこういったモジュールは結構DIPサイズに変換してくれるのが多い気がする。

ブレッドボードに挿しやすくて便利。

ジャンパは表面に全部揃ってる。最初からI2Cモードになってる。

ジャンパには0604のジャンパ抵抗が乗ってるぞ…

両面実装なので、ユニバーサル基板に装着する際もベタ付にはできなさそう。

レベルコンバータとレギュレータも内蔵してるので5Vでも3.3Vでも使用できるみたい。入力した電圧にレベル変換されるっぽい。今回はUSBシリアル変換にCP2102を使用したので3.3Vで使用することに。

まずはROSでUARTで使用するためにI2CモードからUARTモードにジャンパを変更。J5のジャンパ抵抗をハンダで外して、J3にハンダを盛ってジャンパする。

あとはUSBシリアル変換アダプタにつなぐだけ。

まずはrivzでIMUを使用できるようなプラグインをapt-getでインストール。

sudo apt-get install ros-$(rosversion -d)-rviz-imu-plugin

ROS用のドライバにはros_imu_bno055を使用させてもらった。

インストールは説明されている通りのコマンドコピペでcatkinベースでビルドできた。

rivzでそのままセンサーの状態が表示できた。

さすが内蔵のマイコンでフィルターを掛けているだけあって安定している。ArduinoでMPU-6050の生データを見ていたときはZ回転がかなりドリフトしていたのでIMUは難しそうと感じていたけど(DMP使ったら問題なかったけど)、簡単にフィルタ後のデータをUARTで出せるBNO055は流石だ…

最初はArduino+MPU-6050でDMPつかってUARTで出力とか考えていたけど、BNO055つかってみて正解だったかも。

ロボット掃除機のLiDARでHector SLAMを試してみた。

 前回ROSで使えることがわかったロボット掃除機のLiDAR LDS-006でHector SLAMが使えるかどうか試してみた。IMUとかは無しでロボット掃除機に搭載されているレベルのLiDARだけでどのぐらい自己位置推定ができるのかどうか。

Hector SLAMの使い方はRPLidarでHector SLAMを使っている人のものを参考にさせていただいた。検索すると結構出てくるのでそちらは割愛。
前回のROSのテストをした際にframe IDをlaserにしておいて正解だったかも。ほぼコピペでそのまま動いた。

とりあえず自分自身がじゃまになるんじゃないかということで頭の上にLiDARをのっけて少し動いてみた。

緑色の線が動いた軌跡。こんな簡単にSLAMが試せるのか…

ただしLiDARを回したり上下したりするとこんな感じに。スキャンレートが遅いからなのか、回転には弱い気がする。設定が悪いのか、LDS-006の限界なのか…

IMUとかのセンサを組み合わせたサンプルとかあるのかな？回転方向とか傾きとかはIMUのほうが検出しやすそうだし。

LDS-006 LiDARをROSで使ってみた。

 この前購入した格安LiDAR、とりあえず動くところまでわかったので今度はROSで使えないかどうか調べてみた。

ROSで使えるLiDARで似たような形状しているやつだとRPLidarとか、同じようにUARTで通信しているタイプでLaserScanメッセージ対応のROSドライバベースで作れないかと、LiDARの仕様書や使い方のページとにらめっこして、近いプロトコルないかなーとか探してみた。

その過程で見つけたNeato XV-11っていう掃除ロボットに乗ってるLiDARがLDS-006のプロトコルにそっくりということを発見。

そっくりということは実は同じプロトコルでLDS-006の解析結果が間違っているんじゃないかという疑いが…
たしかに他のLiDAR用に作られたツールを改造してそこそこ使えそうな気はしていたんだけど、定期的に変なデータが来て値が飛んでいた。

そこで、解析してみるとLDS-006の距離データの部分に実はエラーフラグとエラーコードが含まれていたっぽい。これはXV-11の方のプロトコル解析結果を見てもしかしてと思って、距離のデータのHigh Byteの上位2ビット目を観察するようにしてみたら判明した。上位2ビットが0以外になると実は距離データのLow Byteがエラーコードになるようで、それが距離データとしては特定の角度で一定の変な値が出ることを確認。

というわけでLDS-006はXV-11のプロトコル互換ということで、XV-11がROS対応しているのをいいことにそのままXV-11用のドライバでROSで動かしてみた。
LDS-006にはモータ制御のコマンドが存在するのでモータ制御のコマンドは自分で実装するか手動で送信するしか無いけど。("startlds$"で回転、"stoplds$"で停止)

2.0mmピッチのコネクタも一応2.54mmピッチに変換して(4pinぐらいなら半田でなんとかなる)USB-Serial変換アダプタはCP2102搭載のものを使用した。

使用したドライバはxv_11_laser_driver
これはパッケージも提供されているようなのでapt-getでインストールできた。

あとはroscoreを動かしたあとに別なターミナルで

echo "startlds$" > /dev/ttyUSB0

をするとLDS-006が回り始める。
次に

rosrun xv_11_laser_driver neato_laser_publisher _port:=/dev/ttyUSB0 _firmware_version:=2 _frame_id:=laser

これでROSでLiDARのデータを解析してROSに渡してくれる。firmware versionはXV11のLiDARは通信プロトコルが2種類あるので2のほうがLDS-006と互換のほう。frame idはデフォルトでneato_laserになってるんだけど、laserにしたほうがRPLidarとかの使い方のサイトを参考にしたときにそのままコピペできるので便利だと思ってこのオプションを追加して変更した。

/dev/ttyUSB0は環境に合わせて変更。Permission Errorが出たので

sudo chmod a+rw /dev/ttyUSB0

してやった。適当だけど動いたので良し…(あとでちゃんとグループ調べて追加しないと…

あとはRVizを開いてFixed Frameにlaserって入れれば見れるはず。

データの取得ができた。やっぱりエラーフラグもちゃんと処理してるのか、動かしても変なデータが出ないのでいい感じ。

xv11が5ｍなので5mまでしか見ていないかもしれないけど、LDS-006がどのぐらい届くかは見てみないとなぁ

ちなみに分解能はやっぱり1°で、スキャン周波数は5Hzぐらいだった。

ROS対応でこのぐらいの性能なら1万円ぐらいの低価格帯のLiDARレベルかもしれない。

とりあえずRPLIDARと同等ぐらいにはROSで扱えそうな気がしてきたので、RPLIDARの情報を参考にSLAMとか試してみようかな。

Xbox OneコントローラをBLE対応にアップデートしてみた。

 2019年10月に4.8.1923.0にアップデートされたXbox Oneコントローラ(2世代)にBLE対応などのファームウェアアップデートが来ていたのでアップデートしてみた。

Xbox本体を持っていないのでWindows 10のXbox アクセサリーアプリからアップデートを行うことができた。

ちなみにShareボタンが追加されたり、TypeC端子になっている3世代目のコントローラはおそらくもうすでにBLE対応していると思う。

自分のはMicroUSB接続な第2世代なのでアップデートをしてみた。1世代目はそもそもBluetoothに対応していない。

まずはコントローラーをUSBで接続。

Xbox アクセサリーアプリを起動すると認識されるはずなので「…」をクリック。

自分の場合は「アップデートはありません」と表示されていたのだが、

試しに「アップデートはありません」をクリックしてみると続けるボタンが表示されたので押してみた。

続けるをクリック。

コントローラーが点滅しはじめるので1分ほど待つ。

1分ぐらい待っていると画面が変わるので次へをクリック。

ファームウェア バージョン 5.13.3143.0になっていれば完了！

このアップデートによりShareボタンがついてる第3世代のコントローラーと同じバージョンになった。
このバージョンになるとiOSやAndroid端末でBLE対応コントローラーとして使用できたり、ペアリング済みデバイスを切り替えられるようになるとか。

ちなみにWindows 10とペアリングし直してみたらバッテリ残量も見れるようになっていた。これはMicrosoftのBLEマウスもそうだったのでBLEのバッテリ残量サービスに対応してるのかな。

TAMIYAユニバーサルプレートが届いた。

 仲間内で電子工作で何か動くものを作ろうということで、タミヤの楽しい工作シリーズのユニバーサルプレートを久しぶりに購入してみた。とりあえずタイヤで走るにしてもクローラにするにしても必要なので…

昔作った車体もまだあるけど、いろんなことを試すのにもう一台車体を作ってしまったほうがいいかなということなんだけど、ここんところ在庫が全くない。タムタムとかマルツに行ったけどギアボックスはあるのにユニバーサルプレートがない。ということでAmazonでポチって1ヶ月以上かかって届いた。249円だった。

No.98のユニバーサルプレートは昔ながらのネジ付きで色は木材ぽい感じの色。No.157とかの2枚セットだと色がグレーで、ネジの代わりにプラリベットが付属しているらしい。

昔から変わっていない気がする。

フィリピン製らしい。近頃金属とか樹脂が不足している影響がでて生産がうまく行ってなかったのかなぁ？値上がりとかしないといいけど。

さてモータとかタイヤとかどうしようかなぁ

とりあえず制御はESP32かRaspberry Pi Picoあたりで、モータドライバはDRV8833あたりが安くていいんじゃないかなーと。