Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
vl53l1x_api.c
Go to the documentation of this file.
1 /*
2  Copyright (c) 2017, STMicroelectronics - All Rights Reserved
3 
4  This file : part of VL53L1 Core and : dual licensed,
5  either 'STMicroelectronics
6  Proprietary license'
7  or 'BSD 3-clause "New" or "Revised" License' , at your option.
8 
9 *******************************************************************************
10 
11  'STMicroelectronics Proprietary license'
12 
13 *******************************************************************************
14 
15  License terms: STMicroelectronics Proprietary in accordance with licensing
16  terms at www.st.com/sla0081
17 
18  STMicroelectronics confidential
19  Reproduction and Communication of this document : strictly prohibited unless
20  specifically authorized in writing by STMicroelectronics.
21 
22 
23 *******************************************************************************
24 
25  Alternatively, VL53L1 Core may be distributed under the terms of
26  'BSD 3-clause "New" or "Revised" License', in which case the following
27  provisions apply instead of the ones mentioned above :
28 
29 *******************************************************************************
30 
31  License terms: BSD 3-clause "New" or "Revised" License.
32 
33  Redistribution and use in source and binary forms, with or without
34  modification, are permitted provided that the following conditions are met:
35 
36  1. Redistributions of source code must retain the above copyright notice, this
37  list of conditions and the following disclaimer.
38 
39  2. Redistributions in binary form must reproduce the above copyright notice,
40  this list of conditions and the following disclaimer in the documentation
41  and/or other materials provided with the distribution.
42 
43  3. Neither the name of the copyright holder nor the names of its contributors
44  may be used to endorse or promote products derived from this software
45  without specific prior written permission.
46 
47  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
48  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
49  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
50  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
51  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
52  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
53  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
54  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
55  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
56  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
57 *******************************************************************************
58 */
59 
65 #include "vl53l1x_api.h"
66 #include <string.h>
67 
68 #define I2C_1V8 0x00
69 #define I2C_2V8 0x01
70 #ifndef VL53L1X_I2C_LEVEL
71 #define VL53L1X_I2C_LEVEL I2C_2V8
72 #endif
73 
74 
75 #if 0
76 uint8_t VL51L1X_NVM_CONFIGURATION[] = {
77  0x00, /* 0x00 : not user-modifiable */
78  0x29, /* 0x01 : 7 bits I2C address (default=0x29), use SetI2CAddress(). Warning: after changing the register value to a new I2C address, the device will only answer to the new address */
79  0x00, /* 0x02 : not user-modifiable */
80  0x00, /* 0x03 : not user-modifiable */
81  0x00, /* 0x04 : not user-modifiable */
82  0x00, /* 0x05 : not user-modifiable */
83  0x00, /* 0x06 : not user-modifiable */
84  0x00, /* 0x07 : not user-modifiable */
85  0x00, /* 0x08 : not user-modifiable */
86  0x50, /* 0x09 : not user-modifiable */
87  0x00, /* 0x0A : not user-modifiable */
88  0x00, /* 0x0B : not user-modifiable */
89  0x00, /* 0x0C : not user-modifiable */
90  0x00, /* 0x0D : not user-modifiable */
91  0x0a, /* 0x0E : not user-modifiable */
92  0x00, /* 0x0F : not user-modifiable */
93  0x00, /* 0x10 : not user-modifiable */
94  0x00, /* 0x11 : not user-modifiable */
95  0x00, /* 0x12 : not user-modifiable */
96  0x00, /* 0x13 : not user-modifiable */
97  0x00, /* 0x14 : not user-modifiable */
98  0x00, /* 0x15 : not user-modifiable */
99  0x00, /* 0x16 : Xtalk calibration value MSB (7.9 format in kcps), use SetXtalk() */
100  0x00, /* 0x17 : Xtalk calibration value LSB */
101  0x00, /* 0x18 : not user-modifiable */
102  0x00, /* 0x19 : not user-modifiable */
103  0x00, /* 0x1a : not user-modifiable */
104  0x00, /* 0x1b : not user-modifiable */
105  0x00, /* 0x1e : Part to Part offset x4 MSB (in mm), use SetOffset() */
106  0x50, /* 0x1f : Part to Part offset x4 LSB */
107  0x00, /* 0x20 : not user-modifiable */
108  0x00, /* 0x21 : not user-modifiable */
109  0x00, /* 0x22 : not user-modifiable */
110  0x00, /* 0x23 : not user-modifiable */
111 }
112 #endif
113 
115  0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */
116  VL53L1X_I2C_LEVEL, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
117  0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */
118  0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */
119  0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */
120  0x00, /* 0x32 : not user-modifiable */
121  0x02, /* 0x33 : not user-modifiable */
122  0x08, /* 0x34 : not user-modifiable */
123  0x00, /* 0x35 : not user-modifiable */
124  0x08, /* 0x36 : not user-modifiable */
125  0x10, /* 0x37 : not user-modifiable */
126  0x01, /* 0x38 : not user-modifiable */
127  0x01, /* 0x39 : not user-modifiable */
128  0x00, /* 0x3a : not user-modifiable */
129  0x00, /* 0x3b : not user-modifiable */
130  0x00, /* 0x3c : not user-modifiable */
131  0x00, /* 0x3d : not user-modifiable */
132  0xff, /* 0x3e : not user-modifiable */
133  0x00, /* 0x3f : not user-modifiable */
134  0x0F, /* 0x40 : not user-modifiable */
135  0x00, /* 0x41 : not user-modifiable */
136  0x00, /* 0x42 : not user-modifiable */
137  0x00, /* 0x43 : not user-modifiable */
138  0x00, /* 0x44 : not user-modifiable */
139  0x00, /* 0x45 : not user-modifiable */
140  0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */
141  0x0b, /* 0x47 : not user-modifiable */
142  0x00, /* 0x48 : not user-modifiable */
143  0x00, /* 0x49 : not user-modifiable */
144  0x02, /* 0x4a : not user-modifiable */
145  0x0a, /* 0x4b : not user-modifiable */
146  0x21, /* 0x4c : not user-modifiable */
147  0x00, /* 0x4d : not user-modifiable */
148  0x00, /* 0x4e : not user-modifiable */
149  0x05, /* 0x4f : not user-modifiable */
150  0x00, /* 0x50 : not user-modifiable */
151  0x00, /* 0x51 : not user-modifiable */
152  0x00, /* 0x52 : not user-modifiable */
153  0x00, /* 0x53 : not user-modifiable */
154  0xc8, /* 0x54 : not user-modifiable */
155  0x00, /* 0x55 : not user-modifiable */
156  0x00, /* 0x56 : not user-modifiable */
157  0x38, /* 0x57 : not user-modifiable */
158  0xff, /* 0x58 : not user-modifiable */
159  0x01, /* 0x59 : not user-modifiable */
160  0x00, /* 0x5a : not user-modifiable */
161  0x08, /* 0x5b : not user-modifiable */
162  0x00, /* 0x5c : not user-modifiable */
163  0x00, /* 0x5d : not user-modifiable */
164  0x01, /* 0x5e : not user-modifiable */
165  0xcc, /* 0x5f : not user-modifiable */
166  0x0f, /* 0x60 : not user-modifiable */
167  0x01, /* 0x61 : not user-modifiable */
168  0xf1, /* 0x62 : not user-modifiable */
169  0x0d, /* 0x63 : not user-modifiable */
170  0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */
171  0x68, /* 0x65 : Sigma threshold LSB */
172  0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */
173  0x80, /* 0x67 : Min count Rate LSB */
174  0x08, /* 0x68 : not user-modifiable */
175  0xb8, /* 0x69 : not user-modifiable */
176  0x00, /* 0x6a : not user-modifiable */
177  0x00, /* 0x6b : not user-modifiable */
178  0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */
179  0x00, /* 0x6d : Intermeasurement period */
180  0x0f, /* 0x6e : Intermeasurement period */
181  0x89, /* 0x6f : Intermeasurement period LSB */
182  0x00, /* 0x70 : not user-modifiable */
183  0x00, /* 0x71 : not user-modifiable */
184  0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */
185  0x00, /* 0x73 : distance threshold high LSB */
186  0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */
187  0x00, /* 0x75 : distance threshold low LSB */
188  0x00, /* 0x76 : not user-modifiable */
189  0x01, /* 0x77 : not user-modifiable */
190  0x0f, /* 0x78 : not user-modifiable */
191  0x0d, /* 0x79 : not user-modifiable */
192  0x0e, /* 0x7a : not user-modifiable */
193  0x0e, /* 0x7b : not user-modifiable */
194  0x00, /* 0x7c : not user-modifiable */
195  0x00, /* 0x7d : not user-modifiable */
196  0x02, /* 0x7e : not user-modifiable */
197  0xc7, /* 0x7f : ROI center, use SetROI() */
198  0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */
199  0x9B, /* 0x81 : not user-modifiable */
200  0x00, /* 0x82 : not user-modifiable */
201  0x00, /* 0x83 : not user-modifiable */
202  0x00, /* 0x84 : not user-modifiable */
203  0x01, /* 0x85 : not user-modifiable */
204  0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */
205  0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */
206 };
207 
208 static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
209  255, 255, 9, 13, 255, 255, 255, 255, 10, 6,
210  255, 255, 11, 12
211  };
212 
213 #if VL53L1X_AUTO_INCR_ADDR
214 // start after default address
215 static uint8_t auto_incr_addr = VL53L1_DEFAULT_ADDRESS + 2;
216 #endif
217 
219 {
220  VL53L1X_ERROR Status = 0;
221 
226  return Status;
227 }
228 
230 {
231  VL53L1X_ERROR status = 0;
232 
234  if (!status) { dev->i2c_trans.slave_addr = new_address; }
235  return status;
236 }
237 
239 {
240  VL53L1X_ERROR status = 0;
241  uint8_t Addr = 0x00, tmp;
242 
243  for (Addr = 0x2D; Addr <= 0x87; Addr++) {
244  status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
245  }
247  tmp = 0;
248  while (tmp == 0) {
250  }
254  status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
255 #if VL53L1X_AUTO_INCR_ADDR
256  status = VL53L1X_SetI2CAddress(dev, auto_incr_addr);
257  auto_incr_addr += 2; // auto increment by 2 (+1 on 7 bits address)
258 #endif
259  return status;
260 }
261 
262 void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs)
263 {
264  uint8_t state;
265  do {
267  } while (!state);
269  /* Configure sensor */
270  VL53L1X_SetTimingBudgetInMs(dev, TimingBudgetInMs);
271  VL53L1X_SetDistanceMode(dev, DistanceMode);
272  VL53L1X_SetInterMeasurementInMs(dev, InterMeasurementInMs);
273  /* Start measurement */
275 }
276 
277 
279 {
280  VL53L1X_ERROR status = 0;
281 
283  return status;
284 }
285 
287 {
288  uint8_t Temp;
289  VL53L1X_ERROR status = 0;
290 
292  Temp = Temp & 0xEF;
293  status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4);
294  return status;
295 }
296 
298 {
299  uint8_t Temp;
300  VL53L1X_ERROR status = 0;
301 
303  Temp = Temp & 0x10;
304  *pInterruptPolarity = !(Temp >> 4);
305  return status;
306 }
307 
309 {
310  VL53L1X_ERROR status = 0;
311 
312  status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */
313  return status;
314 }
315 
317 {
318  VL53L1X_ERROR status = 0;
319 
320  status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */
321  return status;
322 }
323 
325 {
326  uint8_t Temp;
327  uint8_t IntPol;
328  VL53L1X_ERROR status = 0;
329 
332  /* Read in the register to check if a new value is available */
333  if (status == 0) {
334  if ((Temp & 1) == IntPol) {
335  *isDataReady = 1;
336  } else {
337  *isDataReady = 0;
338  }
339  }
340  return status;
341 }
342 
344 {
345  uint16_t DM;
346  VL53L1X_ERROR status = 0;
347 
349  if (DM == 0) {
350  return 1;
351  } else if (DM == 1) { /* Short DistanceMode */
352  switch (TimingBudgetInMs) {
353  case 15: /* only available in short distance mode */
355  0x01D);
357  0x0027);
358  break;
359  case 20:
361  0x0051);
363  0x006E);
364  break;
365  case 33:
367  0x00D6);
369  0x006E);
370  break;
371  case 50:
373  0x1AE);
375  0x01E8);
376  break;
377  case 100:
379  0x02E1);
381  0x0388);
382  break;
383  case 200:
385  0x03E1);
387  0x0496);
388  break;
389  case 500:
391  0x0591);
393  0x05C1);
394  break;
395  default:
396  status = 1;
397  break;
398  }
399  } else {
400  switch (TimingBudgetInMs) {
401  case 20:
403  0x001E);
405  0x0022);
406  break;
407  case 33:
409  0x0060);
411  0x006E);
412  break;
413  case 50:
415  0x00AD);
417  0x00C6);
418  break;
419  case 100:
421  0x01CC);
423  0x01EA);
424  break;
425  case 200:
427  0x02D9);
429  0x02F8);
430  break;
431  case 500:
433  0x048F);
435  0x04A4);
436  break;
437  default:
438  status = 1;
439  break;
440  }
441  }
442  return status;
443 }
444 
446 {
447  uint16_t Temp;
448  VL53L1X_ERROR status = 0;
449 
451  switch (Temp) {
452  case 0x001D :
453  *pTimingBudget = 15;
454  break;
455  case 0x0051 :
456  case 0x001E :
457  *pTimingBudget = 20;
458  break;
459  case 0x00D6 :
460  case 0x0060 :
461  *pTimingBudget = 33;
462  break;
463  case 0x1AE :
464  case 0x00AD :
465  *pTimingBudget = 50;
466  break;
467  case 0x02E1 :
468  case 0x01CC :
469  *pTimingBudget = 100;
470  break;
471  case 0x03E1 :
472  case 0x02D9 :
473  *pTimingBudget = 200;
474  break;
475  case 0x0591 :
476  case 0x048F :
477  *pTimingBudget = 500;
478  break;
479  default:
480  status = 1;
481  *pTimingBudget = 0;
482  }
483  return status;
484 }
485 
487 {
488  uint16_t TB;
489  VL53L1X_ERROR status = 0;
490 
492  if (status != 0) {
493  return 1;
494  }
495  switch (DM) {
496  case 1:
503  break;
504  case 2:
511  break;
512  default:
513  status = 1;
514  break;
515  }
516 
517  if (status == 0) {
519  }
520  return status;
521 }
522 
524 {
525  uint8_t TempDM, status = 0;
526 
528  if (TempDM == 0x14) {
529  *DM = 1;
530  }
531  if (TempDM == 0x0A) {
532  *DM = 2;
533  }
534  return status;
535 }
536 
538 {
539  uint16_t ClockPLL;
540  VL53L1X_ERROR status = 0;
541 
543  ClockPLL = ClockPLL & 0x3FF;
545  (uint32_t)(ClockPLL * InterMeasMs * 1.075));
546  return status;
547 
548 }
549 
551 {
552  uint16_t ClockPLL;
553  VL53L1X_ERROR status = 0;
554  uint32_t tmp;
555 
557  *pIM = (uint16_t)tmp;
559  ClockPLL = ClockPLL & 0x3FF;
560  *pIM = (uint16_t)(*pIM / (ClockPLL * 1.065));
561  return status;
562 }
563 
565 {
566  VL53L1X_ERROR status = 0;
567  uint8_t tmp = 0;
568 
570  *state = tmp;
571  return status;
572 }
573 
575 {
576  VL53L1X_ERROR status = 0;
577  uint16_t tmp = 0;
578 
580  *sensorId = tmp;
581  return status;
582 }
583 
585 {
586  VL53L1X_ERROR status = 0;
587  uint16_t tmp;
588 
591  *distance = tmp;
592  return status;
593 }
594 
596 {
597  VL53L1X_ERROR status = 0;
598  uint16_t SpNb = 1, signal;
599 
604  *signalRate = (uint16_t)(2000.0 * signal / SpNb);
605  return status;
606 }
607 
609 {
610  VL53L1X_ERROR status = 0;
611  uint16_t AmbientRate, SpNb = 1;
612 
615  *ambPerSp = (uint16_t)(2000.0 * AmbientRate / SpNb);
616  return status;
617 }
618 
620 {
621  VL53L1X_ERROR status = 0;
622  uint16_t tmp;
623 
626  *signal = tmp * 8;
627  return status;
628 }
629 
631 {
632  VL53L1X_ERROR status = 0;
633  uint16_t tmp;
634 
637  *spNb = tmp >> 8;
638  return status;
639 }
640 
642 {
643  VL53L1X_ERROR status = 0;
644  uint16_t tmp;
645 
647  *ambRate = tmp * 8;
648  return status;
649 }
650 
652 {
653  VL53L1X_ERROR status = 0;
654  uint8_t RgSt;
655 
656  *rangeStatus = 255;
658  RgSt = RgSt & 0x1F;
659  if (RgSt < 24) {
660  *rangeStatus = status_rtn[RgSt];
661  }
662  return status;
663 }
664 
666 {
667  VL53L1X_ERROR status = 0;
668  uint8_t Temp[17];
669  uint8_t RgSt = 255;
670 
672  RgSt = Temp[0] & 0x1F;
673  if (RgSt < 24) {
674  RgSt = status_rtn[RgSt];
675  }
676  pResult->Status = RgSt;
677  pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8;
678  pResult->NumSPADs = Temp[3];
679  pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8;
680  pResult->Distance = Temp[13] << 8 | Temp[14];
681 
682  return status;
683 }
684 
686 {
687  VL53L1X_ERROR status = 0;
688  int16_t Temp;
689 
690  Temp = (OffsetValue * 4);
692  (uint16_t)Temp);
695  return status;
696 }
697 
699 {
700  VL53L1X_ERROR status = 0;
701  uint16_t Temp;
702 
704  Temp = Temp << 3;
705  Temp = Temp >> 5;
706  *offset = (int16_t)(Temp);
707  return status;
708 }
709 
711 {
712  /* XTalkValue in count per second to avoid float type */
713  VL53L1X_ERROR status = 0;
714 
717  0x0000);
719  0x0000);
721  (XtalkValue << 9) / 1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */
722  return status;
723 }
724 
726 {
727  VL53L1X_ERROR status = 0;
728  uint32_t tmp;
729 
731  *xtalk = (uint16_t)(tmp * 1000) >> 9; /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */
732  return status;
733 }
734 
736  uint16_t ThreshHigh, uint8_t Window,
737  uint8_t IntOnNoTarget)
738 {
739  VL53L1X_ERROR status = 0;
740  uint8_t Temp = 0;
741 
743  Temp = Temp & 0x47;
744  if (IntOnNoTarget == 0) {
746  (Temp | (Window & 0x07)));
747  } else {
749  ((Temp | (Window & 0x07)) | 0x40));
750  }
753  return status;
754 }
755 
757 {
758  VL53L1X_ERROR status = 0;
759  uint8_t tmp;
761  *window = (uint16_t)(tmp & 0x7);
762  return status;
763 }
764 
766 {
767  VL53L1X_ERROR status = 0;
768  uint16_t tmp;
769 
771  *low = tmp;
772  return status;
773 }
774 
776 {
777  VL53L1X_ERROR status = 0;
778  uint16_t tmp;
779 
781  *high = tmp;
782  return status;
783 }
784 
786 {
787  VL53L1X_ERROR status = 0;
789  return status;
790 }
791 
793 {
794  VL53L1X_ERROR status = 0;
795  uint8_t tmp;
797  *ROICenter = tmp;
798  return status;
799 }
800 
802 {
803  uint8_t OpticalCenter;
804  VL53L1X_ERROR status = 0;
805 
807  if (X > 16) {
808  X = 16;
809  }
810  if (Y > 16) {
811  Y = 16;
812  }
813  if (X > 10 || Y > 10) {
814  OpticalCenter = 199;
815  }
818  (Y - 1) << 4 | (X - 1));
819  return status;
820 }
821 
823 {
824  VL53L1X_ERROR status = 0;
825  uint8_t tmp;
826 
828  *ROI_X = ((uint16_t)tmp & 0x0F) + 1;
829  *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1;
830  return status;
831 }
832 
834 {
835  VL53L1X_ERROR status = 0;
836 
838  return status;
839 }
840 
842 {
843  VL53L1X_ERROR status = 0;
844  uint16_t tmp;
845 
848  *signal = tmp << 3;
849  return status;
850 }
851 
853 {
854  VL53L1X_ERROR status = 0;
855 
856  if (Sigma > (0xFFFF >> 2)) {
857  return 1;
858  }
859  /* 16 bits register 14.2 format */
861  return status;
862 }
863 
865 {
866  VL53L1X_ERROR status = 0;
867  uint16_t tmp;
868 
870  *sigma = tmp >> 2;
871  return status;
872 
873 }
874 
876 {
877  VL53L1X_ERROR status = 0;
878  uint8_t tmp = 0;
879 
881  status = VL53L1_WrByte(dev, 0x0B, 0x92);
883  while (tmp == 0) {
885  }
886  tmp = 0;
890  status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */
891  return status;
892 }
VL53L1X_GetSignalPerSpad
VL53L1X_ERROR VL53L1X_GetSignalPerSpad(VL53L1_DEV dev, uint16_t *signalRate)
This function returns the returned signal per SPAD in kcps/SPAD.
Definition: vl53l1x_api.c:595
PHASECAL_CONFIG__TIMEOUT_MACROP
#define PHASECAL_CONFIG__TIMEOUT_MACROP
Definition: vl53l1x_api.h:92
uint16_t
unsigned short uint16_t
Definition: types.h:16
RESULT__AMBIENT_COUNT_RATE_MCPS_SD
#define RESULT__AMBIENT_COUNT_RATE_MCPS_SD
Definition: vl53l1x_api.h:114
VL53L1_WrByte
int8_t VL53L1_WrByte(VL53L1_DEV dev, uint16_t index, uint8_t data)
VL53L1_WrByte() definition.
Definition: vl53l1_platform.c:64
ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS
#define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS
Definition: vl53l1x_api.h:85
VL51L1X_DEFAULT_CONFIGURATION
const uint8_t VL51L1X_DEFAULT_CONFIGURATION[]
Definition: vl53l1x_api.c:114
VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0
#define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0
Definition: vl53l1x_api.h:115
VL53L1X_BootDevice
void VL53L1X_BootDevice(VL53L1_DEV dev, uint16_t TimingBudgetInMs, uint16_t DistanceMode, uint32_t InterMeasurementInMs)
Implement boot sequence of VL53L1 device as described in documentation See VL53L1X_SetTimingBudgetInM...
Definition: vl53l1x_api.c:262
VL53L1X_SetInterruptPolarity
VL53L1X_ERROR VL53L1X_SetInterruptPolarity(VL53L1_DEV dev, uint8_t NewPolarity)
This function programs the interrupt polarity 1=active high (default), 0=active low.
Definition: vl53l1x_api.c:286
VL53L1X_GetSensorId
VL53L1X_ERROR VL53L1X_GetSensorId(VL53L1_DEV dev, uint16_t *sensorId)
This function returns the sensor id, sensor Id must be 0xEEAC.
Definition: vl53l1x_api.c:574
status
uint8_t status
Definition: nps_radio_control_spektrum.c:101
RANGE_CONFIG__VALID_PHASE_HIGH
#define RANGE_CONFIG__VALID_PHASE_HIGH
Definition: vl53l1x_api.h:100
VL53L1_FIRMWARE__SYSTEM_STATUS
#define VL53L1_FIRMWARE__SYSTEM_STATUS
Definition: vl53l1x_api.h:118
VL53L1X_GetAmbientRate
VL53L1X_ERROR VL53L1X_GetAmbientRate(VL53L1_DEV dev, uint16_t *ambRate)
This function returns the ambient rate in kcps.
Definition: vl53l1x_api.c:641
VL53L1X_Version_t::build
uint8_t build
Definition: vl53l1x_api.h:134
VL53L1X_GetDistanceThresholdWindow
VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(VL53L1_DEV dev, uint16_t *window)
This function returns the window detection mode (0=below; 1=above; 2=out; 3=in)
Definition: vl53l1x_api.c:756
VL53L1X_SetDistanceMode
VL53L1X_ERROR VL53L1X_SetDistanceMode(VL53L1_DEV dev, uint16_t DM)
This function programs the distance mode (1=short, 2=long(default)).
Definition: vl53l1x_api.c:486
VL53L1X_CheckForDataReady
VL53L1X_ERROR VL53L1X_CheckForDataReady(VL53L1_DEV dev, uint8_t *isDataReady)
This function checks if the new ranging data is available by polling the dedicated register.
Definition: vl53l1x_api.c:324
VL53L1X_Result_t::Ambient
uint16_t Ambient
Definition: vl53l1x_api.h:142
VL53L1X_GetRangeStatus
VL53L1X_ERROR VL53L1X_GetRangeStatus(VL53L1_DEV dev, uint8_t *rangeStatus)
This function returns the ranging status error (0:no error, 1:sigma failed, 2:signal failed,...
Definition: vl53l1x_api.c:651
VL53L1X_StopRanging
VL53L1X_ERROR VL53L1X_StopRanging(VL53L1_DEV dev)
This function stops the ranging.
Definition: vl53l1x_api.c:316
RANGE_CONFIG__SIGMA_THRESH
#define RANGE_CONFIG__SIGMA_THRESH
Definition: vl53l1x_api.h:98
uint32_t
unsigned long uint32_t
Definition: types.h:18
vl53l1x_api.h
Functions definition.
RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS
#define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS
Definition: vl53l1x_api.h:99
VL53L1X_SetSigmaThreshold
VL53L1X_ERROR VL53L1X_SetSigmaThreshold(VL53L1_DEV dev, uint16_t Sigma)
This function programs a new sigma threshold in mm (default=15 mm)
Definition: vl53l1x_api.c:852
SYSTEM__THRESH_LOW
#define SYSTEM__THRESH_LOW
Definition: vl53l1x_api.h:103
VL53L1_ReadMulti
int8_t VL53L1_ReadMulti(VL53L1_DEV dev, uint16_t index, uint8_t *pdata, uint32_t count)
VL53L1_ReadMulti() definition.
Definition: vl53l1_platform.c:53
VL53L1X_GetROICenter
VL53L1X_ERROR VL53L1X_GetROICenter(VL53L1_DEV dev, uint8_t *ROICenter)
This function returns the current user ROI center.
Definition: vl53l1x_api.c:792
VL53L1X_SetDistanceThreshold
VL53L1X_ERROR VL53L1X_SetDistanceThreshold(VL53L1_DEV dev, uint16_t ThreshLow, uint16_t ThreshHigh, uint8_t Window, uint8_t IntOnNoTarget)
This function programs the threshold detection mode Example: VL53L1X_SetDistanceThreshold(dev,...
Definition: vl53l1x_api.c:735
VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD
#define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD
Definition: vl53l1x_api.h:101
VL53L1X_ClearInterrupt
VL53L1X_ERROR VL53L1X_ClearInterrupt(VL53L1_DEV dev)
This function clears the interrupt, to be called after a ranging data reading to arm the interrupt fo...
Definition: vl53l1x_api.c:278
VL53L1X_GetDistanceThresholdLow
VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(VL53L1_DEV dev, uint16_t *low)
This function returns the low threshold in mm.
Definition: vl53l1x_api.c:765
VL53L1X_Version_t::major
uint8_t major
Definition: vl53l1x_api.h:132
SYSTEM__INTERRUPT_CLEAR
#define SYSTEM__INTERRUPT_CLEAR
Definition: vl53l1x_api.h:110
VL53L1X_Result_t::NumSPADs
uint16_t NumSPADs
Definition: vl53l1x_api.h:144
VL53L1X_I2C_LEVEL
#define VL53L1X_I2C_LEVEL
Definition: vl53l1x_api.c:71
VL53L1X_GetSWVersion
VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion)
This function returns the SW driver version.
Definition: vl53l1x_api.c:218
VL53L1X_SetSignalThreshold
VL53L1X_ERROR VL53L1X_SetSignalThreshold(VL53L1_DEV dev, uint16_t Signal)
This function programs a new signal threshold in kcps (default=1024 kcps .
Definition: vl53l1x_api.c:833
VL53L1X_GetDistanceThresholdHigh
VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(VL53L1_DEV dev, uint16_t *high)
This function returns the high threshold in mm.
Definition: vl53l1x_api.c:775
GPIO_HV_MUX__CTRL
#define GPIO_HV_MUX__CTRL
Definition: vl53l1x_api.h:89
VL53L1X_GetInterruptPolarity
VL53L1X_ERROR VL53L1X_GetInterruptPolarity(VL53L1_DEV dev, uint8_t *pInterruptPolarity)
This function returns the current interrupt polarity 1=active high (default), 0=active low.
Definition: vl53l1x_api.c:297
VL53L1_RdWord
int8_t VL53L1_RdWord(VL53L1_DEV dev, uint16_t index, uint16_t *data)
VL53L1_RdWord() definition.
Definition: vl53l1_platform.c:94
VL53L1X_StartRanging
VL53L1X_ERROR VL53L1X_StartRanging(VL53L1_DEV dev)
This function starts the ranging distance operation The ranging operation is continuous.
Definition: vl53l1x_api.c:308
RANGE_CONFIG__VCSEL_PERIOD_B
#define RANGE_CONFIG__VCSEL_PERIOD_B
Definition: vl53l1x_api.h:95
VL53L1X_Result_t
defines packed reading results type
Definition: vl53l1x_api.h:139
SYSTEM__THRESH_HIGH
#define SYSTEM__THRESH_HIGH
Definition: vl53l1x_api.h:102
ROI_CONFIG__USER_ROI_CENTRE_SPAD
#define ROI_CONFIG__USER_ROI_CENTRE_SPAD
Definition: vl53l1x_api.h:106
ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE
#define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE
Definition: vl53l1x_api.h:107
VL53L1_DEFAULT_ADDRESS
#define VL53L1_DEFAULT_ADDRESS
Definition: vl53l1_platform.h:40
VL53L1X_GetSigmaThreshold
VL53L1X_ERROR VL53L1X_GetSigmaThreshold(VL53L1_DEV dev, uint16_t *sigma)
This function returns the current sigma threshold in mm.
Definition: vl53l1x_api.c:864
VL53L1X_Version_t::minor
uint8_t minor
Definition: vl53l1x_api.h:133
VL53L1X_SetInterMeasurementInMs
VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(VL53L1_DEV dev, uint32_t InterMeasMs)
This function programs the Intermeasurement period in ms Intermeasurement period must be >/= timing b...
Definition: vl53l1x_api.c:537
ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS
#define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS
Definition: vl53l1x_api.h:84
VL53L1X_GetROI_XY
VL53L1X_ERROR VL53L1X_GetROI_XY(VL53L1_DEV dev, uint16_t *ROI_X, uint16_t *ROI_Y)
This function returns width X and height Y.
Definition: vl53l1x_api.c:822
VL53L1X_SetROI
VL53L1X_ERROR VL53L1X_SetROI(VL53L1_DEV dev, uint16_t X, uint16_t Y)
This function programs the ROI (Region of Interest) The ROI position is centered, only the ROI size c...
Definition: vl53l1x_api.c:801
VL53L1X_IMPLEMENTATION_VER_MINOR
#define VL53L1X_IMPLEMENTATION_VER_MINOR
Definition: vl53l1x_api.h:74
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
VL53L1X_GetDistance
VL53L1X_ERROR VL53L1X_GetDistance(VL53L1_DEV dev, uint16_t *distance)
This function returns the distance measured by the sensor in mm.
Definition: vl53l1x_api.c:584
GPIO__TIO_HV_STATUS
#define GPIO__TIO_HV_STATUS
Definition: vl53l1x_api.h:90
VL53L1X_GetOffset
VL53L1X_ERROR VL53L1X_GetOffset(VL53L1_DEV dev, int16_t *offset)
This function returns the programmed offset correction value in mm.
Definition: vl53l1x_api.c:698
SD_CONFIG__INITIAL_PHASE_SD0
#define SD_CONFIG__INITIAL_PHASE_SD0
Definition: vl53l1x_api.h:105
VL53L1X_SetTimingBudgetInMs
VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(VL53L1_DEV dev, uint16_t TimingBudgetInMs)
This function programs the timing budget in ms.
Definition: vl53l1x_api.c:343
SD_CONFIG__WOI_SD0
#define SD_CONFIG__WOI_SD0
Definition: vl53l1x_api.h:104
int16_t
signed short int16_t
Definition: types.h:17
VL53L1_RESULT__RANGE_STATUS
#define VL53L1_RESULT__RANGE_STATUS
Definition: vl53l1x_api.h:112
uint8_t
unsigned char uint8_t
Definition: types.h:14
ALGO__PART_TO_PART_RANGE_OFFSET_MM
#define ALGO__PART_TO_PART_RANGE_OFFSET_MM
Definition: vl53l1x_api.h:86
VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0
#define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0
Definition: vl53l1x_api.h:113
VL53L1X_Result_t::SigPerSPAD
uint16_t SigPerSPAD
Definition: vl53l1x_api.h:143
VL53L1X_GetXtalk
VL53L1X_ERROR VL53L1X_GetXtalk(VL53L1_DEV dev, uint16_t *xtalk)
This function returns the current programmed xtalk correction value in cps.
Definition: vl53l1x_api.c:725
VL53L1X_GetInterMeasurementInMs
VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(VL53L1_DEV dev, uint16_t *pIM)
This function returns the Intermeasurement period in ms.
Definition: vl53l1x_api.c:550
VL53L1X_ERROR
int8_t VL53L1X_ERROR
Definition: vl53l1x_api.h:78
VL53L1X_SensorInit
VL53L1X_ERROR VL53L1X_SensorInit(VL53L1_DEV dev)
This function loads the 135 bytes default values to initialize the sensor.
Definition: vl53l1x_api.c:238
VL53L1X_BootState
VL53L1X_ERROR VL53L1X_BootState(VL53L1_DEV dev, uint8_t *state)
This function returns the boot state of the device (1:booted, 0:not booted)
Definition: vl53l1x_api.c:564
VL53L1X_GetAmbientPerSpad
VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(VL53L1_DEV dev, uint16_t *ambPerSp)
This function returns the ambient per SPAD in kcps/SPAD.
Definition: vl53l1x_api.c:608
MM_CONFIG__INNER_OFFSET_MM
#define MM_CONFIG__INNER_OFFSET_MM
Definition: vl53l1x_api.h:87
VL53L1_I2C_SLAVE__DEVICE_ADDRESS
#define VL53L1_I2C_SLAVE__DEVICE_ADDRESS
Definition: vl53l1x_api.h:81
offset
static const float offset[]
Definition: dw1000_arduino.c:199
ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS
#define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS
Definition: vl53l1x_api.h:83
VL53L1X_IMPLEMENTATION_VER_REVISION
#define VL53L1X_IMPLEMENTATION_VER_REVISION
Definition: vl53l1x_api.h:76
VL53L1_RESULT__OSC_CALIBRATE_VAL
#define VL53L1_RESULT__OSC_CALIBRATE_VAL
Definition: vl53l1x_api.h:117
RANGE_CONFIG__TIMEOUT_MACROP_A_HI
#define RANGE_CONFIG__TIMEOUT_MACROP_A_HI
Definition: vl53l1x_api.h:93
VL53L1X_IMPLEMENTATION_VER_MAJOR
#define VL53L1X_IMPLEMENTATION_VER_MAJOR
Definition: vl53l1x_api.h:73
RANGE_CONFIG__TIMEOUT_MACROP_B_HI
#define RANGE_CONFIG__TIMEOUT_MACROP_B_HI
Definition: vl53l1x_api.h:96
VL53L1X_GetSignalRate
VL53L1X_ERROR VL53L1X_GetSignalRate(VL53L1_DEV dev, uint16_t *signal)
This function returns the returned signal in kcps.
Definition: vl53l1x_api.c:619
VL53L1_WrDWord
int8_t VL53L1_WrDWord(VL53L1_DEV dev, uint16_t index, uint32_t data)
VL53L1_WrDWord() definition.
Definition: vl53l1_platform.c:78
VL53L1X_SetROICenter
VL53L1X_ERROR VL53L1X_SetROICenter(VL53L1_DEV dev, uint8_t ROICenter)
This function programs the new user ROI center, please to be aware that there is no check in this fun...
Definition: vl53l1x_api.c:785
VL53L1X_SetXtalk
VL53L1X_ERROR VL53L1X_SetXtalk(VL53L1_DEV dev, uint16_t XtalkValue)
This function programs the xtalk correction value in cps (Count Per Second).
Definition: vl53l1x_api.c:710
VL53L1X_Version_t
defines SW Version
Definition: vl53l1x_api.h:129
VL53L1_RdByte
int8_t VL53L1_RdByte(VL53L1_DEV dev, uint16_t index, uint8_t *data)
VL53L1_RdByte() definition.
Definition: vl53l1_platform.c:89
VL53L1X_GetSpadNb
VL53L1X_ERROR VL53L1X_GetSpadNb(VL53L1_DEV dev, uint16_t *spNb)
This function returns the current number of enabled SPADs.
Definition: vl53l1x_api.c:630
VL53L1_RdDWord
int8_t VL53L1_RdDWord(VL53L1_DEV dev, uint16_t index, uint32_t *data)
VL53L1_RdDWord() definition.
Definition: vl53l1_platform.c:102
VL53L1X_StartTemperatureUpdate
VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(VL53L1_DEV dev)
This function performs the temperature calibration.
Definition: vl53l1x_api.c:875
VL53L1X_Result_t::Status
uint8_t Status
Definition: vl53l1x_api.h:140
SYSTEM__MODE_START
#define SYSTEM__MODE_START
Definition: vl53l1x_api.h:111
SYSTEM__INTERRUPT_CONFIG_GPIO
#define SYSTEM__INTERRUPT_CONFIG_GPIO
Definition: vl53l1x_api.h:91
VL53L1X_IMPLEMENTATION_VER_SUB
#define VL53L1X_IMPLEMENTATION_VER_SUB
Definition: vl53l1x_api.h:75
VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0
#define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0
Definition: vl53l1x_api.h:116
VL53L1X_GetTimingBudgetInMs
VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(VL53L1_DEV dev, uint16_t *pTimingBudget)
This function returns the current timing budget in ms.
Definition: vl53l1x_api.c:445
RANGE_CONFIG__VCSEL_PERIOD_A
#define RANGE_CONFIG__VCSEL_PERIOD_A
Definition: vl53l1x_api.h:94
VL53L1X_SetOffset
VL53L1X_ERROR VL53L1X_SetOffset(VL53L1_DEV dev, int16_t OffsetValue)
This function programs the offset correction in mm.
Definition: vl53l1x_api.c:685
state
struct State state
Definition: state.c:36
VL53L1_WrWord
int8_t VL53L1_WrWord(VL53L1_DEV dev, uint16_t index, uint16_t data)
VL53L1_WrWord() definition.
Definition: vl53l1_platform.c:69
VL53L1X_GetSignalThreshold
VL53L1X_ERROR VL53L1X_GetSignalThreshold(VL53L1_DEV dev, uint16_t *signal)
This function returns the current signal threshold in kcps.
Definition: vl53l1x_api.c:841
VL53L1X_GetResult
VL53L1X_ERROR VL53L1X_GetResult(VL53L1_DEV dev, VL53L1X_Result_t *pResult)
This function returns measurements and the range status in a single read access.
Definition: vl53l1x_api.c:665
VL53L1X_GetDistanceMode
VL53L1X_ERROR VL53L1X_GetDistanceMode(VL53L1_DEV dev, uint16_t *DM)
This function returns the current distance mode (1=short, 2=long).
Definition: vl53l1x_api.c:523
VL53L1_Dev_t
Definition: vl53l1_platform.h:50
mesonh.mesonh_atmosphere.Y
int Y
Definition: mesonh_atmosphere.py:44
VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD
#define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD
Definition: vl53l1x_api.h:120
VL53L1X_Version_t::revision
uint32_t revision
Definition: vl53l1x_api.h:135
VL53L1X_Result_t::Distance
uint16_t Distance
Definition: vl53l1x_api.h:141
status_rtn
static const uint8_t status_rtn[24]
Definition: vl53l1x_api.c:208
VL53L1_IDENTIFICATION__MODEL_ID
#define VL53L1_IDENTIFICATION__MODEL_ID
Definition: vl53l1x_api.h:119
VL53L1X_SetI2CAddress
VL53L1X_ERROR VL53L1X_SetI2CAddress(VL53L1_DEV dev, uint8_t new_address)
This function sets the sensor I2C address used in case multiple devices application,...
Definition: vl53l1x_api.c:229
mesonh.mesonh_atmosphere.X
int X
Definition: mesonh_atmosphere.py:43
VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND
#define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND
Definition: vl53l1x_api.h:82
MM_CONFIG__OUTER_OFFSET_MM
#define MM_CONFIG__OUTER_OFFSET_MM
Definition: vl53l1x_api.h:88