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ってことがわかったのであとは距離を測ってみないと。

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