1 /*
2 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
3 *
4 *
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <string.h>
10 #include "aos/kernel.h"
11 #include "sensor_drv_api.h"
12 #include "sensor_hal.h"
13
14 //#include "vl53l0x_proximity.h"
15 #include "vl53l0x/vl53l0x_def.h"
16 #include "vl53l0x/vl53l0x_api.h"
17 #include "vl53l0x_platform.h"
18
19 /* Private typedef -----------------------------------------------------------*/
20 /* Private defines -----------------------------------------------------------*/
21
22 #define PROXIMITY_I2C_ADDRESS ((uint16_t)0x0052)
23 #define VL53L0X_ID ((uint16_t)0xEEAA)
24 #define VL53L0X_XSHUT_Pin GPIO_PIN_6
25 #define VL53L0X_XSHUT_GPIO_Port GPIOC
26 #define VL53L0X_IRQ_PIN (62)
27
28 static gpio_irq_trigger_t vl53l0x_irq_mode_rising = IRQ_TRIGGER_RISING_EDGE;
29 bool g_interrupt_received = false;
30
31 /* Private macros ------------------------------------------------------------*/
32 /* Private variables ---------------------------------------------------------*/
33 VL53L0X_Dev_t Dev = { .I2cDevAddr = PROXIMITY_I2C_ADDRESS };
34
35 i2c_dev_t vl53l0x_ctx_i2c = {
36 .port = 3,
37 .config.address_width = 8,
38 .config.freq = 400000,
39 .config.dev_addr = PROXIMITY_I2C_ADDRESS,
40 };
41
42 static sensor_obj_t sensor;
43
44 /* Private function prototypes -----------------------------------------------*/
45 void SENSOR_IO_Init(void);
46 void SetupSingleShot(VL53L0X_Dev_t Dev);
47
48
VL53L0X_INTERRRUPT_INIT()49 void VL53L0X_INTERRRUPT_INIT()
50 {
51 int status;
52 (void)status;
53 if (sensor.mode == DEV_INT) {
54 status = VL53L0X_SetGpioConfig(
55 &Dev, 0, 0, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_LEVEL_HIGH,
56 VL53L0X_INTERRUPTPOLARITY_HIGH);
57 status = VL53L0X_SetInterruptThresholds(
58 &Dev, 0, (FixPoint1616_t)100 * 65536, (FixPoint1616_t)200 * 65536);
59 status =
60 VL53L0X_SetDeviceMode(&Dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
61 VL53L0X_ClearInterruptMask(&Dev, 0);
62 } else if (sensor.mode == DEV_DATA_READY) {
63 status = VL53L0X_SetGpioConfig(
64 &Dev, 0, 0, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY,
65 VL53L0X_INTERRUPTPOLARITY_HIGH);
66 status =
67 VL53L0X_SetDeviceMode(&Dev, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING);
68 VL53L0X_ClearInterruptMask(&Dev, 0);
69 } else {
70 }
71 }
72
73
74 /**
75 * @brief VL53L0X proximity sensor Initialization.
76 */
VL53L0X_PROXIMITY_Init(void)77 void VL53L0X_PROXIMITY_Init(void)
78 {
79 uint16_t vl53l0x_id = 0;
80 VL53L0X_DeviceInfo_t VL53L0X_DeviceInfo;
81
82 memset(&VL53L0X_DeviceInfo, 0, sizeof(VL53L0X_DeviceInfo_t));
83
84 if (VL53L0X_ERROR_NONE ==
85 VL53L0X_GetDeviceInfo(&Dev, &VL53L0X_DeviceInfo)) {
86 if (VL53L0X_ERROR_NONE ==
87 VL53L0X_RdWord(&Dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID,
88 (uint16_t *)&vl53l0x_id)) {
89 if (vl53l0x_id == VL53L0X_ID) {
90 if (VL53L0X_ERROR_NONE == VL53L0X_DataInit(&Dev)) {
91 SetupSingleShot(Dev);
92 }
93 }
94 } else {
95 LOG("VL53L0X Time of Flight Failed to Initialize!\n");
96 }
97 } else {
98 LOG("VL53L0X Time of Flight Failed to get infos!\n");
99 }
100 VL53L0X_SetInterMeasurementPeriodMilliSeconds(&Dev, 30);
101 VL53L0X_INTERRRUPT_INIT();
102 }
103
104 /**
105 * @brief Get distance from VL53L0X proximity sensor.
106 * @retval Distance in mm
107 */
VL53L0X_PROXIMITY_GetDistance(void)108 uint16_t VL53L0X_PROXIMITY_GetDistance(void)
109 {
110 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
111
112 VL53L0X_PerformSingleRangingMeasurement(&Dev, &RangingMeasurementData);
113
114 return RangingMeasurementData.RangeMilliMeter;
115 }
116
117 /**
118 * @brief VL53L0X proximity sensor Msp Initialization.
119 */
VL53L0X_PROXIMITY_MspInit(void)120 void VL53L0X_PROXIMITY_MspInit(void) {}
121
122 /**
123 * Setup all detected sensors for single shot mode and setup ranging
124 * configuration
125 */
SetupSingleShot(VL53L0X_Dev_t Dev)126 void SetupSingleShot(VL53L0X_Dev_t Dev)
127 {
128 int status;
129 uint8_t VhvSettings;
130 uint8_t PhaseCal;
131 uint32_t refSpadCount;
132 uint8_t isApertureSpads;
133 FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25 * 65536);
134 FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18 * 65536);
135 uint32_t timingBudget = 33000;
136 uint8_t preRangeVcselPeriod = 14;
137 uint8_t finalRangeVcselPeriod = 10;
138
139
140 if (Dev.Present) {
141 status = VL53L0X_StaticInit(&Dev);
142 if (status) {
143 LOG("VL53L0X_StaticInit failed\n");
144 }
145
146
147 status = VL53L0X_PerformRefCalibration(&Dev, &VhvSettings, &PhaseCal);
148 if (status) {
149 LOG("VL53L0X_PerformRefCalibration failed\n");
150 }
151
152 status = VL53L0X_PerformRefSpadManagement(&Dev, &refSpadCount,
153 &isApertureSpads);
154 if (status) {
155 LOG("VL53L0X_PerformRefSpadManagement failed\n");
156 }
157
158 status = VL53L0X_SetDeviceMode(
159 &Dev,
160 VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
161 if (status) {
162 LOG("VL53L0X_SetDeviceMode failed\n");
163 }
164
165 status = VL53L0X_SetLimitCheckEnable(
166 &Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
167 if (status) {
168 LOG("VL53L0X_SetLimitCheckEnable failed\n");
169 }
170
171 status = VL53L0X_SetLimitCheckEnable(
172 &Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
173 1); // Enable Signa limit
174 if (status) {
175 LOG("VL53L0X_SetLimitCheckEnable failed\n");
176 }
177
178 /* Ranging configuration */
179 signalLimit = (FixPoint1616_t)(0.1 * 65536);
180 sigmaLimit = (FixPoint1616_t)(60 * 65536);
181 // signalLimit = (FixPoint1616_t)(0.25*65536);
182 // sigmaLimit = (FixPoint1616_t)(18*65536);
183 timingBudget = 33000;
184 // timingBudget = 200000;
185 preRangeVcselPeriod = 18;
186 finalRangeVcselPeriod = 14;
187
188 status = VL53L0X_SetLimitCheckValue(
189 &Dev, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
190
191 if (status) {
192 LOG("VL53L0X_SetLimitCheckValue failed\n");
193 }
194
195 status = VL53L0X_SetLimitCheckValue(
196 &Dev, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
197 if (status) {
198 LOG("VL53L0X_SetLimitCheckValue failed\n");
199 }
200
201 status =
202 VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&Dev, timingBudget);
203 if (status) {
204 LOG("VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
205 }
206
207 status = VL53L0X_SetVcselPulsePeriod(
208 &Dev, VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
209 if (status) {
210 LOG("VL53L0X_SetVcselPulsePeriod failed\n");
211 }
212
213 status = VL53L0X_SetVcselPulsePeriod(
214 &Dev, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
215 if (status) {
216 LOG("VL53L0X_SetVcselPulsePeriod failed\n");
217 }
218
219 Dev.LeakyFirst = 1;
220 }
221 }
222
223
drv_ps_st_vl53l0x_irq_handle(void)224 static void drv_ps_st_vl53l0x_irq_handle(void)
225 {
226 VL53L0X_ClearInterruptMask(&Dev, 0);
227 g_interrupt_received = true;
228 }
229
drv_ps_st_vl53l0x_open(void)230 static int drv_ps_st_vl53l0x_open(void)
231 {
232 VL53L0X_StartMeasurement(&Dev);
233 LOG("%s %s successfully \n", SENSOR_STR, __func__);
234 return 0;
235 }
236
drv_ps_st_vl53l0x_close(void)237 static int drv_ps_st_vl53l0x_close(void)
238 {
239 VL53L0X_StopMeasurement(&Dev);
240 LOG("%s %s successfully \n", SENSOR_STR, __func__);
241 return 0;
242 }
243
drv_ps_st_vl53l0x_read(void * buf,size_t len)244 static int drv_ps_st_vl53l0x_read(void *buf, size_t len)
245 {
246 size_t size;
247 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
248 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
249 proximity_data_t * pdata = (proximity_data_t *)buf;
250 if (buf == NULL) {
251 return -1;
252 }
253 size = sizeof(proximity_data_t);
254 if (len < size) {
255 return -1;
256 }
257 if (g_interrupt_received == false) {
258 pdata->present = VL53L0X_PROXIMITY_GetDistance();
259 } else {
260 g_interrupt_received = false;
261 VL53L0X_ClearInterruptMask(&Dev, 0);
262 Status =
263 VL53L0X_GetRangingMeasurementData(&Dev, &RangingMeasurementData);
264 if (Status != VL53L0X_ERROR_NONE)
265 return -1;
266 pdata->present = RangingMeasurementData.RangeMilliMeter;
267 }
268 LOG("drv_ps_st_vl53l0x_read DATA=%d\n ", pdata->present);
269 return (int)size;
270 }
271
drv_ps_st_vl53l0x_write(const void * buf,size_t len)272 static int drv_ps_st_vl53l0x_write(const void *buf, size_t len)
273 {
274 return 0;
275 }
276
drv_ps_st_vl53l0x_ioctl(int cmd,unsigned long arg)277 static int drv_ps_st_vl53l0x_ioctl(int cmd, unsigned long arg)
278 {
279 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
280 switch (cmd) {
281 case SENSOR_IOCTL_ODR_SET: {
282 } break;
283 case SENSOR_IOCTL_SET_POWER: {
284 Status = VL53L0X_SetPowerMode(&Dev, arg);
285 if (Status != VL53L0X_ERROR_NONE)
286 return -1;
287 } break;
288 case SENSOR_IOCTL_GET_INFO: {
289 } break;
290
291 default:
292 break;
293 }
294
295 return 0;
296 }
297
298
drv_ps_st_vl53l0x_init(void)299 int drv_ps_st_vl53l0x_init(void)
300 {
301
302 int ret = 0;
303
304 /* fill the sensor obj parameters here */
305 sensor.tag = TAG_DEV_PS;
306 sensor.path = dev_ps_path;
307 sensor.io_port = I2C_PORT;
308 sensor.open = drv_ps_st_vl53l0x_open;
309 sensor.close = drv_ps_st_vl53l0x_close;
310 sensor.read = drv_ps_st_vl53l0x_read;
311 sensor.write = drv_ps_st_vl53l0x_write;
312 sensor.ioctl = drv_ps_st_vl53l0x_ioctl;
313 sensor.irq_handle = drv_ps_st_vl53l0x_irq_handle;
314 sensor.mode = DEV_POLLING;
315 sensor.gpio.port = VL53L0X_IRQ_PIN;
316 sensor.gpio.config = IRQ_MODE;
317 sensor.gpio.priv = &vl53l0x_irq_mode_rising;
318 sensor.data_len = sizeof(proximity_data_t);
319
320
321 ret = sensor_create_obj(&sensor);
322 if (unlikely(ret)) {
323 return -1;
324 }
325 VL53L0X_PROXIMITY_Init();
326
327 LOG("%s %s successfully \n", SENSOR_STR, __func__);
328
329 return 0;
330 }
331
332 SENSOR_DRV_ADD(drv_ps_st_vl53l0x_init);
333
334