極小LiDAR（マルチゾーンToFセンサ） VL53L5CX で遊ぶ
VL53LCXの導入方法です。研究で使うために調べた時のメモです。特に説明するまでもない話なんですが、Lidarとか使いたいロボット系の人は機械出身でArduinoとか苦手かもしれないのでメモとして残しておきます。
なんのデバイス？
STMが面白いTOFセンサを販売しています。その名もVL53LCXです。簡単に言ってしまえば凄く安価で、8x8の解像度を持つ、極小のLiDARです。
解像度はファミコンのスプライト並みに低いですが、ハンドジェスチャーの認識などをターゲットとしているらしく、時系列処理することで、1フレームから得られる情報よりも複雑な情報が取り出せるはずです。使い方によってはCHLAC特徴量なんかとも相性が良いのではないでしょうか？まあこの規模のテンソルならraspicoなどM0マイコンでも深層学習で処理できてしまいます。（なんだかんだディープでポンッ！でも精度が出やすい畳み込み演算は強い）
スペック
基本的なスペックは以下のような感じです。さらに詳細な内容はデータシート読んでください。
- 視野角：65度
- 解像度：4x4、または8x8
- 最大計測距離：400cm
- フレームレート：
4x4：60Hz
8x8：15Hz - インタフェース：I2C
- 電源：3.3Vの単電源、もしくは3.3Vのアナログ電源と1.8VのIO電源の組み合わせ
デバイス
Qwwickなどからも販売されていますが、私が使っているのはSTM公式のBreakoutBoardです。私はNucleoに接続して使っています。ブレッドボードで接続スレば良いのですが、ToFセンサの性質上、向きを変えるたびにI2Cにノイズが乗ったり、いろいろ面倒くさくなってユニバーサル基板にはんだ付けしています。
私が使っているNucleoはNUCLEO-L476RGです
NucleoとVL53L5CX-SATELの接続のピン対応です
VL53L5CX-SATEL stm32 機能
サンプルプログラムを試す
STMが公開しているサンプルプログラムを試してみます。サンプルコードはSTM32Duinoで提供されているため、まだArduinoを使ったことがない人は適当にArduinoIDEをインストールして下さい。僕の環境はversion:2.03のを使っています。
STM32Duinoの環境構築
stm32duinoの環境をセットアップします。とりあえず上のリポジトリを見ればできると思います。
赤マーカー引いているところにjsonファイル「https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json」と書き込んでokを押せば追加できます。
そしてツール->ボードマネージャでstm32と打つと以下のような候補が出るのでインストールします。
VL53L5CXのサンプルスケッチの追加
VL53L5CXをstm32duino上で動かすことができるサンプルスケッチがSTMから公開されているので、ArduinoIDEに追加します。
リポジトリからzipでダウンロードします
ダウンロードしたzipファイルをArduinoIDEの「スケッチ→ライブラリをインクルード→.ZIP形式のライブラリをインストール」から読み込みます。
サンプルプログラムを動かしてみる
私の環境はNUCLEO-L476RGなのでArduinoIDEのToolから以下のように設定しています。
早速サンプルのスケッチを動かしてみましょう
上の図のようにサンプルプログラムを開いて書き込みます。
そしてシリアルモニタを開くと以下のようなデータが流れてくるはずです。サンプルコードだと4x4モードになっているようです。
ゾーンとそのゾーンの領域ごとの距離が表示されています。マックス4mのはずなんですが、机において実験していて、天井があるので1800くらいがマックスになってますね。データシートからの抜粋ですが、それぞれの領域が相当する箇所は以下のようになっています。
可視化プログラムを作る
stm32のプログラムを改造し、Python上で簡易的に可視化するプログラムを作りましたのでシェアします。
PC側
Pythonの可視化プログラムはＵＡＲＴで読んでOpenCVで表示させているだけです。表示はESCキーで終了します。COMポートは自分の環境に合わせてください。
#!/usr/bin/env python3
import sys
import numpy as np
import serial
import cv2
with serial.Serial('てめぇのCOMポート', 115200, timeout=0.1) as ser:
while(1):
print("fetch")
b_data = ser.read(65*2)
print(len(b_data))
np_data = np.frombuffer(b_data, dtype = np.uint16)
np_buf = np.concatenate([np_buf, np_data])
print(np_buf.shape)
for index, item in enumerate(np_buf):
if(item == 0xFFFF):
slice_data = (np_buf[index+1:index+64+1]/2000*256).astype(np.uint8)
np_buf = np_buf[index+64+1:]
resized_image = slice_data.repeat(100,axis=0).repeat(100,axis=1)
dst = cv2.applyColorMap(resized_image, cv2.COLORMAP_JET)
cv2.imshow('camera' ,dst)
out_image = cv2.resize(slice_data, (800,800),cv2.INTER_CUBIC )
dst = cv2.applyColorMap(out_image, cv2.COLORMAP_JET)
cv2.imshow('by cubic' ,dst)
#繰り返し分から抜けるためのif文
key =cv2.waitKey(10)
if key == 27:
break
stm32側
STM32側のプログラムです。8x8での出力が最高速の１５FPSで動くはずなんですが、なぜかそれ以上出るのでプログラムが間違っているのもしれません。高速化するために出力をバイナリにしているので、シリアルモニタで見ても謎の文字列しか出てこないです。
#include <platform.h>
#include <platform_config.h>
#include <platform_config_default.h>
#include <vl53l5cx_buffers.h>
#include <vl53l5cx_class.h>
#include <vl53l5cx_plugin_detection_thresholds.h>
#include <vl53l5cx_plugin_motion_indicator.h>
#include <vl53l5cx_plugin_xtalk.h>
/**
******************************************************************************
* @file VL53L5CX_Sat_HelloWorld.ino
* @author STMicroelectronics
* @version V1.0.0
* @date 11 November 2021
* proximity sensor satellite based on FlightSense.
* This application makes use of C++ classes obtained from the C
* components' drivers.
******************************************************************************
* @attention
*
* <h2><center>© COPYRIGHT(c) 2021 STMicroelectronics</center></h2>
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/*
* To use these examples you need to connect the VL53L5CX satellite sensor directly to the Nucleo board with wires as explained below:
* pin 1 (GND) of the VL53L5CX satellite connected to GND of the Nucleo board
* pin 2 (IOVDD) of the VL53L5CX satellite connected to 3V3 pin of the Nucleo board
* pin 3 (AVDD) of the VL53L5CX satellite connected to 5V pin of the Nucleo board
* pin 4 (PWREN) of the VL53L5CX satellite connected to pin A5-A0 of the Nucleo board
* pin 5 (LPn) of the VL53L5CX satellite connected to pin A3 of the Nucleo board
* pin 6 (SCL) of the VL53L5CX satellite connected to pin D15 (SCL) of the Nucleo board
* pin 7 (SDA) of the VL53L5CX satellite connected to pin D14 (SDA) of the Nucleo board
* pin 8 (I2C_RST) of the VL53L5CX satellite connected to pin A1 of the Nucleo board
* pin 9 (INT) of the VL53L5CX satellite connected to pin A2 of the Nucleo board
*/
/* Includes ------------------------------------------------------------------*/
#include <vl53l5cx_class.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <assert.h>
#include <stdlib.h>
#define DEV_I2C Wire1
#else
#endif
#define SerialPort Serial
#ifndef LED_BUILTIN
#define LED_BUILTIN 13
#endif
#define LedPin LED_BUILTIN
#define LPN_PIN A3
#define I2C_RST_PIN A1
#define PWREN_PIN A5
// Components.
VL53L5CX sensor_vl53l5cx_sat(&DEV_I2C, LPN_PIN, I2C_RST_PIN);
/* Setup ---------------------------------------------------------------------*/
void setup()
{
// Led.
pinMode(LedPin, OUTPUT);
// Enable PWREN pin if present
if (PWREN_PIN >= 0) {
pinMode(PWREN_PIN, OUTPUT);
digitalWrite(PWREN_PIN, HIGH);
delay(10);
}
// Initialize serial for output.
SerialPort.begin(115200);
SerialPort.println("Initialize... Please wait, it may take few seconds...");
// Initialize I2C bus.
DEV_I2C.begin();
DEV_I2C.setClock(1000000);
// Configure VL53L5CX satellite component.
sensor_vl53l5cx_sat.begin();
sensor_vl53l5cx_sat.init_sensor();
// Start Measurements
sensor_vl53l5cx_sat.vl53l5cx_set_resolution(VL53L5CX_RESOLUTION_8X8);
sensor_vl53l5cx_sat.vl53l5cx_set_ranging_frequency_hz(15);
sensor_vl53l5cx_sat.vl53l5cx_start_ranging();
}
void loop()
{
static uint8_t loop_count = 0;
VL53L5CX_ResultsData Results;
uint8_t NewDataReady = 0;
char report[64*2];
uint8_t status;
uint8_t banpei[] = {0xFF,0xFF};
do {
status = sensor_vl53l5cx_sat.vl53l5cx_check_data_ready(&NewDataReady);
} while (!NewDataReady);
//Led on
digitalWrite(LedPin, HIGH);
status = sensor_vl53l5cx_sat.vl53l5cx_get_ranging_data(&Results);
for(int i =0; i < 64; i++){
int index = i*2;
report[index] = Results.distance_mm[i] & 0x00FF;
report[index+1] = (Results.distance_mm[i] >> 8)& 0x00FF;
}
SerialPort.write(banpei, 2);//データ本体
SerialPort.write(report, 64*2);//データ本体
}
digitalWrite(LedPin, LOW);
}
こんな感じで画面が出るはずです。ちなみに移っているのは私の手です。左側は出力そのまま、右側はバイキュービック補完して表示しています。
今後
まだあまり調べられていないのですが、VL53L7CXという新型がでていて、そっちは視野角が90度もあるらしいので試してみたいです。
