1 /* vl53l0x.c - Driver for ST VL53L0X time of flight sensor */
2
3 #define DT_DRV_COMPAT st_vl53l0x
4
5 /*
6 * Copyright (c) 2017 STMicroelectronics
7 *
8 * SPDX-License-Identifier: Apache-2.0
9 */
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/pm/device.h>
19 #include <zephyr/sys/__assert.h>
20 #include <zephyr/types.h>
21 #include <zephyr/device.h>
22 #include <zephyr/logging/log.h>
23 #include <zephyr/drivers/sensor/vl53l0x.h>
24
25 #include "vl53l0x_api.h"
26 #include "vl53l0x_platform.h"
27
28 LOG_MODULE_REGISTER(VL53L0X, CONFIG_SENSOR_LOG_LEVEL);
29
30 #define VL53L0X_FIXPOINT1616_SCALE_FACTOR (65536)
31 #define VL53L0X_SENSOR_CHANNEL_VAL2_FACTOR (1000000)
32
33 /* All the values used in this driver are coming from ST datasheet and examples.
34 * It can be found here:
35 * https://www.st.com/en/embedded-software/stsw-img005.html
36 * There are also examples of use in the L4 cube FW:
37 * https://www.st.com/en/embedded-software/stm32cubel4.html
38 */
39 #define VL53L0X_INITIAL_ADDR 0x29
40 #define VL53L0X_REG_WHO_AM_I 0xC0
41 #define VL53L0X_CHIP_ID 0xEEAA
42 #define VL53L0X_SETUP_SIGNAL_LIMIT (0.1 * 65536)
43 #define VL53L0X_SETUP_SIGMA_LIMIT (60 * 65536)
44 #define VL53L0X_SETUP_MAX_TIME_FOR_RANGING 33000
45 #define VL53L0X_SETUP_PRE_RANGE_VCSEL_PERIOD 18
46 #define VL53L0X_SETUP_FINAL_RANGE_VCSEL_PERIOD 14
47
48 /* tBOOT (1.2ms max.) VL53L0X firmware boot period */
49 #define T_BOOT K_USEC(1200)
50
51 struct vl53l0x_config {
52 struct i2c_dt_spec i2c;
53 struct gpio_dt_spec xshut;
54 };
55
56 struct vl53l0x_data {
57 bool started;
58 VL53L0X_Dev_t vl53l0x;
59 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
60 };
61
vl53l0x_setup_single_shot(const struct device * dev)62 static int vl53l0x_setup_single_shot(const struct device *dev)
63 {
64 struct vl53l0x_data *drv_data = dev->data;
65 int ret;
66 uint8_t VhvSettings;
67 uint8_t PhaseCal;
68 uint32_t refSpadCount;
69 uint8_t isApertureSpads;
70
71 ret = VL53L0X_StaticInit(&drv_data->vl53l0x);
72 if (ret) {
73 LOG_ERR("[%s] VL53L0X_StaticInit failed",
74 dev->name);
75 goto exit;
76 }
77
78 ret = VL53L0X_PerformRefCalibration(&drv_data->vl53l0x,
79 &VhvSettings,
80 &PhaseCal);
81 if (ret) {
82 LOG_ERR("[%s] VL53L0X_PerformRefCalibration failed",
83 dev->name);
84 goto exit;
85 }
86
87 ret = VL53L0X_PerformRefSpadManagement(&drv_data->vl53l0x,
88 &refSpadCount,
89 &isApertureSpads);
90 if (ret) {
91 LOG_ERR("[%s] VL53L0X_PerformRefSpadManagement failed",
92 dev->name);
93 goto exit;
94 }
95
96 ret = VL53L0X_SetDeviceMode(&drv_data->vl53l0x,
97 VL53L0X_DEVICEMODE_SINGLE_RANGING);
98 if (ret) {
99 LOG_ERR("[%s] VL53L0X_SetDeviceMode failed",
100 dev->name);
101 goto exit;
102 }
103
104 ret = VL53L0X_SetLimitCheckEnable(&drv_data->vl53l0x,
105 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
106 1);
107 if (ret) {
108 LOG_ERR("[%s] VL53L0X_SetLimitCheckEnable sigma failed",
109 dev->name);
110 goto exit;
111 }
112
113 ret = VL53L0X_SetLimitCheckEnable(&drv_data->vl53l0x,
114 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
115 1);
116 if (ret) {
117 LOG_ERR("[%s] VL53L0X_SetLimitCheckEnable signal rate failed",
118 dev->name);
119 goto exit;
120 }
121
122 ret = VL53L0X_SetLimitCheckValue(&drv_data->vl53l0x,
123 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
124 VL53L0X_SETUP_SIGNAL_LIMIT);
125
126 if (ret) {
127 LOG_ERR("[%s] VL53L0X_SetLimitCheckValue signal rate failed",
128 dev->name);
129 goto exit;
130 }
131
132 ret = VL53L0X_SetLimitCheckValue(&drv_data->vl53l0x,
133 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
134 VL53L0X_SETUP_SIGMA_LIMIT);
135 if (ret) {
136 LOG_ERR("[%s] VL53L0X_SetLimitCheckValue sigma failed",
137 dev->name);
138 goto exit;
139 }
140
141 ret = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&drv_data->vl53l0x,
142 VL53L0X_SETUP_MAX_TIME_FOR_RANGING);
143 if (ret) {
144 LOG_ERR("[%s] VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed",
145 dev->name);
146 goto exit;
147 }
148
149 ret = VL53L0X_SetVcselPulsePeriod(&drv_data->vl53l0x,
150 VL53L0X_VCSEL_PERIOD_PRE_RANGE,
151 VL53L0X_SETUP_PRE_RANGE_VCSEL_PERIOD);
152 if (ret) {
153 LOG_ERR("[%s] VL53L0X_SetVcselPulsePeriod pre range failed",
154 dev->name);
155 goto exit;
156 }
157
158 ret = VL53L0X_SetVcselPulsePeriod(&drv_data->vl53l0x,
159 VL53L0X_VCSEL_PERIOD_FINAL_RANGE,
160 VL53L0X_SETUP_FINAL_RANGE_VCSEL_PERIOD);
161 if (ret) {
162 LOG_ERR("[%s] VL53L0X_SetVcselPulsePeriod final range failed",
163 dev->name);
164 goto exit;
165 }
166
167 exit:
168 return ret;
169 }
170
vl53l0x_start(const struct device * dev)171 static int vl53l0x_start(const struct device *dev)
172 {
173 const struct vl53l0x_config *const config = dev->config;
174 struct vl53l0x_data *drv_data = dev->data;
175 int r;
176 VL53L0X_Error ret;
177 uint16_t vl53l0x_id = 0U;
178 VL53L0X_DeviceInfo_t vl53l0x_dev_info = { 0 };
179
180 LOG_DBG("[%s] Starting", dev->name);
181
182 if (config->xshut.port) {
183 r = gpio_pin_configure_dt(&config->xshut, GPIO_OUTPUT_INACTIVE);
184 if (r < 0) {
185 LOG_ERR("[%s] Unable to inactivate XSHUT: %d",
186 dev->name, r);
187 return -EIO;
188 }
189 k_sleep(T_BOOT);
190 }
191
192 #ifdef CONFIG_VL53L0X_RECONFIGURE_ADDRESS
193 if (config->i2c.addr != VL53L0X_INITIAL_ADDR) {
194 ret = VL53L0X_SetDeviceAddress(&drv_data->vl53l0x, 2 * config->i2c.addr);
195 if (ret != 0) {
196 LOG_ERR("[%s] Unable to reconfigure I2C address",
197 dev->name);
198 return -EIO;
199 }
200
201 drv_data->vl53l0x.I2cDevAddr = config->i2c.addr;
202 LOG_DBG("[%s] I2C address reconfigured", dev->name);
203 k_sleep(T_BOOT);
204 }
205 #endif
206
207 ret = VL53L0X_GetDeviceInfo(&drv_data->vl53l0x, &vl53l0x_dev_info);
208 if (ret < 0) {
209 LOG_ERR("[%s] Could not get info from device.", dev->name);
210 return -ENODEV;
211 }
212
213 LOG_DBG("[%s] VL53L0X_GetDeviceInfo = %d", dev->name, ret);
214 LOG_DBG(" Device Name : %s", vl53l0x_dev_info.Name);
215 LOG_DBG(" Device Type : %s", vl53l0x_dev_info.Type);
216 LOG_DBG(" Device ID : %s", vl53l0x_dev_info.ProductId);
217 LOG_DBG(" ProductRevisionMajor : %d",
218 vl53l0x_dev_info.ProductRevisionMajor);
219 LOG_DBG(" ProductRevisionMinor : %d",
220 vl53l0x_dev_info.ProductRevisionMinor);
221
222 ret = VL53L0X_RdWord(&drv_data->vl53l0x,
223 VL53L0X_REG_WHO_AM_I,
224 &vl53l0x_id);
225 if ((ret < 0) || (vl53l0x_id != VL53L0X_CHIP_ID)) {
226 LOG_ERR("[%s] Issue on device identification", dev->name);
227 return -ENOTSUP;
228 }
229
230 /* sensor init */
231 ret = VL53L0X_DataInit(&drv_data->vl53l0x);
232 if (ret < 0) {
233 LOG_ERR("[%s] VL53L0X_DataInit return error (%d)",
234 dev->name, ret);
235 return -ENOTSUP;
236 }
237
238 ret = vl53l0x_setup_single_shot(dev);
239 if (ret < 0) {
240 return -ENOTSUP;
241 }
242
243 drv_data->started = true;
244 LOG_DBG("[%s] Started", dev->name);
245 return 0;
246 }
247
vl53l0x_sample_fetch(const struct device * dev,enum sensor_channel chan)248 static int vl53l0x_sample_fetch(const struct device *dev,
249 enum sensor_channel chan)
250 {
251 struct vl53l0x_data *drv_data = dev->data;
252 VL53L0X_Error ret;
253 int r;
254
255 __ASSERT_NO_MSG((chan == SENSOR_CHAN_ALL)
256 || (chan == SENSOR_CHAN_DISTANCE)
257 || (chan == SENSOR_CHAN_PROX));
258
259 if (!drv_data->started) {
260 r = vl53l0x_start(dev);
261 if (r < 0) {
262 return r;
263 }
264 }
265
266 ret = VL53L0X_PerformSingleRangingMeasurement(&drv_data->vl53l0x,
267 &drv_data->RangingMeasurementData);
268 if (ret < 0) {
269 LOG_ERR("[%s] Could not perform measurment (error=%d)",
270 dev->name, ret);
271 return -EINVAL;
272 }
273
274 return 0;
275 }
276
277
vl53l0x_channel_get(const struct device * dev,enum sensor_channel chan,struct sensor_value * val)278 static int vl53l0x_channel_get(const struct device *dev,
279 enum sensor_channel chan,
280 struct sensor_value *val)
281 {
282 struct vl53l0x_data *drv_data = dev->data;
283
284 if (chan == SENSOR_CHAN_PROX) {
285 if (drv_data->RangingMeasurementData.RangeMilliMeter <=
286 CONFIG_VL53L0X_PROXIMITY_THRESHOLD) {
287 val->val1 = 1;
288 } else {
289 val->val1 = 0;
290 }
291 val->val2 = 0;
292 } else if (chan == SENSOR_CHAN_DISTANCE) {
293 val->val1 = drv_data->RangingMeasurementData.RangeMilliMeter / 1000;
294 val->val2 = (drv_data->RangingMeasurementData.RangeMilliMeter % 1000) * 1000;
295 } else if ((enum sensor_channel_vl53l0x)chan ==
296 SENSOR_CHAN_VL53L0X_EFFECTIVE_SPAD_RTN_COUNT) {
297 val->val1 = drv_data->RangingMeasurementData.EffectiveSpadRtnCount / 256;
298 val->val2 = 0;
299 } else if ((enum sensor_channel_vl53l0x)chan == SENSOR_CHAN_VL53L0X_AMBIENT_RATE_RTN_CPS) {
300 val->val1 = (drv_data->RangingMeasurementData.AmbientRateRtnMegaCps >> 16) +
301 (((drv_data->RangingMeasurementData.AmbientRateRtnMegaCps & 0xFFFF) *
302 VL53L0X_SENSOR_CHANNEL_VAL2_FACTOR) / VL53L0X_FIXPOINT1616_SCALE_FACTOR);
303 val->val2 = 0;
304 } else if ((enum sensor_channel_vl53l0x)chan == SENSOR_CHAN_VL53L0X_SIGNAL_RATE_RTN_CPS) {
305 val->val1 = (drv_data->RangingMeasurementData.SignalRateRtnMegaCps >> 16) +
306 (((drv_data->RangingMeasurementData.SignalRateRtnMegaCps & 0xFFFF) *
307 VL53L0X_SENSOR_CHANNEL_VAL2_FACTOR) / VL53L0X_FIXPOINT1616_SCALE_FACTOR);
308 val->val2 = 0;
309 } else if ((enum sensor_channel_vl53l0x)chan == SENSOR_CHAN_VL53L0X_RANGE_DMAX) {
310 val->val1 = drv_data->RangingMeasurementData.RangeDMaxMilliMeter / 1000;
311 val->val2 = (drv_data->RangingMeasurementData.RangeDMaxMilliMeter % 1000) * 1000;
312 } else if ((enum sensor_channel_vl53l0x)chan == SENSOR_CHAN_VL53L0X_RANGE_STATUS) {
313 val->val1 = drv_data->RangingMeasurementData.RangeStatus;
314 val->val2 = 0;
315 } else {
316 return -ENOTSUP;
317 }
318
319 return 0;
320 }
321
322 static DEVICE_API(sensor, vl53l0x_api_funcs) = {
323 .sample_fetch = vl53l0x_sample_fetch,
324 .channel_get = vl53l0x_channel_get,
325 };
326
vl53l0x_init(const struct device * dev)327 static int vl53l0x_init(const struct device *dev)
328 {
329 int r;
330 struct vl53l0x_data *drv_data = dev->data;
331 const struct vl53l0x_config *const config = dev->config;
332
333 /* Initialize the HAL peripheral with the default sensor address,
334 * ie. the address on power up
335 */
336 drv_data->vl53l0x.I2cDevAddr = VL53L0X_INITIAL_ADDR;
337 drv_data->vl53l0x.i2c = config->i2c.bus;
338
339 #if defined(CONFIG_VL53L0X_RECONFIGURE_ADDRESS) || defined(CONFIG_PM_DEVICE)
340 if (config->xshut.port == NULL) {
341 LOG_ERR("[%s] Missing XSHUT gpio spec", dev->name);
342 return -ENOTSUP;
343 }
344 #endif
345
346 #ifdef CONFIG_VL53L0X_RECONFIGURE_ADDRESS
347 /*
348 * Shutdown all vl53l0x sensors so at each sensor's 1st fetch call
349 * they can be enabled one at a time and programmed with their address.
350 */
351 r = gpio_pin_configure_dt(&config->xshut, GPIO_OUTPUT_ACTIVE);
352 if (r < 0) {
353 LOG_ERR("[%s] Unable to shutdown sensor", dev->name);
354 return -EIO;
355 }
356 LOG_DBG("[%s] Shutdown", dev->name);
357 #else
358 if (config->i2c.addr != VL53L0X_INITIAL_ADDR) {
359 LOG_ERR("[%s] Invalid device address (should be 0x%X or "
360 "CONFIG_VL53L0X_RECONFIGURE_ADDRESS should be enabled)",
361 dev->name, VL53L0X_INITIAL_ADDR);
362 return -ENOTSUP;
363 }
364
365 r = vl53l0x_start(dev);
366 if (r) {
367 return r;
368 }
369 #endif
370
371 LOG_DBG("[%s] Initialized", dev->name);
372 return 0;
373 }
374
375 #ifdef CONFIG_PM_DEVICE
vl53l0x_pm_action(const struct device * dev,enum pm_device_action action)376 static int vl53l0x_pm_action(const struct device *dev,
377 enum pm_device_action action)
378 {
379 const struct vl53l0x_config *const config = dev->config;
380 int ret;
381
382 switch (action) {
383 case PM_DEVICE_ACTION_RESUME:
384 ret = vl53l0x_init(dev);
385 if (ret != 0) {
386 LOG_ERR("resume init: %d", ret);
387 }
388 break;
389 case PM_DEVICE_ACTION_SUSPEND:
390 /* HW Standby */
391 ret = gpio_pin_set_dt(&config->xshut, 1);
392 if (ret < 0) {
393 LOG_ERR("[%s] XSHUT pin active", dev->name);
394 }
395 break;
396 default:
397 ret = -ENOTSUP;
398 break;
399 }
400
401 return ret;
402 }
403 #endif
404
405 #define VL53L0X_INIT(inst) \
406 static struct vl53l0x_config vl53l0x_##inst##_config = { \
407 .i2c = I2C_DT_SPEC_INST_GET(inst), \
408 .xshut = GPIO_DT_SPEC_INST_GET_OR(inst, xshut_gpios, {}) \
409 }; \
410 \
411 static struct vl53l0x_data vl53l0x_##inst##_driver; \
412 \
413 PM_DEVICE_DT_INST_DEFINE(inst, vl53l0x_pm_action); \
414 \
415 SENSOR_DEVICE_DT_INST_DEFINE(inst, vl53l0x_init, \
416 PM_DEVICE_DT_INST_GET(inst), \
417 &vl53l0x_##inst##_driver, \
418 &vl53l0x_##inst##_config, \
419 POST_KERNEL, \
420 CONFIG_SENSOR_INIT_PRIORITY, \
421 &vl53l0x_api_funcs);
422
423 DT_INST_FOREACH_STATUS_OKAY(VL53L0X_INIT)
424
