1 /* vl53l1.c - Driver for ST VL53L1X time of flight sensor */
2
3 /*
4 * Copyright (c) 2023 Prosaris SOlutions Inc.
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #define DT_DRV_COMPAT st_vl53l1x
10
11 #include <errno.h>
12
13 #include <zephyr/kernel.h>
14 #include <zephyr/drivers/i2c.h>
15 #include <zephyr/drivers/sensor.h>
16 #include <zephyr/init.h>
17 #include <zephyr/drivers/gpio.h>
18 #include <zephyr/sys/__assert.h>
19 #include <zephyr/types.h>
20 #include <zephyr/device.h>
21 #include <zephyr/logging/log.h>
22
23 #include "vl53l1_api.h"
24 #include "vl53l1_platform.h"
25
26 LOG_MODULE_REGISTER(VL53L1X, CONFIG_SENSOR_LOG_LEVEL);
27
28 struct vl53l1x_config {
29 struct i2c_dt_spec i2c;
30 #ifdef CONFIG_VL53L1X_XSHUT
31 struct gpio_dt_spec xshut;
32 #endif
33 #ifdef CONFIG_VL53L1X_INTERRUPT_MODE
34 struct gpio_dt_spec gpio1;
35 #endif
36 };
37
38 struct vl53l1x_data {
39 VL53L1_Dev_t vl53l1x;
40 VL53L1_RangingMeasurementData_t data;
41 VL53L1_DistanceModes distance_mode;
42 #ifdef CONFIG_VL53L1X_INTERRUPT_MODE
43 struct gpio_callback gpio_cb;
44 struct k_work work;
45 const struct device *dev;
46 #endif
47 };
48
vl53l1x_read_sensor(struct vl53l1x_data * drv_data)49 static VL53L1_Error vl53l1x_read_sensor(struct vl53l1x_data *drv_data)
50 {
51 VL53L1_Error ret;
52
53 ret = VL53L1_GetRangingMeasurementData(&drv_data->vl53l1x, &drv_data->data);
54 if (ret != VL53L1_ERROR_NONE) {
55 LOG_ERR("VL53L1_GetRangingMeasurementData return error (%d)", ret);
56 return ret;
57 }
58
59 ret = VL53L1_ClearInterruptAndStartMeasurement(&drv_data->vl53l1x);
60 if (ret != VL53L1_ERROR_NONE) {
61 LOG_ERR("VL53L1_ClearInterruptAndStartMeasurement return error (%d)", ret);
62 return ret;
63 }
64
65 return VL53L1_ERROR_NONE;
66 }
67
68 #ifdef CONFIG_VL53L1X_INTERRUPT_MODE
vl53l1x_worker(struct k_work * work)69 static void vl53l1x_worker(struct k_work *work)
70 {
71 struct vl53l1x_data *drv_data = CONTAINER_OF(work, struct vl53l1x_data, work);
72
73 vl53l1x_read_sensor(drv_data);
74 }
75
vl53l1x_gpio_callback(const struct device * dev,struct gpio_callback * cb,uint32_t pins)76 static void vl53l1x_gpio_callback(const struct device *dev,
77 struct gpio_callback *cb, uint32_t pins)
78 {
79 struct vl53l1x_data *drv_data = CONTAINER_OF(cb, struct vl53l1x_data, gpio_cb);
80
81 k_work_submit(&drv_data->work);
82 }
83
vl53l1x_init_interrupt(const struct device * dev)84 static int vl53l1x_init_interrupt(const struct device *dev)
85 {
86 struct vl53l1x_data *drv_data = dev->data;
87 const struct vl53l1x_config *config = dev->config;
88 int ret;
89
90 drv_data->dev = dev;
91
92 if (!gpio_is_ready_dt(&config->gpio1)) {
93 LOG_ERR("%s: device %s is not ready", dev->name, config->gpio1.port->name);
94 return -ENODEV;
95 }
96
97 ret = gpio_pin_configure_dt(&config->gpio1, GPIO_INPUT | GPIO_PULL_UP);
98 if (ret < 0) {
99 LOG_ERR("[%s] Unable to configure GPIO interrupt", dev->name);
100 return -EIO;
101 }
102
103 gpio_init_callback(&drv_data->gpio_cb,
104 vl53l1x_gpio_callback,
105 BIT(config->gpio1.pin));
106
107 ret = gpio_add_callback(config->gpio1.port, &drv_data->gpio_cb);
108 if (ret < 0) {
109 LOG_ERR("Failed to set gpio callback!");
110 return -EIO;
111 }
112
113 drv_data->work.handler = vl53l1x_worker;
114
115 return 0;
116 }
117 #endif
118
vl53l1x_initialize(const struct device * dev)119 static int vl53l1x_initialize(const struct device *dev)
120 {
121 struct vl53l1x_data *drv_data = dev->data;
122 VL53L1_Error ret;
123 VL53L1_DeviceInfo_t vl53l1x_dev_info;
124
125 LOG_DBG("[%s] Initializing ", dev->name);
126
127 /* Pull XSHUT high to start the sensor */
128 #ifdef CONFIG_VL53L1X_XSHUT
129 const struct vl53l1x_config *const config = dev->config;
130
131 if (config->xshut.port) {
132 int gpio_ret = gpio_pin_set_dt(&config->xshut, 1);
133
134 if (gpio_ret < 0) {
135 LOG_ERR("[%s] Unable to set XSHUT gpio (error %d)", dev->name, gpio_ret);
136 return -EIO;
137 }
138 /* Boot duration is 1.2 ms max */
139 k_sleep(K_MSEC(2));
140 }
141 #endif
142
143 /* ONE TIME device initialization.
144 * To be called ONLY ONCE after device is brought out of reset
145 */
146 ret = VL53L1_DataInit(&drv_data->vl53l1x);
147 if (ret != VL53L1_ERROR_NONE) {
148 LOG_ERR("[%s] VL53L1X_DataInit return error (%d)", dev->name, ret);
149 return -ENOTSUP;
150 }
151
152 /* Do basic device init */
153 ret = VL53L1_StaticInit(&drv_data->vl53l1x);
154 if (ret != VL53L1_ERROR_NONE) {
155 LOG_ERR("[%s] VL53L1_StaticInit return error (%d)", dev->name, ret);
156 return -ENOTSUP;
157 }
158
159 /* Get info from sensor */
160 (void)memset(&vl53l1x_dev_info, 0, sizeof(VL53L1_DeviceInfo_t));
161
162 ret = VL53L1_GetDeviceInfo(&drv_data->vl53l1x, &vl53l1x_dev_info);
163 if (ret != VL53L1_ERROR_NONE) {
164 LOG_ERR("[%s] VL53L1_GetDeviceInfo return error (%d)", dev->name, ret);
165 return -ENODEV;
166 }
167
168 LOG_DBG("[%s] VL53L1X_GetDeviceInfo returned %d", dev->name, ret);
169 LOG_DBG(" Device Name : %s", vl53l1x_dev_info.Name);
170 LOG_DBG(" Device Type : %s", vl53l1x_dev_info.Type);
171 LOG_DBG(" Device ID : %s", vl53l1x_dev_info.ProductId);
172 LOG_DBG(" ProductRevisionMajor : %d", vl53l1x_dev_info.ProductRevisionMajor);
173 LOG_DBG(" ProductRevisionMinor : %d", vl53l1x_dev_info.ProductRevisionMinor);
174
175 /* Set default distance mode */
176 drv_data->distance_mode = VL53L1_DISTANCEMODE_LONG;
177 ret = VL53L1_SetDistanceMode(&drv_data->vl53l1x, drv_data->distance_mode);
178 if (ret != VL53L1_ERROR_NONE) {
179 LOG_ERR("[%s] VL53L1_SetDistanceMode return error (%d)", dev->name, ret);
180 return -EINVAL;
181 }
182
183 return 0;
184 }
185
186 /* Mapping is 1:1 with the API.
187 * From VL531X datasheet:
188 * | Max distance | Max distance in
189 * Mode | in dark (cm) | strong ambient light (cm)
190 * ----------------------------------------------------
191 * short | 136 | 135
192 * medium | 290 | 76
193 * long | 360 | 73
194 */
vl53l1x_set_mode(const struct device * dev,const struct sensor_value * val)195 static int vl53l1x_set_mode(const struct device *dev,
196 const struct sensor_value *val)
197 {
198 struct vl53l1x_data *drv_data = dev->data;
199 VL53L1_Error ret;
200
201 switch (val->val1) {
202 /* short */
203 case 1:
204 /* medium */
205 case 2:
206 /* long */
207 case 3:
208 drv_data->distance_mode = val->val1;
209 break;
210 default:
211 drv_data->distance_mode = VL53L1_DISTANCEMODE_LONG;
212 break;
213 }
214
215 ret = VL53L1_SetDistanceMode(&drv_data->vl53l1x, drv_data->distance_mode);
216 if (ret != VL53L1_ERROR_NONE) {
217 LOG_ERR("[%s] VL53L1_SetDistanceMode return error (%d)", dev->name, ret);
218 return -EINVAL;
219 }
220
221 return 0;
222 }
223
224 /*
225 * The ROI is a 16x16 grid.
226 * The bottom left is (0,0), top right is (15, 15), for
227 * a total of 256 squares (numbered 0 through 255).
228 * The default ROI is val1 = 240, val2 = 15 (the full grid).
229 * See UM2356 User Manual (VL531X API doc).
230 */
vl53l1x_set_roi(const struct device * dev,const struct sensor_value * val)231 static int vl53l1x_set_roi(const struct device *dev,
232 const struct sensor_value *val)
233 {
234 struct vl53l1x_data *drv_data = dev->data;
235 VL53L1_Error ret;
236
237 if ((val->val1 < 0) ||
238 (val->val2 < 0) ||
239 (val->val1 > 255) ||
240 (val->val2 > 255) ||
241 (val->val2 >= val->val1)) {
242 return -EINVAL;
243 }
244
245 /* Map val to pUserROi */
246 VL53L1_UserRoi_t pUserROi = {
247 .TopLeftX = val->val1 % 16,
248 .TopLeftY = (uint8_t)(val->val1 / 16),
249 .BotRightX = val->val2 % 16,
250 .BotRightY = (uint8_t)(val->val2 / 16),
251 };
252
253 ret = VL53L1_SetUserROI(&drv_data->vl53l1x, &pUserROi);
254 if (ret != VL53L1_ERROR_NONE) {
255 LOG_ERR("[%s] VL53L1_SetUserROI return error (%d)", dev->name, ret);
256 return -EINVAL;
257 }
258
259 return 0;
260 }
261
vl53l1x_get_mode(const struct device * dev,struct sensor_value * val)262 static int vl53l1x_get_mode(const struct device *dev,
263 struct sensor_value *val)
264 {
265 struct vl53l1x_data *drv_data = dev->data;
266 VL53L1_DistanceModes mode;
267 VL53L1_Error ret;
268
269 ret = VL53L1_GetDistanceMode(&drv_data->vl53l1x, &mode);
270 if (ret != VL53L1_ERROR_NONE) {
271 LOG_ERR("[%s] VL53L1_GetDistanceMode return error (%d)", dev->name, ret);
272 return -ENODATA;
273 }
274
275 /* Mapping is 1:1 with the API */
276 val->val1 = (int32_t)mode;
277 val->val2 = 0;
278 return 0;
279 }
280
vl53l1x_get_roi(const struct device * dev,struct sensor_value * val)281 static int vl53l1x_get_roi(const struct device *dev,
282 struct sensor_value *val)
283 {
284 struct vl53l1x_data *drv_data = dev->data;
285 VL53L1_Error ret;
286 VL53L1_UserRoi_t pUserROi;
287
288 ret = VL53L1_GetUserROI(&drv_data->vl53l1x, &pUserROi);
289 if (ret != VL53L1_ERROR_NONE) {
290 LOG_ERR("[%s] VL53L1_GetUserROI return error (%d)", dev->name, ret);
291 return -ENODATA;
292 }
293
294 /* Map pUserROi to val */
295 val->val1 = (int32_t)((16 * pUserROi.TopLeftY) + pUserROi.TopLeftX);
296 val->val2 = (int32_t)((16 * pUserROi.BotRightY) + pUserROi.BotRightX);
297 return 0;
298 }
299
vl53l1x_sample_fetch(const struct device * dev,enum sensor_channel chan)300 static int vl53l1x_sample_fetch(const struct device *dev,
301 enum sensor_channel chan)
302 {
303 struct vl53l1x_data *drv_data = dev->data;
304 VL53L1_Error ret;
305
306 __ASSERT_NO_MSG((chan == SENSOR_CHAN_ALL)
307 || (chan == SENSOR_CHAN_DISTANCE));
308
309 /* Will immediately stop current measurement */
310 ret = VL53L1_StopMeasurement(&drv_data->vl53l1x);
311 if (ret != VL53L1_ERROR_NONE) {
312 LOG_ERR("VL53L1_StopMeasurement return error (%d)", ret);
313 return -EBUSY;
314 }
315
316 #ifdef CONFIG_VL53L1X_INTERRUPT_MODE
317 const struct vl53l1x_config *config = dev->config;
318
319 ret = gpio_pin_interrupt_configure_dt(&config->gpio1, GPIO_INT_EDGE_TO_INACTIVE);
320 if (ret < 0) {
321 LOG_ERR("[%s] Unable to config interrupt", dev->name);
322 return -EIO;
323 }
324 #endif
325
326 ret = VL53L1_StartMeasurement(&drv_data->vl53l1x);
327 if (ret != VL53L1_ERROR_NONE) {
328 LOG_ERR("[%s] VL53L1_StartMeasurement return error (%d)", dev->name, ret);
329 return -EBUSY;
330 }
331
332 return 0;
333 }
334
vl53l1x_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)335 static int vl53l1x_channel_get(const struct device *dev,
336 enum sensor_channel chan,
337 struct sensor_value *val)
338 {
339 struct vl53l1x_data *drv_data = dev->data;
340 VL53L1_Error ret;
341
342 if (chan != SENSOR_CHAN_DISTANCE) {
343 return -ENOTSUP;
344 }
345
346 /* Calling VL53L1_WaitMeasurementDataReady regardless of using interrupt or
347 * polling method ensures user does not have to consider the time between
348 * calling fetch and get.
349 */
350 ret = VL53L1_WaitMeasurementDataReady(&drv_data->vl53l1x);
351 if (ret != VL53L1_ERROR_NONE) {
352 LOG_ERR("[%s] VL53L1_WaitMeasurementDataReady return error (%d)", dev->name, ret);
353 return -EBUSY;
354 }
355
356 if (IS_ENABLED(CONFIG_VL53L1X_INTERRUPT_MODE) == 0) {
357 /* Using driver poling mode */
358 ret = vl53l1x_read_sensor(drv_data);
359 if (ret != VL53L1_ERROR_NONE) {
360 return -ENODATA;
361 }
362 }
363
364 val->val1 = (int32_t)(drv_data->data.RangeMilliMeter);
365 /* RangeFractionalPart not implemented in API */
366 val->val2 = 0;
367
368 return 0;
369 }
370
vl53l1x_attr_get(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,struct sensor_value * val)371 static int vl53l1x_attr_get(const struct device *dev,
372 enum sensor_channel chan,
373 enum sensor_attribute attr,
374 struct sensor_value *val)
375 {
376 __ASSERT_NO_MSG(chan == SENSOR_CHAN_DISTANCE);
377
378 if (attr == SENSOR_ATTR_CONFIGURATION) {
379 vl53l1x_get_mode(dev, val);
380 } else if (attr == SENSOR_ATTR_CALIB_TARGET) {
381 vl53l1x_get_roi(dev, val);
382 } else {
383 return -ENOTSUP;
384 }
385
386 return 0;
387 }
388
vl53l1x_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)389 static int vl53l1x_attr_set(const struct device *dev,
390 enum sensor_channel chan,
391 enum sensor_attribute attr,
392 const struct sensor_value *val)
393 {
394 __ASSERT_NO_MSG(chan == SENSOR_CHAN_DISTANCE);
395
396 if (attr == SENSOR_ATTR_CONFIGURATION) {
397 vl53l1x_set_mode(dev, val);
398 } else if (attr == SENSOR_ATTR_CALIB_TARGET) {
399 vl53l1x_set_roi(dev, val);
400 } else {
401 return -ENOTSUP;
402 }
403
404 return 0;
405 }
406
407 static const struct sensor_driver_api vl53l1x_api_funcs = {
408 .sample_fetch = vl53l1x_sample_fetch,
409 .channel_get = vl53l1x_channel_get,
410 .attr_get = vl53l1x_attr_get,
411 .attr_set = vl53l1x_attr_set,
412 };
413
vl53l1x_init(const struct device * dev)414 static int vl53l1x_init(const struct device *dev)
415 {
416 int ret = 0;
417 struct vl53l1x_data *drv_data = dev->data;
418 const struct vl53l1x_config *config = dev->config;
419
420 /* Initialize the HAL i2c peripheral */
421 drv_data->vl53l1x.i2c = &config->i2c;
422
423 if (!device_is_ready(config->i2c.bus)) {
424 LOG_ERR("I2C bus is not ready");
425 return -ENODEV;
426 }
427
428 /* Configure gpio connected to VL53L1X's XSHUT pin to
429 * allow deepest sleep mode
430 */
431 #ifdef CONFIG_VL53L1X_XSHUT
432 if (config->xshut.port) {
433 ret = gpio_pin_configure_dt(&config->xshut, GPIO_OUTPUT);
434 if (ret < 0) {
435 LOG_ERR("[%s] Unable to configure GPIO as output", dev->name);
436 return -EIO;
437 }
438 }
439 #endif
440
441 #ifdef CONFIG_VL53L1X_INTERRUPT_MODE
442 if (config->gpio1.port) {
443 ret = vl53l1x_init_interrupt(dev);
444 if (ret < 0) {
445 LOG_ERR("Failed to initialize interrupt!");
446 return -EIO;
447 }
448 }
449 #endif
450
451 ret = vl53l1x_initialize(dev);
452 if (ret) {
453 return ret;
454 }
455
456 LOG_DBG("[%s] Initialized", dev->name);
457 return 0;
458 }
459
460 #define VL53L1X_INIT(i) \
461 static const struct vl53l1x_config vl53l1x_config_##i = { \
462 .i2c = I2C_DT_SPEC_INST_GET(i), \
463 IF_ENABLED(CONFIG_VL53L1X_XSHUT, ( \
464 .xshut = GPIO_DT_SPEC_INST_GET_OR(i, xshut_gpios, { 0 }),)) \
465 IF_ENABLED(CONFIG_VL53L1X_INTERRUPT_MODE, ( \
466 .gpio1 = GPIO_DT_SPEC_INST_GET_OR(i, int_gpios, { 0 }),)) \
467 }; \
468 \
469 static struct vl53l1x_data vl53l1x_data_##i; \
470 \
471 SENSOR_DEVICE_DT_INST_DEFINE(i, \
472 vl53l1x_init, \
473 NULL, \
474 &vl53l1x_data_##i, \
475 &vl53l1x_config_##i, \
476 POST_KERNEL, \
477 CONFIG_SENSOR_INIT_PRIORITY, \
478 &vl53l1x_api_funcs);
479
480 DT_INST_FOREACH_STATUS_OKAY(VL53L1X_INIT)
481
