diff --git a/README.md b/README.md
new file mode 100644
index 0000000..dffb1a9
--- /dev/null
+++ b/README.md
@@ -0,0 +1,266 @@
+# VL53L0X TOF传感器驱动软件包
+
+
+
+## 1 简介
+
+vl53l0x软件包是基于RT-Thread sensor框架实现的一个驱动包。基于该软件包,RT-Thread应用程序可以使用标准的sensor接口访问vl53l0x,获取传感器数据。
+
+
+
+### 1.1 目录结构
+
+| 名称 | 说明 |
+| ---------- | ------------------------------ |
+| docs | 文档目录 |
+| vl53l0x | 官方库函数以及i2c platform对接 |
+| examples | 例子目录 |
+| inc | 头文件目录 |
+| src | 源代码目录 |
+| LICENSE | 许可证文件 |
+| SConscript | RT-Thread默认构建脚本 |
+
+
+
+### 1.2 许可证
+
+vl53l0x软件包遵循 Apache license v2.0 许可,详见 `LICENSE` 文件。
+
+
+
+## 2 传感器介绍
+
+vl53l0x是 STMicroelectronics(意法半导体)公司推出的新一代单点TOF(Time of Flight)传感器,具备体积小、测量精度高、测量距离远、无需增加外部光学器件、使用简单等优点;此外,vl53l0x采用940nm肉眼不可见光源,集成物理红外过滤器,提高对环境光的抗干扰特性,具备良好的鲁棒性和防光学串扰特性。
+
+测量参数:
+
+| 功能 | 量程 | 分辨率 | 精度 |
+| ---- | -------- | ------ | ---- |
+| 距离 | 0—2000mm | 1mm | — |
+
+应用场合:
+
+* 相机对焦
+* 1D手势识别
+* 白色家电检测(额温枪、水龙头)
+* 机器人避障
+
+
+
+## 3 支持情况
+
+
+
+| 包含设备 | TOF |
+| ------------ | ---- |
+| **通信接口** | |
+| IIC | √ |
+| SPI | |
+| **工作模式** | |
+| 轮询 | √ |
+| 中断 | |
+| FIFO | |
+| **电源模式** | |
+| 掉电 | √ |
+| 低功耗 | |
+| 普通 | √ |
+
+>注:
+>
+>目前暂时只支持单次测量模式;后续增加:
+>
+>* 连续测量模式
+>* 定时测量模式
+>* 测距校准功能
+
+
+
+
+
+## 4 使用说明
+
+### 4.1 依赖
+
+- RT-Thread 4.0.0+
+
+- sensor 框架组件
+
+- I2C 驱动,vl53l0x使用 I2C 进行数据通讯,需要系统 I2C 驱动框架支持
+
+- PIN驱动,用于vl53l0x开关机(复位)引脚控制
+
+
+
+### 4.2 获取软件包
+
+使用 vl53l0xpackage 需要在 RT-Thread 的包管理器中选择它,具体路径如下。然后让 RT-Thread 的包管理器自动更新,或者使用 `pkgs --update` 命令更新包到 BSP 中。如需使用示例程序,则使能Enable vl53l0x sample
。
+
+```
+RT-Thread online packages --->
+ peripheral libraries and drivers --->
+ sensors drivers --->
+ [*] VL53L0X Time of flight(TOF) sensor.
+ [*] Enable vl53l0x sample
+ Version (latest) --->
+```
+
+> **Version**:软件包版本选择,默认选择最新版本。
+
+
+
+### 4.3 初始化
+
+vl53l0x软件包初始化函数如下所示:
+
+```c
+int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg,rt_base_t xsht_pin);
+```
+
+| 参数 | 描述 |
+| --------- | -------------- |
+| name | 传感器名称 |
+| cfg | sensor配置信息 |
+| xsht | 电源控制GPIO |
+| **返回** | — |
+| RT_EOK | 初始化成功 |
+| -RT_ERROR | 初始化失败 |
+
+
+
+该函数需要由用户调用,函数主要完成的功能有:
+
+- 根据配置信息配置i2c名称、i2c地址等(可增加其他配置信息),然后初始化设备
+
+- 选择电源控制GPIO
+
+- 注册相应的传感器设备,完成 vl53l0x设备的注册
+
+
+
+**参考示例:**
+
+```c
+#include "vl53l0x.h"
+
+static int rt_hw_vl53l0x_port(void)
+{
+ struct rt_sensor_config cfg;
+
+ cfg.intf.dev_name = "i2c1"; /* i2c bus */
+ cfg.intf.user_data = (void *)0x29; /* i2c slave addr */
+ rt_hw_vl53l0x_init("vl53l0x", &cfg, 57);/* xshutdown ctrl pin */
+
+ return RT_EOK;
+}
+INIT_COMPONENT_EXPORT(rt_hw_vl53l0x_port);
+```
+
+
+
+### 4.4 读取数据
+
+vl53l0x软件包基于sensor框架,sensor框架继承于RT-Thread标准设备框架,可以使用RT-Thread标准设备接口"open/read"读取传感器数据。
+
+
+
+**参考伪代码:**
+
+```
+temp_dev = rt_device_find("tof_vl53l0x");
+rt_device_open(temp_dev, RT_DEVICE_FLAG_RDONLY);
+rt_device_read(temp_dev, 0, &sensor_data, 1);
+```
+
+
+
+### 4.5 msh/finsh测试
+
+#### 查看设备注册
+
+```
+msh >list_device
+device type ref count
+-------- -------------------- ----------
+tof_vl53 Sensor Device 0
+i2c1 I2C Bus 0
+pin Miscellaneous Device 0
+uart2 Character Device 0
+uart1 Character Device 2
+```
+
+>注:
+>
+>完整设备名称为“tof_vl53l0x”,终端显示有长度限制
+
+
+
+#### 运行例程周期打印数据
+
+```b
+ \ | /
+- RT - Thread Operating System
+ / | \ 4.0.1 build Dec 17 2020
+ 2006 - 2019 Copyright by rt-thread team
+[I/sensor] rt_sensor init success
+[I/vl53l0x] vl53l0x info:
+ Name[VL53L0X ES1 or later]
+ Type[VL53L0X]
+ ProductId[VL53L0CBV0DH/1$1]
+
+msh >distance[153mm],timestamp[87]
+distance[161mm],timestamp[192]
+distance[154mm],timestamp[297]
+distance[152mm],timestamp[402]
+```
+
+
+
+#### 使用RTT自带的测试命令
+
+* 探测设备
+
+```b
+msh >sensor probe tof_vl53l0x
+[I/sensor.cmd] device id: 0xee!
+```
+
+
+
+* 获取传感器信息
+
+```
+msh >sensor info
+vendor :STMicroelectronics
+model :vl53l0x
+unit :mm
+range_max :2000
+range_min :0
+period_min:100ms
+fifo_max :0
+```
+
+> 注:
+>
+> sensor框架暂未提供tof传感器相关描述信息,已在RT-Thread源码上PR。
+
+* 读取测距数据
+
+```b
+msh >sensor read 3
+[I/sensor.cmd] num: 0, distance: 212, timestamp:131863
+[I/sensor.cmd] num: 1, distance: 213, timestamp:131878
+[I/sensor.cmd] num: 2, distance: 213, timestamp:131893
+```
+
+
+
+## 5 注意事项
+
+暂无
+
+
+
+## 6 联系方式
+
+- 维护:[Acuity](https://github.com/Prry)
+- 主页:
\ No newline at end of file
diff --git a/examples/vl53l0x_sample.c b/examples/vl53l0x_sample.c
index ff502dc..ab7e422 100644
--- a/examples/vl53l0x_sample.c
+++ b/examples/vl53l0x_sample.c
@@ -14,14 +14,14 @@
#include "sensor.h"
#include "vl53l0x.h"
-static void read_temp_entry(void *parameter)
+static void read_distance_entry(void *parameter)
{
rt_device_t temp_dev = RT_NULL;
struct rt_sensor_data temp_data;
rt_size_t res = 0;
rt_uint32_t index = 0;
- temp_dev = rt_device_find("pr_vl53l0x");
+ temp_dev = rt_device_find("tof_vl53l0x");
if (temp_dev == RT_NULL)
{
rt_kprintf("not found tof_vl53l0x device\r\n");
@@ -45,7 +45,7 @@ static void read_temp_entry(void *parameter)
}
else
{
- rt_kprintf("temp[%dmm],timestamp[%d]\r\n",temp_data.data.proximity,
+ rt_kprintf("distance[%dmm],timestamp[%d]\r\n",temp_data.data.proximity,
temp_data.timestamp);
}
@@ -58,24 +58,24 @@ static void read_temp_entry(void *parameter)
}
}
-static int temp_read_sample(void)
+static int read_distance_sample(void)
{
- rt_thread_t temp_thread;
+ rt_thread_t distance_thread;
- temp_thread = rt_thread_create("tof_r",
- read_temp_entry,
+ distance_thread = rt_thread_create("tof_r",
+ read_distance_entry,
RT_NULL,
1024,
RT_THREAD_PRIORITY_MAX / 2,
20);
- if (temp_thread != RT_NULL)
+ if (distance_thread != RT_NULL)
{
- rt_thread_startup(temp_thread);
+ rt_thread_startup(distance_thread);
}
return RT_EOK;
}
-INIT_APP_EXPORT(temp_read_sample);
+INIT_APP_EXPORT(read_distance_sample);
static int rt_hw_vl53l0x_port(void)
{
@@ -83,7 +83,7 @@ static int rt_hw_vl53l0x_port(void)
cfg.intf.dev_name = "i2c1"; /* i2c bus */
cfg.intf.user_data = (void *)0x29; /* i2c slave addr */
- rt_hw_vl53l0x_init("vl53l0x", &cfg, 57);/* vl53l0x */
+ rt_hw_vl53l0x_init("vl53l0x", &cfg, 57);/* xshutdown ctrl pin */
return RT_EOK;
}
diff --git a/inc/vl53l0x.h b/inc/vl53l0x.h
index 545b6cc..86ce112 100644
--- a/inc/vl53l0x.h
+++ b/inc/vl53l0x.h
@@ -4,6 +4,6 @@
#include
-extern int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t rst_pin);
+extern int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t xsht_pin);
#endif
\ No newline at end of file
diff --git a/src/vl53l0x.c b/src/vl53l0x.c
index 612c88d..b2bb852 100644
--- a/src/vl53l0x.c
+++ b/src/vl53l0x.c
@@ -30,10 +30,11 @@
/* vl53l0x for RT-Thread sensor device */
#define VL53L0X_DIST_RANGE_MAX (2000) /* 1mm */
#define VL53L0X_DIST_RANGE_MIN (0) /* 1mm */
-#define VL53L0X_DIST_PEROID (100) /* 10ms */
+#define VL53L0X_DIST_PEROID (100) /* 1ms */
-static struct rt_i2c_bus_device *i2c_bus_dev;
-static VL53L0X_Dev_t vl53l0x_dev =
+static struct rt_i2c_bus_device *i2c_bus_dev;/* i2c bus device */
+static rt_base_t xshutdown_pin = 0;/* shutdown control pin */
+static VL53L0X_Dev_t vl53l0x_dev = /* vl53l0x device */
{
.comms_type = 1,
.comms_speed_khz = 400,
@@ -86,6 +87,27 @@ int32_t vl53l0x_read_regs(uint8_t slave_addr, uint8_t reg, uint8_t *data, uint16
}
}
+static rt_err_t vl53l0x_set_power(rt_sensor_t psensor, rt_uint8_t power)
+{
+ if (power == RT_SENSOR_POWER_DOWN)
+ {
+ rt_pin_write(xshutdown_pin, PIN_LOW);
+ psensor->config.power = RT_SENSOR_POWER_DOWN;
+ return RT_EOK;
+ }
+ else if (power == RT_SENSOR_POWER_NORMAL)
+ {
+ rt_pin_write(xshutdown_pin, PIN_HIGH);
+ psensor->config.power = RT_SENSOR_POWER_NORMAL;
+ return RT_EOK;
+ }
+ else
+ {
+ return -RT_ERROR;
+ }
+
+}
+
VL53L0X_Error vl53l0x_single_ranging_mode(VL53L0X_Dev_t *pdev)
{
VL53L0X_Error Status = VL53L0X_ERROR_NONE;
@@ -164,11 +186,11 @@ static rt_size_t vl53l0x_polling_get_data(rt_sensor_t psensor, struct rt_sensor_
static VL53L0X_RangingMeasurementData_t vl53l0x_data;
pdev = (VL53L0X_Dev_t*)psensor->parent.user_data;
- if(psensor->info.type == RT_SENSOR_CLASS_PROXIMITY)
+ if(psensor->info.type == RT_SENSOR_CLASS_TOF)
{/* actual distance */
if (vl53l0x_single_read_data(pdev, &vl53l0x_data) == RT_EOK)
{
- sensor_data->type = RT_SENSOR_CLASS_PROXIMITY;
+ sensor_data->type = RT_SENSOR_CLASS_TOF;
sensor_data->data.proximity = vl53l0x_data.RangeMilliMeter;
sensor_data->timestamp = rt_sensor_get_ts();
}
@@ -228,7 +250,7 @@ static rt_err_t vl53l0x_control(struct rt_sensor_device *psensor, int cmd, void
break;
case RT_SENSOR_CTRL_SET_POWER:
-
+ vl53l0x_set_power(psensor, (rt_uint32_t)args & 0xff);
break;
default:
@@ -243,17 +265,18 @@ static struct rt_sensor_ops vl53l0x_ops =
vl53l0x_control,
};
-int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t rst_pin)
+int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t xsht_pin)
{
rt_err_t ret = RT_EOK;
rt_sensor_t sensor_dist = RT_NULL;
VL53L0X_DeviceInfo_t vl53l0x_info;
struct rt_i2c_bus_device *i2c_bus = RT_NULL; /* linked i2c bus */
-
- rt_pin_mode(rst_pin, PIN_MODE_OUTPUT);
- rt_pin_write(rst_pin, PIN_LOW);
+
+ xshutdown_pin = xsht_pin;
+ rt_pin_mode(xsht_pin, PIN_MODE_OUTPUT);
+ rt_pin_write(xsht_pin, PIN_LOW);
rt_thread_delay(10);
- rt_pin_write(rst_pin, PIN_HIGH);
+ rt_pin_write(xsht_pin, PIN_HIGH);
i2c_bus = rt_i2c_bus_device_find(cfg->intf.dev_name);
if(i2c_bus == RT_NULL)
@@ -266,6 +289,7 @@ int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t
vl53l0x_dev.I2cDevAddr = (rt_uint32_t)(cfg->intf.user_data) & 0xff;
vl53l0x_dev.RegRead = vl53l0x_read_regs;
vl53l0x_dev.RegWrite = vl53l0x_write_regs;
+
/* tof sensor register */
{
sensor_dist = rt_calloc(1, sizeof(struct rt_sensor_device));
@@ -274,9 +298,9 @@ int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t
goto __exit;
}
rt_memset(sensor_dist, 0x0, sizeof(struct rt_sensor_device));
- sensor_dist->info.type = RT_SENSOR_CLASS_PROXIMITY;
+ sensor_dist->info.type = RT_SENSOR_CLASS_TOF;
sensor_dist->info.vendor = RT_SENSOR_VENDOR_STM;
- sensor_dist->info.model = "vl53l0x_tof";
+ sensor_dist->info.model = "vl53l0x";
sensor_dist->info.unit = RT_SENSOR_UNIT_MM;
sensor_dist->info.intf_type = RT_SENSOR_INTF_I2C;
sensor_dist->info.range_max = VL53L0X_DIST_RANGE_MAX;
@@ -294,18 +318,21 @@ int rt_hw_vl53l0x_init(const char *name, struct rt_sensor_config *cfg, rt_base_t
}
}
+ /* vl53l0x init */
if (VL53L0X_ERROR_NONE != VL53L0X_DataInit(&vl53l0x_dev))
{
LOG_E("vl53l0x data init failed\r\n");
goto __exit;
}
+ /* vl53l0x read version */
if (VL53L0X_ERROR_NONE == VL53L0X_GetDeviceInfo(&vl53l0x_dev, &vl53l0x_info))
{
LOG_I("vl53l0x info:\n Name[%s]\n Type[%s]\n ProductId[%s]\r\n",
vl53l0x_info.Name, vl53l0x_info.Type, vl53l0x_info.ProductId);
}
+ /* set single ranging mode */
if (VL53L0X_ERROR_NONE != vl53l0x_single_ranging_mode(&vl53l0x_dev))
{
LOG_E("vl53l0x single ranging init failed\r\n");