送料込み$10.00の激安LiDARとM5Stackを繋いでみる
激安LiDARって?
Aliexpressで激安のLiDARが販売されているとの記事をTwitterで見かけて、注文してみました。本来は\$20らしいのですが、動作保証無し品が送料込み\$10.00ということのようです。こちらを今回購入してみました。リンクは準備物のところに記載させていただきました。
なお、Aliexpressの商品紹介には記載されていなかったようなのですが、本体には「Camsense X1」との商品名のステッカーが貼られています。たぶんこちらの製品なのだと思います→http://www.camsense.cn/
準備物
- M5Stack
今回はたまたま持っていたM5Stack Fireを使用しましたが、未確認ですがM5Stack系どれでもOKだと思います。今回作成したコードはM5Stackの表示部分以外はArduino APIしか使用していないので、一般的なArduinoで使用できると思います。
- LiDAR Camsense X1
私は以下の「動作保証無し」の機器を送料込み$10で購入しました。
Aliexpress : laser radar 360 degree laser radar scanning distance measuring sensor diy wireless transmission infrared data transmission
以下だと本体が$19になりますが、動作確認品なのかと思います。
Aliexpress : laser radar 360 degree laser radar scanning distance measuring sensor diy wireless transmission infrared data transmission
- Groveケーブル
M5Stackの側面のGroveコネクタの信号を使用してLiDARと接続している為、ケーブルを使用しました。他の端子に変更すればケーブル不要です。
- 5V電源
LiDAR用の電源です。私の環境では、M5Stackの5V出力では安定してモーターが回転しなかったため用意しました。
- 線材等
機器をつなぐのに使用します。
M5Stackとの接続方法
本LiDARは外部との接続用にコネクタが設けられていますが、対となるコネクタを持っていなかったので、画像のように、ランドから線出しをしています。この図では上から、TX(シリアル出力、3.3V系)、GN(GND)、VC(VCC=5V)になっています。
これをM5Stack側面のGrove Aコネクタに接続します。
また、LiDARのVCC、GNDに5V電源を接続します。
シリアル通信の内容
こちらの記事「激安LiDAR(Camsense X1)を使ってみる」の通信プロトコルの説明を参照させていただきました。
M5Stackからの制御
LiDARからは常にUARTから情報が出力されているので、M5StackのGrove AコネクタをUARTに設定すると、そのままデータを受信することができます。あとはヘッダ部を受信したら、そのまま続くデータを取り込み、画面にプロットしていくだけです。
以下のGithubにもコミットしています。
https://github.com/yishii/LiDAR_Camsense_X1_M5Stack
#include <M5Stack.h>
#define LIDARSerial Serial1
typedef struct
{
uint16_t x;
uint16_t y;
} Point_t;
typedef enum
{
STATE_WAIT_HEADER = 0,
STATE_READ_HEADER,
STATE_READ_PAYLOAD,
STATE_READ_DONE
} State_t;
typedef struct {
uint8_t header0;
uint8_t header1;
uint8_t header2;
uint8_t header3;
uint16_t rotation_speed;
uint16_t angle_begin;
uint16_t distance_0;
uint8_t reserved_0;
uint16_t distance_1;
uint8_t reserved_1;
uint16_t distance_2;
uint8_t reserved_2;
uint16_t distance_3;
uint8_t reserved_3;
uint16_t distance_4;
uint8_t reserved_4;
uint16_t distance_5;
uint8_t reserved_5;
uint16_t distance_6;
uint8_t reserved_6;
uint16_t distance_7;
uint8_t reserved_7;
uint16_t angle_end;
uint16_t crc;
} __attribute__((packed)) LidarPacket_t;
const uint8_t header[] = { 0x55, 0xaa, 0x03, 0x08 };
void setup(void) {
M5.begin();
// M5.Lcd.setRotation(1);
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.printf("Start\n");
Serial.begin(115200);
LIDARSerial.begin(115200, SERIAL_8N1, 21, 22);
delay(10);
Serial.printf("Start\n");
}
uint16_t convertDegree(uint16_t input)
{
return (input - 40960) / 64;
}
uint16_t convertSpeed(uint16_t input)
{
return input / 64;
}
void remapDegrees(uint16_t minAngle, uint16_t maxAngle, uint16_t *map)
{
int16_t delta = maxAngle - minAngle;
if (maxAngle < minAngle) {
delta += 360;
}
if ((map == NULL) || (delta < 0)) {
return;
}
for (int32_t cnt = 0; cnt < 8; cnt++)
{
map[cnt] = minAngle + (delta * cnt / 7);
if (map[cnt] >= 360) {
map[cnt] -= 360;
}
}
}
void plotDistanceMap(uint16_t* degrees, uint16_t* distances)
{
int32_t i;
uint32_t x, y;
static Point_t pointCloud[360]; // 360度分の点群
for (i = 0; i < 8; i++) {
M5.Lcd.drawPixel(pointCloud[degrees[i]].x, pointCloud[degrees[i]].y, BLACK);
if (distances[i] < 1000) {
x = cos((1.f * PI * degrees[i]) / 180) * (distances[i] / 10) + 160;
y = sin((1.f * PI * degrees[i]) / 180) * (distances[i] / 10) + 120;
M5.Lcd.drawPixel(x, y, WHITE);
pointCloud[degrees[i]].x = x;
pointCloud[degrees[i]].y = y;
}
}
}
void loop() {
static State_t state;
static uint32_t counter;
static uint8_t payload[64];
if (LIDARSerial.available())
{
uint8_t data = LIDARSerial.read();
switch (state)
{
case STATE_WAIT_HEADER:
if (data == header[0]) {
counter++;
payload[0] = data;
state = STATE_READ_HEADER;
} else {
//printf("?? (%02X) Please do LiDAR power cycle\n", data);
LIDARSerial.flush();
}
break;
case STATE_READ_HEADER:
if (data == header[counter]) {
payload[counter] = data;
counter++;
if (counter == sizeof(header)) {
state = STATE_READ_PAYLOAD;
}
} else {
counter = 0;
state = STATE_WAIT_HEADER;
}
break;
case STATE_READ_PAYLOAD:
payload[counter] = data;
counter++;
if (counter == sizeof(LidarPacket_t)) {
state = STATE_READ_DONE;
}
break;
case STATE_READ_DONE:
LidarPacket_t* packet = (LidarPacket_t*)payload;
{
uint16_t degree_begin;
uint16_t degree_end;
degree_begin = convertDegree(packet->angle_begin);
degree_end = convertDegree(packet->angle_end);
if ((degree_begin < 360) && (degree_end < 360)) {
printf("%3drpm %5d - %5d\n", convertSpeed(packet->rotation_speed), convertDegree(packet->angle_begin), convertDegree(packet->angle_end));
uint16_t map[8];
uint16_t distances[8];
remapDegrees(degree_begin, degree_end, map);
distances[0] = packet->distance_0;
distances[1] = packet->distance_1;
distances[2] = packet->distance_2;
distances[3] = packet->distance_3;
distances[4] = packet->distance_4;
distances[5] = packet->distance_5;
distances[6] = packet->distance_6;
distances[7] = packet->distance_7;
plotDistanceMap(map, distances);
}
}
// M5.Lcd.setCursor(0, 0);
// M5.Lcd.printf("Speed : %d rpm \n", convertSpeed(packet->rotation_speed)); state = STATE_WAIT_HEADER;
counter = 0;
state = STATE_WAIT_HEADER;
break;
}
}
}
まとめ、謝辞
以下のようになりました。
送料込み$10.00のLIDARセンサー買ってみて、M5Stackで表示するプログラム作ってみたけどとりあえず使えた!安いよなー https://t.co/3AFNIdKAeO pic.twitter.com/7wfXszI15M
— Yasuhiro ISHII (@yishii) August 28, 2020
本LiDARとLCDが付いたM5Stackとを接続することで、お手軽に周辺の情報を表示することができました。今回これを作成できたのは、通信内容の記事「激安LiDAR(Camsense X1)を使ってみる」があったからです。ありがとうございます。