🛸
STMicro VL53L5CXをRaspberry Pi 5で使ってみる
エリア検出できる距離センサーを探していたらSTMicroのVL53L5CXというセンサーを見つけたのでRaspberry Pi 5での使い方をまとめてみた。
センサーの仕様
センサーの仕様は以下のページの通り。画角は63°で最大4mまでの測定が可能。8x8のエリアを15Hzでスキャンできる。発行レーザーでスキャンするが、レーザーの波長は940nm。昼間の屋外だと厳しいかも？？電流は100mAくらい消費するらしい。
試したモジュール
スイッチサイエンスさんからContaモジュールとして販売されているのでこれを使った。MakerFairで購入したので少しだけお安かった。
ドライバの作成
STMicroのホームページからRaspberry Pi用のドライバのソースコードをダウンロードする。
ダウンロードにはSTMicroのアカウントがないとできないので必要に応じて作っておく。
展開してビルドしようとすると、Kernel 6.12ではI2Cのプローブ方法が変更になっていたのでビルドエラーが出る。なのでソースを修正する。修正するファイルはstmvl53l5cx.c。これを以下の内容で置き換える。
stmvl53l5cx.c
stmvl53l5cx.c
// SPDX-License-Identifier: GPL-2.0
/*
* Driver for VL53L5CX dTOF sensors
*
* Copyright (C) STMicroelectronics SA 2023
*/
#include <linux/module.h>
#include <linux/i2c.h>
#include <linux/miscdevice.h>
#include <linux/version.h>
#include <linux/spi/spi.h>
#include <linux/uaccess.h>
#include <linux/gpio/consumer.h>
#include <linux/interrupt.h>
#include <linux/fs.h>
#define VL53L5CX_COMMS_CHUNK_SIZE 1024
#define ST_TOF_IOCTL_TRANSFER _IOWR('a',0x1, struct stmvl53l5cx_comms_struct)
#define ST_TOF_IOCTL_WAIT_FOR_INTERRUPT _IO('a',0x2)
struct stmvl53l5cx_drvdata {
struct i2c_client *client;
int irq;
struct miscdevice misc;
uint8_t * reg_buf; /*[0-1]: register, [2...]: data*/
atomic_t intr_ready_flag;
wait_queue_head_t wq;
int dev_num;
};
struct stmvl53l5cx_comms_struct {
__u16 len;
__u16 reg_index;
__u8 write_not_read;
__u8 padding[3]; /* 64bits alignment */
__u64 bufptr;
};
static int stmvl53l5cx_i2c_read(struct stmvl53l5cx_drvdata *drvdata, uint32_t count)
{
int ret = 0;
struct i2c_client *client = drvdata->client;
uint8_t * reg_buf = drvdata->reg_buf;
uint8_t * data_buf = drvdata->reg_buf + 2;
struct i2c_msg msg[2];
msg[0].addr = client->addr;
msg[0].flags = client->flags;
msg[0].buf = reg_buf;
msg[0].len = 2;
msg[1].addr = client->addr;
msg[1].flags = I2C_M_RD | client->flags;
msg[1].buf = data_buf;
msg[1].len = count;
ret = i2c_transfer(client->adapter, msg, 2);
if (ret != 2) {
pr_err("%s: err[%d]\n", __func__, ret);
ret = -1;
} else {
ret = 0;
}
return ret;
}
static int stmvl53l5cx_i2c_write(struct stmvl53l5cx_drvdata *drvdata, uint32_t count)
{
int ret = 0;
struct i2c_client *client = drvdata->client;
struct i2c_msg msg;
msg.addr = client->addr;
msg.flags = client->flags;
msg.buf = drvdata->reg_buf;
msg.len = count + 2;
ret = i2c_transfer(client->adapter, &msg, 1);
if (ret != 1) {
pr_err("%s: err[%d]\n", __func__, ret);
ret = -1;
} else {
ret = 0;
}
return ret;
}
static int stmvl53l5cx_read_regs(struct stmvl53l5cx_drvdata *drvdata, uint16_t reg_index,
uint8_t *pdata, uint32_t count, char __user *useraddr)
{
int ret = 0;
uint8_t * reg_buf = drvdata->reg_buf;
uint8_t * data_buf = drvdata->reg_buf + 2;
reg_buf[0] = (reg_index >> 8) & 0xFF;
reg_buf[1] = reg_index & 0xFF;
if (drvdata->client) {
ret = stmvl53l5cx_i2c_read(drvdata, count);
} else {
pr_err("%s:%d input wrong params\n", __func__, __LINE__);
ret = -1;
return ret;
}
if (ret == 0) {
if (useraddr) {
ret = copy_to_user(useraddr, data_buf, count);
if (ret) {
pr_err("%s:%d error[%d]\n", __func__, __LINE__, ret);
return ret;
}
} else if (pdata) {
memcpy(pdata, data_buf, count);
} else {
ret = -1;
pr_err("%s: input null data buffer\n", __func__);
return ret;
}
}
return ret;
}
static int stmvl53l5cx_write_regs(struct stmvl53l5cx_drvdata *drvdata, uint16_t reg_index,
uint8_t *pdata, uint32_t count, char __user *useraddr)
{
int ret = 0;
uint8_t * reg_buf = drvdata->reg_buf;
uint8_t * data_buf = drvdata->reg_buf + 2;
reg_buf[0] = (reg_index >> 8) & 0xFF;
reg_buf[1] = reg_index & 0xFF;
if (useraddr) {
ret = copy_from_user(data_buf, useraddr, count);
if (ret) {
pr_err("%s:%d error[%d]\n", __func__, __LINE__, ret);
return ret;
}
} else if (pdata) {
memcpy(data_buf, pdata, count);
} else {
ret = -1;
pr_err("%s: input null data buffer\n", __func__);
return ret;
}
if (drvdata->client) {
ret = stmvl53l5cx_i2c_write(drvdata, count);
} else {
ret = -1;
}
return 0;
}
static int stmvl53l5cx_read_write(struct stmvl53l5cx_drvdata *drvdata, uint16_t reg_index,
char __user *useraddr, uint32_t count, uint8_t write_not_read)
{
int ret = 0;
uint16_t chunks = count / VL53L5CX_COMMS_CHUNK_SIZE;
uint16_t offset = 0;
while (chunks > 0) {
if (write_not_read) {
ret = stmvl53l5cx_write_regs(drvdata, reg_index + offset, NULL,
VL53L5CX_COMMS_CHUNK_SIZE, useraddr + offset);
} else {
ret = stmvl53l5cx_read_regs(drvdata, reg_index + offset, NULL,
VL53L5CX_COMMS_CHUNK_SIZE, useraddr + offset);
}
if (ret) {
pr_err("%s:%d r/w[%d], err[%d]\n", __func__, __LINE__, write_not_read, ret);
return ret;
}
offset += VL53L5CX_COMMS_CHUNK_SIZE;
chunks--;
}
if (count > offset) {
if (write_not_read) {
ret = stmvl53l5cx_write_regs(drvdata, reg_index + offset, NULL,
count - offset, useraddr + offset);
} else {
ret = stmvl53l5cx_read_regs(drvdata, reg_index + offset, NULL,
count - offset, useraddr + offset);
}
if (ret) {
pr_err("%s:%d r/w[%d], err[%d]\n", __func__, __LINE__, write_not_read, ret);
}
}
return ret;
}
static long stmvl53l5cx_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
int ret = 0;
struct stmvl53l5cx_drvdata *drvdata = container_of(file->private_data,
struct stmvl53l5cx_drvdata, misc);
struct stmvl53l5cx_comms_struct comms_struct = {0};
void __user *data_ptr = NULL;
pr_debug("stmvl53l5cx_ioctl : cmd = %u\n", cmd);
switch (cmd) {
case ST_TOF_IOCTL_WAIT_FOR_INTERRUPT:
pr_debug("%s(%d)\n", __func__, __LINE__);
ret = wait_event_interruptible(drvdata->wq,
atomic_read(&drvdata->intr_ready_flag) != 0);
atomic_set(&drvdata->intr_ready_flag, 0);
if (ret) {
pr_info("%s: wait_event_interruptible err=%d\n", __func__, ret);
return -EINTR;
}
break;
case ST_TOF_IOCTL_TRANSFER:
ret = copy_from_user(&comms_struct, (void __user *)arg, sizeof(comms_struct));
if (ret) {
pr_err("%s:%d err[%d]\n", __func__, __LINE__, ret);
return -EINVAL;
}
pr_debug("[0x%x,%d,%d]\n", comms_struct.reg_index,
comms_struct.len, comms_struct.write_not_read);
if (!comms_struct.write_not_read) {
data_ptr = (u8 __user *)(uintptr_t)(comms_struct.bufptr);
ret = stmvl53l5cx_read_write(drvdata, comms_struct.reg_index, data_ptr,
comms_struct.len, comms_struct.write_not_read);
} else {
ret = stmvl53l5cx_read_write(drvdata, comms_struct.reg_index,
(char *)(uintptr_t)comms_struct.bufptr,
comms_struct.len, comms_struct.write_not_read);
}
if (ret) {
pr_err("%s:%d err[%d]\n", __func__, __LINE__, ret);
return -EFAULT;
}
break;
default:
return -EINVAL;
}
return 0;
}
#ifdef CONFIG_COMPAT
static long stmvl53l5cx_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
return stmvl53l5cx_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
}
#endif
static const struct file_operations stmvl53l5cx_fops = {
.owner = THIS_MODULE,
.unlocked_ioctl = stmvl53l5cx_ioctl,
#ifdef CONFIG_COMPAT
.compat_ioctl = stmvl53l5cx_compat_ioctl,
#endif
};
/* Interrupt handler */
static irqreturn_t stmvl53l5cx_intr_handler(int irq, void *dev_id)
{
struct stmvl53l5cx_drvdata *drvdata = (struct stmvl53l5cx_drvdata *)dev_id;
atomic_set(&drvdata->intr_ready_flag, 1);
wake_up_interruptible(&drvdata->wq);
return IRQ_HANDLED;
}
static int stmvl53l5cx_parse_dt(struct device *dev, struct stmvl53l5cx_drvdata *drvdata)
{
int ret = 0;
struct gpio_desc *gpio_int, *gpio_pwr;
gpio_pwr = devm_gpiod_get_optional(dev, "pwr", GPIOD_OUT_HIGH);
if (!gpio_pwr || IS_ERR(gpio_pwr)) {
ret = PTR_ERR(gpio_pwr);
dev_info(dev, "failed to request power GPIO: %d.\n", ret);
}
gpio_int = devm_gpiod_get_optional(dev, "irq", GPIOD_IN);
if (!gpio_int || IS_ERR(gpio_int)) {
ret = PTR_ERR(gpio_int);
dev_err(dev, "failed to request interrupt GPIO: %d\n", ret);
return ret;
}
drvdata->irq = gpiod_to_irq(gpio_int);
if (drvdata->irq < 0) {
ret = drvdata->irq;
dev_err(dev, "failed to get irq: %d\n", ret);
return ret;
}
init_waitqueue_head(&drvdata->wq);
ret = devm_request_threaded_irq(dev, drvdata->irq, NULL,
stmvl53l5cx_intr_handler,
IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
"vl53l5cx_intr", drvdata);
if (ret) {
dev_err(dev, "failed to register plugin det irq (%d)\n", ret);
}
if (of_find_property(dev->of_node, "dev_num", NULL)) {
of_property_read_s32(dev->of_node, "dev_num", &drvdata->dev_num);
dev_info(dev, "dev_num = %d\n", drvdata->dev_num);
} else {
drvdata->dev_num = -1;
}
return ret;
}
static int stmvl53l5cx_detect(struct stmvl53l5cx_drvdata *drvdata)
{
int ret = 0;
uint8_t page = 0, revision_id = 0, device_id = 0;
char devname[16];
ret = stmvl53l5cx_write_regs(drvdata, 0x7FFF, &page, 1, NULL);
ret |= stmvl53l5cx_read_regs(drvdata, 0x00, &device_id, 1, NULL);
ret |= stmvl53l5cx_read_regs(drvdata, 0x01, &revision_id, 1, NULL);
if ((device_id != 0xF0) || (revision_id != 0x02)) {
pr_err("stmvl53l5cx: Error. Could not read device and revision id registers\n");
return -ENODEV;
}
pr_info("stmvl53l5cx: device_id : 0x%x. revision_id : 0x%x\n", device_id, revision_id);
drvdata->misc.minor = MISC_DYNAMIC_MINOR;
drvdata->misc.name = "stmvl53l5cx";
drvdata->misc.fops = &stmvl53l5cx_fops;
if (drvdata->dev_num >= 0) {
snprintf(devname, sizeof(devname), "stmvl53l5cx%d", drvdata->dev_num);
drvdata->misc.name = devname;
}
ret = misc_register(&drvdata->misc);
return ret;
}
/* ===== v6.12+ / legacy 両対応のための共通プローブ ===== */
static int stmvl53l5cx_probe_common(struct i2c_client *client)
{
int ret;
struct stmvl53l5cx_drvdata *drvdata;
drvdata = devm_kzalloc(&client->dev, sizeof(*drvdata), GFP_KERNEL);
if (!drvdata)
return -ENOMEM;
drvdata->reg_buf = devm_kzalloc(&client->dev,
VL53L5CX_COMMS_CHUNK_SIZE + 2,
GFP_DMA | GFP_KERNEL);
if (!drvdata->reg_buf)
return -ENOMEM;
drvdata->client = client;
pr_info("%s: i2c name=%s, addr=0x%x\n", __func__,
client->adapter->name, client->addr);
ret = stmvl53l5cx_parse_dt(&client->dev, drvdata);
if (ret) {
dev_err(&client->dev, "parse dts failed: %d\n", ret);
return ret;
}
ret = stmvl53l5cx_detect(drvdata);
if (ret) {
dev_err(&client->dev, "sensor detect failed: %d\n", ret);
return ret;
}
i2c_set_clientdata(client, drvdata);
return 0;
}
/* Linux 6.12 以降は probe のシグネチャが (client) 単独 */
#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 12, 0)
static int stmvl53l5cx_i2c_probe(struct i2c_client *client)
{
return stmvl53l5cx_probe_common(client);
}
#else
/* 旧カーネル向けの 2 引数版 */
static int stmvl53l5cx_i2c_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
return stmvl53l5cx_probe_common(client);
}
#endif
#if KERNEL_VERSION(6, 1, 0) > LINUX_VERSION_CODE
static int stmvl53l5cx_i2c_remove(struct i2c_client *client)
#else
static void stmvl53l5cx_i2c_remove(struct i2c_client *client)
#endif
{
struct stmvl53l5cx_drvdata *drvdata;
drvdata = i2c_get_clientdata(client);
if (!drvdata) {
pr_err("%s: can't remove %p", __func__, client);
} else {
misc_deregister(&drvdata->misc);
}
#if KERNEL_VERSION(6, 1, 0) > LINUX_VERSION_CODE
return 0;
#endif
}
static const struct of_device_id stmvl53l5cx_dt_ids[] = {
{ .compatible = "st,stmvl53l5cx" },
{ /* sentinel */ },
};
MODULE_DEVICE_TABLE(of, stmvl53l5cx_dt_ids);
static const struct i2c_device_id stmvl53l5cx_i2c_id[] = {
{ "stmvl53l5cx", 0 },
{ },
};
MODULE_DEVICE_TABLE(i2c, stmvl53l5cx_i2c_id);
static struct i2c_driver stmvl53l5cx_i2c_driver = {
.driver = {
.name = "stmvl53l5cx",
.owner = THIS_MODULE,
.of_match_table = stmvl53l5cx_dt_ids,
},
.probe = stmvl53l5cx_i2c_probe,
.remove = stmvl53l5cx_i2c_remove,
.id_table = stmvl53l5cx_i2c_id,
};
static int __init stmvl53l5cx_init(void)
{
int ret = 0;
pr_debug("stmvl53l5cx: module init\n");
/* register as a i2c client device */
ret = i2c_add_driver(&stmvl53l5cx_i2c_driver);
if (ret) {
i2c_del_driver(&stmvl53l5cx_i2c_driver);
printk("stmvl53l5cx: could not add i2c driver\n");
return ret;
}
return ret;
}
static void __exit stmvl53l5cx_exit(void)
{
pr_debug("stmvl53l5cx : module exit\n");
i2c_del_driver(&stmvl53l5cx_i2c_driver);
}
module_init(stmvl53l5cx_init);
module_exit(stmvl53l5cx_exit);
MODULE_AUTHOR("Pengfei Mao <peng-fei.mao@st.com>");
MODULE_DESCRIPTION("VL53L5CX dTOF lightweight driver");
MODULE_LICENSE("GPL v2");
修正したら設定を変更する
- config.txtの修正
/boot/firmware/config.txtに以下の内容を追記する
dtparam=i2c_arm=on
dtparam=i2c1=on
dtparam=i2c1_baudrate=1000000
dtoverlay=stmvl53l5cx
- dtboの作成
以下のコマンドを実行し、devicetreeを作成する
make dtb
make dtbを実行すると/boot/overlays/以下にstmvl53l5cx.dtboがコピーされる。本当は/boot/firmware/overlaysなのでファイルの置き場所を適宜修正する。
- 再起動
一度再起動をする。
sudo reboot
- ドライバのビルド
ドライバ本体をビルドする
make
ディレクトリにstmvl53l5cx.koが作成されていればOK。
実際に使用する
ドライバのロード
以下のコマンドを実行してドライバをロードする。
sudo insmod stmvl53l5cx.ko
/dev以下にstmvl53l5cxというデバイスファイルができていたらOK。
サンプルを実行して動作確認
VL53L5CX_Linux_driver_2.0.0/user/test以下に移動してmakeを実行するとmenuコマンドが作成されるので以下の通り実行する。
sudo menu
以下の通り、コンソールにメニューが表示されるので実行したい番号を入力し、Enterを押すと実行される。
10のmonitor indicateを実行すると以下のような表示がなされる。
複数のプロセスからアクセスするとおかしくなるので要注意。
APIのサンプルはソースコードを見ればわかるので割愛。
サーバ化してみた
Pythonのライブラリもあるのだが、実行するとSegmentation Faultが出てしまって動かないので、Cで距離情報を送信するサーバを作り、PythonからTCP接続で取得できるものを作ってみた。
ポート番号は9999で、JSONでデータを出力する。
{
"ts": "2025-10-13T05:18:31.803Z",
"resolution": "8x8",
"zones": 64,
"data": [
{
"zone": 0,
"distance_mm": 246,
"status": 255,
"motion_power": 11
},
{
"zone": 1,
"distance_mm": 270,
"status": 5,
"motion_power": 11
},
{
"zone": 2,
"distance_mm": 973,
"status": 5,
"motion_power": 31
},
{
"zone": 3,
"distance_mm": 1002,
"status": 5,
"motion_power": 31
},
{
"zone": 4,
"distance_mm": 203,
"status": 5,
"motion_power": 19
},
{
"zone": 5,
"distance_mm": 196,
"status": 5,
"motion_power": 19
},
{
"zone": 6,
"distance_mm": 190,
"status": 5,
"motion_power": 4
},
{
"zone": 7,
"distance_mm": 120,
"status": 5,
"motion_power": 4
},
{
"zone": 8,
"distance_mm": 247,
"status": 255,
"motion_power": 11
},
{
"zone": 9,
"distance_mm": 243,
"status": 255,
"motion_power": 11
},
{
"zone": 10,
"distance_mm": 1236,
"status": 4,
"motion_power": 31
},
{
"zone": 11,
"distance_mm": 197,
"status": 5,
"motion_power": 31
},
{
"zone": 12,
"distance_mm": 188,
"status": 5,
"motion_power": 19
},
{
"zone": 13,
"distance_mm": 184,
"status": 5,
"motion_power": 19
},
{
"zone": 14,
"distance_mm": 157,
"status": 255,
"motion_power": 4
},
{
"zone": 15,
"distance_mm": 93,
"status": 5,
"motion_power": 4
},
{
"zone": 16,
"distance_mm": 1720,
"status": 5,
"motion_power": 8
},
{
"zone": 17,
"distance_mm": 229,
"status": 255,
"motion_power": 8
},
{
"zone": 18,
"distance_mm": 226,
"status": 255,
"motion_power": 19
},
{
"zone": 19,
"distance_mm": 197,
"status": 5,
"motion_power": 19
},
{
"zone": 20,
"distance_mm": 186,
"status": 5,
"motion_power": 15
},
{
"zone": 21,
"distance_mm": 185,
"status": 5,
"motion_power": 15
},
{
"zone": 22,
"distance_mm": 147,
"status": 5,
"motion_power": 8
},
{
"zone": 23,
"distance_mm": 84,
"status": 5,
"motion_power": 8
},
{
"zone": 24,
"distance_mm": 1557,
"status": 5,
"motion_power": 8
},
{
"zone": 25,
"distance_mm": 1567,
"status": 5,
"motion_power": 8
},
{
"zone": 26,
"distance_mm": 1542,
"status": 13,
"motion_power": 19
},
{
"zone": 27,
"distance_mm": 196,
"status": 5,
"motion_power": 19
},
{
"zone": 28,
"distance_mm": 191,
"status": 5,
"motion_power": 15
},
{
"zone": 29,
"distance_mm": 178,
"status": 5,
"motion_power": 15
},
{
"zone": 30,
"distance_mm": 133,
"status": 5,
"motion_power": 8
},
{
"zone": 31,
"distance_mm": 80,
"status": 5,
"motion_power": 8
},
{
"zone": 32,
"distance_mm": 1521,
"status": 5,
"motion_power": 9
},
{
"zone": 33,
"distance_mm": 1520,
"status": 5,
"motion_power": 9
},
{
"zone": 34,
"distance_mm": 1527,
"status": 13,
"motion_power": 15
},
{
"zone": 35,
"distance_mm": 197,
"status": 5,
"motion_power": 15
},
{
"zone": 36,
"distance_mm": 194,
"status": 5,
"motion_power": 13
},
{
"zone": 37,
"distance_mm": 180,
"status": 5,
"motion_power": 13
},
{
"zone": 38,
"distance_mm": 126,
"status": 5,
"motion_power": 10
},
{
"zone": 39,
"distance_mm": 75,
"status": 5,
"motion_power": 10
},
{
"zone": 40,
"distance_mm": 1461,
"status": 5,
"motion_power": 9
},
{
"zone": 41,
"distance_mm": 1446,
"status": 5,
"motion_power": 9
},
{
"zone": 42,
"distance_mm": 197,
"status": 5,
"motion_power": 15
},
{
"zone": 43,
"distance_mm": 192,
"status": 5,
"motion_power": 15
},
{
"zone": 44,
"distance_mm": 193,
"status": 5,
"motion_power": 13
},
{
"zone": 45,
"distance_mm": 175,
"status": 5,
"motion_power": 13
},
{
"zone": 46,
"distance_mm": 118,
"status": 5,
"motion_power": 10
},
{
"zone": 47,
"distance_mm": 66,
"status": 5,
"motion_power": 10
},
{
"zone": 48,
"distance_mm": 1384,
"status": 5,
"motion_power": 31
},
{
"zone": 49,
"distance_mm": 1393,
"status": 5,
"motion_power": 31
},
{
"zone": 50,
"distance_mm": 185,
"status": 5,
"motion_power": 10
},
{
"zone": 51,
"distance_mm": 185,
"status": 5,
"motion_power": 10
},
{
"zone": 52,
"distance_mm": 187,
"status": 5,
"motion_power": 16
},
{
"zone": 53,
"distance_mm": 159,
"status": 255,
"motion_power": 16
},
{
"zone": 54,
"distance_mm": 688,
"status": 5,
"motion_power": 12
},
{
"zone": 55,
"distance_mm": 58,
"status": 5,
"motion_power": 12
},
{
"zone": 56,
"distance_mm": 344,
"status": 5,
"motion_power": 31
},
{
"zone": 57,
"distance_mm": 1362,
"status": 13,
"motion_power": 31
},
{
"zone": 58,
"distance_mm": 176,
"status": 5,
"motion_power": 10
},
{
"zone": 59,
"distance_mm": 179,
"status": 5,
"motion_power": 10
},
{
"zone": 60,
"distance_mm": 181,
"status": 5,
"motion_power": 16
},
{
"zone": 61,
"distance_mm": 666,
"status": 5,
"motion_power": 16
},
{
"zone": 62,
"distance_mm": 626,
"status": 5,
"motion_power": 12
},
{
"zone": 63,
"distance_mm": 57,
"status": 5,
"motion_power": 12
}
]
}
ファイルは以下の場所に配置する。
VL53L5CX_Linux_driver_2.0.0
├── examples
├── platform
├── server
│ ├── Makefile
│ └── server_vl53l5cx.c
├── test
└── uld-driver
├── inc
└── src
Makefile
CC := gcc
# Set to TRUE to enable logging functions.
LOG_ENABLE = FALSE
CORE_INCLUDE_PATHS = -I../uld-driver/inc
PLATFORM_INCLUDE_PATHS = -I../platform
EXAMPLES_INCLUDE_PATHS = -I../examples
BASE_CFLAGS = -Wall -Werror -Wno-missing-braces
CFLAGS_RELEASE = -Os -g0
CFLAGS_RELEASE += -DSTMVL53L5CX_KERNEL
LIB_CORE_SOURCES =\
$(wildcard ../uld-driver/src/*.c)
LIB_PLATFORM_SOURCES =\
$(wildcard ../platform/*.c)
LIB_EXAMPLES_SOURCES =\
$(wildcard ../examples/*.c)
LIB_SOURCES := $(LIB_CORE_SOURCES) $(LIB_PLATFORM_SOURCES) $(LIB_EXAMPLES_SOURCES)
INCLUDE_PATH = $(CORE_INCLUDE_PATHS) $(PLATFORM_INCLUDE_PATHS) $(EXAMPLES_INCLUDE_PATHS)
CFLAGS = $(BASE_CFLAGS) $(CFLAGS_RELEASE) $(INCLUDE_PATH)
all:
$(CC) $(CFLAGS) $(LIB_FLAGS) -o server_vl53l5cx ./server_vl53l5cx.c $(LIB_SOURCES)
clean:
rm -f server_vl53l5cx
server_vl53l5cx.c
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include "vl53l5cx_api.h"
#include "vl53l5cx_plugin_motion_indicator.h"
// ===== ユーザ調整可能な定数 =====
#define SERVER_PORT 9999 // TCP待受ポート
#define PERIODIC_PRINT_MS 1000 // 標準出力にJSONを出す周期
#define POLL_SLEEP_MS 5 // Readyポーリングの待ち
#define RANGING_HZ 10 // 測距周波数の目標値
#define USE_RESOLUTION VL53L5CX_RESOLUTION_4X4 // 4x4固定（8x8に変えるなら両方揃える）
#define DIST_MIN_MM 500 // モーション距離帯（参考：使わなくてもOK）
#define DIST_MAX_MM 1500
static volatile sig_atomic_t g_stop = 0;
static void on_sigint(int sig) { (void)sig; g_stop = 1; }
static int set_nonblock(int fd) {
int fl = fcntl(fd, F_GETFL, 0);
if (fl < 0) return -1;
return fcntl(fd, F_SETFL, fl | O_NONBLOCK);
}
static int create_server_socket(uint16_t port) {
int fd = socket(AF_INET, SOCK_STREAM, 0);
if (fd < 0) { perror("socket"); return -1; }
int yes = 1;
setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));
struct sockaddr_in addr = {0};
addr.sin_family = AF_INET;
addr.sin_port = htons(port);
addr.sin_addr.s_addr = htonl(INADDR_ANY);
if (bind(fd, (struct sockaddr*)&addr, sizeof(addr)) < 0) {
perror("bind"); close(fd); return -1;
}
if (listen(fd, 1) < 0) { perror("listen"); close(fd); return -1; }
return fd;
}
// 送信（部分送信・EINTR対応）
static int send_all(int fd, const char *buf, size_t len) {
size_t off = 0;
while (off < len) {
ssize_t n = send(fd, buf + off, len - off, 0);
if (n > 0) { off += (size_t)n; continue; }
if (n == 0) return -1;
if (errno == EINTR) continue;
if (errno == EAGAIN || errno == EWOULDBLOCK) { usleep(1000); continue; }
return -1;
}
return 0;
}
// ISO8601（UTC, ミリ秒）に整形
static void now_iso8601(char *out, size_t cap) {
struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts);
time_t sec = ts.tv_sec;
struct tm t; gmtime_r(&sec, &t);
int ms = (int)(ts.tv_nsec / 1000000);
snprintf(out, cap, "%04d-%02d-%02dT%02d:%02d:%02d.%03dZ",
t.tm_year + 1900, t.tm_mon + 1, t.tm_mday,
t.tm_hour, t.tm_min, t.tm_sec, ms);
}
// 計測結果をJSON 1行に整形（4x4想定; 8x8にするならループ上限とtotal_zonesを変更）
static size_t build_json_line(char *out, size_t cap,
const VL53L5CX_ResultsData *r,
const VL53L5CX_Motion_Configuration *mcfg,
uint8_t resolution)
{
char ts[128]; now_iso8601(ts, sizeof(ts));
const int total_zones = (resolution == VL53L5CX_RESOLUTION_8X8) ? 64 : 16;
// ヘッダ部
int n = snprintf(out, cap,
"{\"ts\":\"%s\",\"resolution\":\"%s\",\"zones\":%d,\"data\":[",
ts, (resolution == VL53L5CX_RESOLUTION_8X8) ? "8x8" : "4x4",
total_zones);
if (n < 0 || (size_t)n >= cap) return 0;
size_t off = (size_t)n;
for (int i = 0; i < total_zones; i++) {
// 距離: r->distance_mm[i]（単位mm）。測定不可は0や特定値※環境により
int16_t dist = (int16_t)r->distance_mm[i];
uint8_t st = r->target_status[i]; // 0=OK以外は無効・エラー等
// モーション（初期化していれば利用可）
int16_t mpow = 0;
if (mcfg) {
uint8_t mid = mcfg->map_id[i];
mpow = (int16_t)r->motion_indicator.motion[mid];
}
n = snprintf(out + off, cap - off,
"%s{\"zone\":%d,\"distance_mm\":%d,\"status\":%u,\"motion_power\":%d}",
(i==0) ? "" : ",", i, dist, st, mpow);
if (n < 0 || off + (size_t)n >= cap) return 0;
off += (size_t)n;
}
// フッタ部
n = snprintf(out + off, cap - off, "]}\n");
if (n < 0 || off + (size_t)n >= cap) return 0;
off += (size_t)n;
return off; // 生成したJSON長
}
// VL53L5CXのセットアップ（4x4, RANGING_HZ）
static int sensor_setup(VL53L5CX_Configuration *dev,
VL53L5CX_Motion_Configuration *mcfg_out,
uint8_t resolution)
{
uint8_t alive = 0;
uint8_t st = vl53l5cx_is_alive(dev, &alive);
if (alive == 0 ) { fprintf(stderr, "Sensor not alive\n"); return -1; }
st = vl53l5cx_init(dev);
if (st) { fprintf(stderr, "vl53l5cxcd_init failed: %u\n", st); return -1; }
// 解像度（センサ側）
st = vl53l5cx_set_resolution(dev, resolution);
if (st) { fprintf(stderr, "set_resolution failed: %u\n", st); return -1; }
// 周波数
st = vl53l5cx_set_ranging_frequency_hz(dev, RANGING_HZ);
if (st) { fprintf(stderr, "set_ranging_frequency_hz failed: %u\n", st); return -1; }
// モーション（必要ないなら mcfg_out = NULL でもOK）
if (mcfg_out) {
st = vl53l5cx_motion_indicator_init(dev, mcfg_out, resolution);
if (st) { fprintf(stderr, "motion_indicator_init failed: %u\n", st); return -1; }
// 距離帯（幅<=1500mm）
st = vl53l5cx_motion_indicator_set_distance_motion(dev, mcfg_out, DIST_MIN_MM, DIST_MAX_MM);
if (st) { fprintf(stderr, "motion_indicator_set_distance_motion failed: %u\n", st); return -1; }
// 解像度変更時の同期（上で指定済みだが例として）
st = vl53l5cx_motion_indicator_set_resolution(dev, mcfg_out, resolution);
if (st) { fprintf(stderr, "motion_indicator_set_resolution failed: %u\n", st); return -1; }
}
return 0;
}
// メインの監視＋サーバループ
int run_monitor_server(VL53L5CX_Configuration *dev)
{
VL53L5CX_Motion_Configuration mcfg;
memset(&mcfg, 0, sizeof(mcfg));
if (sensor_setup(dev, &mcfg, USE_RESOLUTION) != 0) return -1;
uint8_t st = vl53l5cx_start_ranging(dev);
if (st) { fprintf(stderr, "start_ranging failed: %u\n", st); return -1; }
int srv = create_server_socket(SERVER_PORT);
if (srv < 0) { vl53l5cx_stop_ranging(dev); return -1; }
set_nonblock(srv);
printf("TCP server listening on port %d\n", SERVER_PORT);
int cli = -1;
char json_buf[16 * 1024]; // 4x4なら十分（8x8でも余裕目）
VL53L5CX_ResultsData results;
memset(&results, 0, sizeof(results));
// 周期出力のタイマ
struct timespec last_print; clock_gettime(CLOCK_MONOTONIC, &last_print);
while (!g_stop) {
// クライアント受付（非ブロッキング、単一接続）
if (cli < 0) {
struct sockaddr_in ca; socklen_t calen = sizeof(ca);
int c = accept(srv, (struct sockaddr*)&ca, &calen);
if (c >= 0) {
set_nonblock(c);
cli = c;
fprintf(stderr, "Client connected\n");
}
}
// 計測Ready待ち（軽くポーリング）
uint8_t ready = 0;
st = vl53l5cx_check_data_ready(dev, &ready);
if (st) {
fprintf(stderr, "check_data_ready err=%u\n", st);
VL53L5CX_WaitMs(&dev->platform, POLL_SLEEP_MS);
continue;
}
if (ready) {
// 新しい結果を取得
st = vl53l5cx_get_ranging_data(dev, &results);
if (st) {
fprintf(stderr, "get_ranging_data err=%u\n", st);
} else {
// JSON整形
size_t jlen = build_json_line(json_buf, sizeof(json_buf), &results, &mcfg, USE_RESOLUTION);
if (jlen > 0) {
// 1) クライアントに通知（接続中のみ）
if (cli >= 0) {
if (send_all(cli, json_buf, jlen) != 0) {
fprintf(stderr, "Client disconnected\n");
close(cli); cli = -1;
}
}
// 2) 周期的に標準出力へ（一定周期だけ）
struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now);
long diff_ms = (now.tv_sec - last_print.tv_sec) * 1000L
+ (now.tv_nsec - last_print.tv_nsec) / 1000000L;
if (diff_ms >= PERIODIC_PRINT_MS) {
fwrite(json_buf, 1, jlen, stdout);
fflush(stdout);
last_print = now;
}
}
}
}
// クライアント側の切断検知（読み取りで0/EPIPEを見に行く簡易版）
if (cli >= 0) {
char tmp[1];
ssize_t n = recv(cli, tmp, sizeof(tmp), MSG_DONTWAIT);
if (n == 0) { // orderly shutdown
fprintf(stderr, "Client closed\n");
close(cli); cli = -1;
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) {
fprintf(stderr, "Client recv err=%d\n", errno);
close(cli); cli = -1;
}
}
VL53L5CX_WaitMs(&dev->platform, POLL_SLEEP_MS);
}
if (cli >= 0) close(cli);
close(srv);
vl53l5cx_stop_ranging(dev);
return 0;
}
#define BUILD_STANDALONE_MAIN
#ifdef BUILD_STANDALONE_MAIN
#include "platform.h"
int main(void) {
signal(SIGINT, on_sigint);
VL53L5CX_Configuration dev;
int status;
/* Initialize channel com */
status = vl53l5cx_comms_init(&dev.platform);
if(status)
{
printf("VL53L5CX comms init failed\n");
return -1;
}
if (run_monitor_server(&dev) != 0) {
fprintf(stderr, "run_monitor_server failed\n");
return 1;
}
return 0;
}
#endif
Pythonのクライアントは以下の通り。
client.py
#!/usr/bin/env python3
import socket
import time
import json
import sys
import threading
from typing import Callable, Optional, IO, Any, Dict, List
import copy
import math
class VL53L5CXClient:
"""
VL53L5CX NDJSON TCP client.
- 接続して常にデータ取得（切断時は自動リトライ）
- 受信した最新データを内部に保持（スレッドセーフ）
- サンプル受信ごとに on_sample コールバックを呼び出し（未設定なら何もしない）
- 最新データを get_latest() で取得可能
- 8x8要約表示を作る format_matrix() を提供（出力は文字列）
"""
def __init__(
self,
host: str = "127.0.0.1",
port: int = 9999,
reconnect_delay: float = 2.0,
on_sample: Optional[Callable[[Dict[str, Any]], None]] = None,
) -> None:
self.host = host
self.port = port
self.reconnect_delay = reconnect_delay
self._on_sample = on_sample # 未設定OK
self._sock: Optional[socket.socket] = None
self._fp: Optional[IO[str]] = None
self._stopping = threading.Event()
self._latest_lock = threading.Lock()
self._latest: Optional[Dict[str, Any]] = None
# ---------------------------
# Public API
# ---------------------------
def run_forever(self) -> None:
try:
while not self._stopping.is_set():
try:
self._connect()
self._read_loop()
except (ConnectionError, OSError, socket.error) as e:
self._close_net()
if self._stopping.is_set():
break
self._log(f"Disconnected or failed: {e}. Reconnecting in {self.reconnect_delay}s ...")
time.sleep(self.reconnect_delay)
except KeyboardInterrupt:
self._log("Interrupted.")
break
finally:
self._close_all()
def stop(self) -> None:
self._stopping.set()
self._close_net()
def set_callback(self, on_sample: Optional[Callable[[Dict[str, Any]], None]]) -> None:
self._on_sample = on_sample
def get_latest(self) -> Optional[Dict[str, Any]]:
with self._latest_lock:
return copy.deepcopy(self._latest) if self._latest is not None else None
# ---- 8x8 要約表示ユーティリティ ----
def format_matrix(
self,
obj: Dict[str, Any],
field: str = "distance", # "distance" -> distance_mm, "motion" -> motion_power
color: bool = False,
clear: bool = False,
pad: int = 4, # 各セルの幅（整数右寄せ）
) -> str:
"""
obj から 8x8 マトリクスを整形して文字列で返す。
- zones が 64 の場合はそのまま 8x8
- zones が 16 の場合は 2x2 で最近傍拡大して 8x8
- field: "distance" (distance_mm) or "motion" (motion_power)
- color: ANSI 256色背景ヒートマップ
- clear: True なら先頭に画面クリアシーケンスを入れる
"""
key = "distance_mm" if field == "distance" else "motion_power"
zones = int(obj.get("zones", 0))
data = obj.get("data", [])
ts = obj.get("ts", "")
frame = obj.get("frame", -1)
res = obj.get("resolution", "")
# 1D -> 2D
if zones not in (16, 64):
header = f"[{ts}] frame={frame} resolution={res} zones={zones} (unsupported zones)\n"
return header
side = int(math.isqrt(zones))
small: List[List[Optional[int]]] = []
idx = 0
for r in range(side):
row: List[Optional[int]] = []
for c in range(side):
v = data[idx].get(key, None) if idx < len(data) else None
row.append(v)
idx += 1
small.append(row)
# 4x4 -> 8x8 (2x2 NN拡大)
if side == 4:
grid = [[None for _ in range(8)] for _ in range(8)]
for r in range(4):
for c in range(4):
v = small[r][c]
R, C = r * 2, c * 2
grid[R][C] = v
grid[R+1][C] = v
grid[R][C+1] = v
grid[R+1][C+1] = v
else:
grid = small # 8x8
# スカラー範囲（色用）
flat_vals = [v for row in grid for v in row if v is not None]
vmin = min(flat_vals) if flat_vals else 0
vmax = max(flat_vals) if flat_vals else 1
if vmax == vmin:
vmax = vmin + 1
def cell_str(v: Optional[int]) -> str:
return f"{v:>{pad}d}" if isinstance(v, int) else " " * (pad - 1) + "-"
def colorize(txt: str, v: Optional[int]) -> str:
if not color or v is None:
return txt
# 0..1 正規化→ ANSI 256 グレイスケール 232..255
norm = (v - vmin) / (vmax - vmin) if vmax > vmin else 0.0
code = 232 + int(23 * max(0.0, min(1.0, norm)))
return f"\x1b[48;5;{code}m{txt}\x1b[0m"
lines = []
if clear:
lines.append("\x1b[2J\x1b[H")
lines.append(f"[{ts}] frame={frame} resolution={res} zones={zones} field={key}")
for r in range(8):
parts = []
for c in range(8):
v = grid[r][c]
parts.append(colorize(cell_str(v), v))
lines.append(" ".join(parts))
lines.append("") # 末尾に空行
return "\n".join(lines)
# ---------------------------
# Internals
# ---------------------------
def _connect(self) -> None:
self._log(f"Connecting to {self.host}:{self.port} ...")
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
s.connect((self.host, self.port))
self._sock = s
self._fp = s.makefile("r", encoding="utf-8", newline="\n")
self._log("Connected.")
def _read_loop(self) -> None:
assert self._fp is not None
while not self._stopping.is_set():
line = self._fp.readline()
if not line:
raise ConnectionError("server closed")
line = line.strip()
if not line:
continue
try:
obj = json.loads(line)
except json.JSONDecodeError as e:
self._elog(f"JSON parse error: {e}; line={line[:120]}...")
continue
with self._latest_lock:
self._latest = obj
cb = self._on_sample
if cb is not None:
try:
cb(obj)
except Exception as cb_err:
self._elog(f"on_sample error: {cb_err!r}")
def _close_net(self) -> None:
try:
if self._fp:
self._fp.close()
except Exception:
pass
try:
if self._sock:
self._sock.close()
except Exception:
pass
self._fp = None
self._sock = None
def _close_all(self) -> None:
self._close_net()
def _log(self, msg: str) -> None:
print(msg)
sys.stdout.flush()
def _elog(self, msg: str) -> None:
print(msg, file=sys.stderr)
sys.stderr.flush()
# ---- 使用例（8x8要約表示コールバック） ----
if __name__ == "__main__":
import argparse
ap = argparse.ArgumentParser(description="VL53L5CX NDJSON TCP client (8x8 summary via callback)")
ap.add_argument("--host", default="127.0.0.1")
ap.add_argument("--port", type=int, default=9999)
ap.add_argument("--reconnect-delay", type=float, default=2.0)
ap.add_argument("--field", choices=["distance", "motion"], default="distance")
ap.add_argument("--color", action="store_true")
ap.add_argument("--clear-each", action="store_true")
args = ap.parse_args()
client = VL53L5CXClient(
host=args.host,
port=args.port,
reconnect_delay=args.reconnect_delay,
on_sample=None, # 後で設定
)
def on_sample_print_8x8(obj: Dict[str, Any]) -> None:
s = client.format_matrix(
obj,
field=args.field,
color=args.color,
clear=args.clear_each,
pad=4,
)
print(s, end="")
client.set_callback(on_sample_print_8x8)
try:
client.run_forever()
except KeyboardInterrupt:
client.stop()
実行すると以下のように配列として距離を出力できます。
モーションも検出できるようだけど、今回は距離がわかればよいのでここまで。
Discussion