この前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で1周分のデータが揃う用になっていた。回転数は465rpmから470rpmをキープしている。
LDS-006のデータ更新周期は5Hzってことがわかったのであとは距離を測ってみないと。
あとはどうやってこのデータを処理すればよいのか…
0 件のコメント:
コメントを投稿