ESPHome  2022.11.4
vl53l0x_sensor.cpp
Go to the documentation of this file.
1 #include "vl53l0x_sensor.h"
2 #include "esphome/core/log.h"
3 
4 /*
5  * Most of the code in this integration is based on the VL53L0x library
6  * by Pololu (Pololu Corporation), which in turn is based on the VL53L0X
7  * API from ST.
8  *
9  * For more information about licensing, please view the included LICENSE.txt file
10  * in the vl53l0x integration directory.
11  */
12 
13 namespace esphome {
14 namespace vl53l0x {
15 
16 static const char *const TAG = "vl53l0x";
17 
18 std::list<VL53L0XSensor *> VL53L0XSensor::vl53_sensors; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
19 bool VL53L0XSensor::enable_pin_setup_complete = false; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
20 
21 VL53L0XSensor::VL53L0XSensor() { VL53L0XSensor::vl53_sensors.push_back(this); }
22 
24  LOG_SENSOR("", "VL53L0X", this);
25  LOG_UPDATE_INTERVAL(this);
26  LOG_I2C_DEVICE(this);
27  if (this->enable_pin_ != nullptr) {
28  LOG_PIN(" Enable Pin: ", this->enable_pin_);
29  }
30  ESP_LOGCONFIG(TAG, " Timeout: %u%s", this->timeout_us_, this->timeout_us_ > 0 ? "us" : " (no timeout)");
31 }
32 
34  ESP_LOGD(TAG, "'%s' - setup BEGIN", this->name_.c_str());
35 
37  for (auto &vl53_sensor : vl53_sensors) {
38  if (vl53_sensor->enable_pin_ != nullptr) {
39  // Set enable pin as OUTPUT and disable the enable pin to force vl53 to HW Standby mode
40  vl53_sensor->enable_pin_->setup();
41  vl53_sensor->enable_pin_->digital_write(false);
42  }
43  }
45  }
46 
47  if (this->enable_pin_ != nullptr) {
48  // Enable the enable pin to cause FW boot (to get back to 0x29 default address)
49  this->enable_pin_->digital_write(true);
50  delayMicroseconds(100);
51  }
52 
53  // Save the i2c address we want and force it to use the default 0x29
54  // until we finish setup, then re-address to final desired address.
55  uint8_t final_address = address_;
56  this->set_i2c_address(0x29);
57 
58  reg(0x89) |= 0x01;
59  reg(0x88) = 0x00;
60 
61  reg(0x80) = 0x01;
62  reg(0xFF) = 0x01;
63  reg(0x00) = 0x00;
64  this->stop_variable_ = reg(0x91).get();
65 
66  reg(0x00) = 0x01;
67  reg(0xFF) = 0x00;
68  reg(0x80) = 0x00;
69  reg(0x60) |= 0x12;
70  if (this->long_range_)
71  this->signal_rate_limit_ = 0.1;
72  auto rate_value = static_cast<uint16_t>(signal_rate_limit_ * 128);
73  write_byte_16(0x44, rate_value);
74 
75  reg(0x01) = 0xFF;
76 
77  // getSpadInfo()
78  reg(0x80) = 0x01;
79  reg(0xFF) = 0x01;
80  reg(0x00) = 0x00;
81  reg(0xFF) = 0x06;
82  reg(0x83) |= 0x04;
83  reg(0xFF) = 0x07;
84  reg(0x81) = 0x01;
85  reg(0x80) = 0x01;
86  reg(0x94) = 0x6B;
87  reg(0x83) = 0x00;
88 
89  this->timeout_start_us_ = micros();
90  while (reg(0x83).get() == 0x00) {
91  if (this->timeout_us_ > 0 && ((uint16_t)(micros() - this->timeout_start_us_) > this->timeout_us_)) {
92  ESP_LOGE(TAG, "'%s' - setup timeout", this->name_.c_str());
93  this->mark_failed();
94  return;
95  }
96  yield();
97  }
98 
99  reg(0x83) = 0x01;
100  uint8_t tmp = reg(0x92).get();
101  uint8_t spad_count = tmp & 0x7F;
102  bool spad_type_is_aperture = tmp & 0x80;
103 
104  reg(0x81) = 0x00;
105  reg(0xFF) = 0x06;
106  reg(0x83) &= ~0x04;
107  reg(0xFF) = 0x01;
108  reg(0x00) = 0x01;
109  reg(0xFF) = 0x00;
110  reg(0x80) = 0x00;
111 
112  uint8_t ref_spad_map[6] = {};
113  this->read_bytes(0xB0, ref_spad_map, 6);
114 
115  reg(0xFF) = 0x01;
116  reg(0x4F) = 0x00;
117  reg(0x4E) = 0x2C;
118  reg(0xFF) = 0x00;
119  reg(0xB6) = 0xB4;
120 
121  uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
122  uint8_t spads_enabled = 0;
123  for (int i = 0; i < 48; i++) {
124  uint8_t &val = ref_spad_map[i / 8];
125  uint8_t mask = 1 << (i % 8);
126 
127  if (i < first_spad_to_enable || spads_enabled == spad_count) {
128  val &= ~mask;
129  } else if (val & mask) {
130  spads_enabled += 1;
131  }
132  }
133 
134  this->write_bytes(0xB0, ref_spad_map, 6);
135 
136  reg(0xFF) = 0x01;
137  reg(0x00) = 0x00;
138  reg(0xFF) = 0x00;
139  reg(0x09) = 0x00;
140  reg(0x10) = 0x00;
141  reg(0x11) = 0x00;
142  reg(0x24) = 0x01;
143  reg(0x25) = 0xFF;
144  reg(0x75) = 0x00;
145  reg(0xFF) = 0x01;
146  reg(0x4E) = 0x2C;
147  reg(0x48) = 0x00;
148  reg(0x30) = 0x20;
149  reg(0xFF) = 0x00;
150  if (this->long_range_) {
151  reg(0x30) = 0x07; // WAS 0x09
152  } else {
153  reg(0x30) = 0x09;
154  }
155  reg(0x54) = 0x00;
156  reg(0x31) = 0x04;
157  reg(0x32) = 0x03;
158  reg(0x40) = 0x83;
159  reg(0x46) = 0x25;
160  reg(0x60) = 0x00;
161  reg(0x27) = 0x00;
162  reg(0x50) = 0x06;
163  reg(0x51) = 0x00;
164  reg(0x52) = 0x96;
165  reg(0x56) = 0x08;
166  if (this->long_range_) {
167  reg(0x57) = 0x50; // was 0x30
168  } else {
169  reg(0x57) = 0x30;
170  }
171  reg(0x61) = 0x00;
172  reg(0x62) = 0x00;
173  reg(0x64) = 0x00;
174  reg(0x65) = 0x00;
175  reg(0x66) = 0xA0;
176  reg(0xFF) = 0x01;
177  reg(0x22) = 0x32;
178  reg(0x47) = 0x14;
179  reg(0x49) = 0xFF;
180  reg(0x4A) = 0x00;
181  reg(0xFF) = 0x00;
182  reg(0x7A) = 0x0A;
183  reg(0x7B) = 0x00;
184  reg(0x78) = 0x21;
185  reg(0xFF) = 0x01;
186  reg(0x23) = 0x34;
187  reg(0x42) = 0x00;
188  reg(0x44) = 0xFF;
189  reg(0x45) = 0x26;
190  reg(0x46) = 0x05;
191  reg(0x40) = 0x40;
192  reg(0x0E) = 0x06;
193  reg(0x20) = 0x1A;
194  reg(0x43) = 0x40;
195  reg(0xFF) = 0x00;
196  reg(0x34) = 0x03;
197  reg(0x35) = 0x44;
198  reg(0xFF) = 0x01;
199  reg(0x31) = 0x04;
200  reg(0x4B) = 0x09;
201  reg(0x4C) = 0x05;
202  reg(0x4D) = 0x04;
203  reg(0xFF) = 0x00;
204  reg(0x44) = 0x00;
205  reg(0x45) = 0x20;
206  reg(0x47) = 0x08;
207  if (this->long_range_) {
208  reg(0x48) = 0x48; // was 0x28
209  } else {
210  reg(0x48) = 0x28;
211  }
212  reg(0x67) = 0x00;
213  reg(0x70) = 0x04;
214  reg(0x71) = 0x01;
215  reg(0x72) = 0xFE;
216  reg(0x76) = 0x00;
217  reg(0x77) = 0x00;
218  reg(0xFF) = 0x01;
219  reg(0x0D) = 0x01;
220  reg(0xFF) = 0x00;
221  reg(0x80) = 0x01;
222  reg(0x01) = 0xF8;
223  reg(0xFF) = 0x01;
224  reg(0x8E) = 0x01;
225  reg(0x00) = 0x01;
226  reg(0xFF) = 0x00;
227  reg(0x80) = 0x00;
228 
229  reg(0x0A) = 0x04;
230  reg(0x84) &= ~0x10;
231  reg(0x0B) = 0x01;
232 
234  reg(0x01) = 0xE8;
236  reg(0x01) = 0x01;
237 
238  if (!perform_single_ref_calibration_(0x40)) {
239  ESP_LOGW(TAG, "1st reference calibration failed!");
240  this->mark_failed();
241  return;
242  }
243  reg(0x01) = 0x02;
244  if (!perform_single_ref_calibration_(0x00)) {
245  ESP_LOGW(TAG, "2nd reference calibration failed!");
246  this->mark_failed();
247  return;
248  }
249  reg(0x01) = 0xE8;
250 
251  // Set the sensor to the desired final address
252  // The following is different for VL53L0X vs VL53L1X
253  // I2C_SXXXX_DEVICE_ADDRESS = 0x8A for VL53L0X
254  // I2C_SXXXX__DEVICE_ADDRESS = 0x0001 for VL53L1X
255  reg(0x8A) = final_address & 0x7F;
256  this->set_i2c_address(final_address);
257 
258  ESP_LOGD(TAG, "'%s' - setup END", this->name_.c_str());
259 }
260 
262  if (this->initiated_read_ || this->waiting_for_interrupt_) {
263  this->publish_state(NAN);
264  this->status_momentary_warning("update", 5000);
265  ESP_LOGW(TAG, "%s - update called before prior reading complete - initiated:%d waiting_for_interrupt:%d",
266  this->name_.c_str(), this->initiated_read_, this->waiting_for_interrupt_);
267  }
268 
269  // initiate single shot measurement
270  reg(0x80) = 0x01;
271  reg(0xFF) = 0x01;
272 
273  reg(0x00) = 0x00;
274  reg(0x91) = this->stop_variable_;
275  reg(0x00) = 0x01;
276  reg(0xFF) = 0x00;
277  reg(0x80) = 0x00;
278 
279  reg(0x00) = 0x01;
280  this->waiting_for_interrupt_ = false;
281  this->initiated_read_ = true;
282  // wait for timeout
283 }
284 
286  if (this->initiated_read_) {
287  if (reg(0x00).get() & 0x01) {
288  // waiting
289  } else {
290  // done
291  // wait until reg(0x13) & 0x07 is set
292  this->initiated_read_ = false;
293  this->waiting_for_interrupt_ = true;
294  }
295  }
296  if (this->waiting_for_interrupt_) {
297  if (reg(0x13).get() & 0x07) {
298  uint16_t range_mm = 0;
299  this->read_byte_16(0x14 + 10, &range_mm);
300  reg(0x0B) = 0x01;
301  this->waiting_for_interrupt_ = false;
302 
303  if (range_mm >= 8190) {
304  ESP_LOGD(TAG, "'%s' - Distance is out of range, please move the target closer", this->name_.c_str());
305  this->publish_state(NAN);
306  return;
307  }
308 
309  float range_m = range_mm / 1e3f;
310  ESP_LOGD(TAG, "'%s' - Got distance %.3f m", this->name_.c_str(), range_m);
311  this->publish_state(range_m);
312  }
313  }
314 }
315 
317  SequenceStepEnables enables{};
318  SequenceStepTimeouts timeouts{};
319 
320  uint16_t start_overhead = 1910;
321  uint16_t end_overhead = 960;
322  uint16_t msrc_overhead = 660;
323  uint16_t tcc_overhead = 590;
324  uint16_t dss_overhead = 690;
325  uint16_t pre_range_overhead = 660;
326  uint16_t final_range_overhead = 550;
327 
328  // "Start and end overhead times always present"
329  uint32_t budget_us = start_overhead + end_overhead;
330 
331  get_sequence_step_enables_(&enables);
332  get_sequence_step_timeouts_(&enables, &timeouts);
333 
334  if (enables.tcc)
335  budget_us += (timeouts.msrc_dss_tcc_us + tcc_overhead);
336 
337  if (enables.dss) {
338  budget_us += 2 * (timeouts.msrc_dss_tcc_us + dss_overhead);
339  } else if (enables.msrc) {
340  budget_us += (timeouts.msrc_dss_tcc_us + msrc_overhead);
341  }
342 
343  if (enables.pre_range)
344  budget_us += (timeouts.pre_range_us + pre_range_overhead);
345 
346  if (enables.final_range)
347  budget_us += (timeouts.final_range_us + final_range_overhead);
348 
349  measurement_timing_budget_us_ = budget_us; // store for internal reuse
350  return budget_us;
351 }
352 
354  SequenceStepEnables enables{};
355  SequenceStepTimeouts timeouts{};
356 
357  uint16_t start_overhead = 1320; // note that this is different than the value in get_
358  uint16_t end_overhead = 960;
359  uint16_t msrc_overhead = 660;
360  uint16_t tcc_overhead = 590;
361  uint16_t dss_overhead = 690;
362  uint16_t pre_range_overhead = 660;
363  uint16_t final_range_overhead = 550;
364 
365  uint32_t min_timing_budget = 20000;
366 
367  if (budget_us < min_timing_budget) {
368  return false;
369  }
370 
371  uint32_t used_budget_us = start_overhead + end_overhead;
372 
373  get_sequence_step_enables_(&enables);
374  get_sequence_step_timeouts_(&enables, &timeouts);
375 
376  if (enables.tcc) {
377  used_budget_us += (timeouts.msrc_dss_tcc_us + tcc_overhead);
378  }
379 
380  if (enables.dss) {
381  used_budget_us += 2 * (timeouts.msrc_dss_tcc_us + dss_overhead);
382  } else if (enables.msrc) {
383  used_budget_us += (timeouts.msrc_dss_tcc_us + msrc_overhead);
384  }
385 
386  if (enables.pre_range) {
387  used_budget_us += (timeouts.pre_range_us + pre_range_overhead);
388  }
389 
390  if (enables.final_range) {
391  used_budget_us += final_range_overhead;
392 
393  // "Note that the final range timeout is determined by the timing
394  // budget and the sum of all other timeouts within the sequence.
395  // If there is no room for the final range timeout, then an error
396  // will be set. Otherwise the remaining time will be applied to
397  // the final range."
398 
399  if (used_budget_us > budget_us) {
400  // "Requested timeout too big."
401  return false;
402  }
403 
404  uint32_t final_range_timeout_us = budget_us - used_budget_us;
405 
406  // set_sequence_step_timeout() begin
407  // (SequenceStepId == VL53L0X_SEQUENCESTEP_FINAL_RANGE)
408 
409  // "For the final range timeout, the pre-range timeout
410  // must be added. To do this both final and pre-range
411  // timeouts must be expressed in macro periods MClks
412  // because they have different vcsel periods."
413 
414  uint16_t final_range_timeout_mclks =
415  timeout_microseconds_to_mclks_(final_range_timeout_us, timeouts.final_range_vcsel_period_pclks);
416 
417  if (enables.pre_range) {
418  final_range_timeout_mclks += timeouts.pre_range_mclks;
419  }
420 
421  write_byte_16(0x71, encode_timeout_(final_range_timeout_mclks));
422 
423  // set_sequence_step_timeout() end
424 
425  measurement_timing_budget_us_ = budget_us; // store for internal reuse
426  }
427  return true;
428 }
429 
431  uint8_t sequence_config = reg(0x01).get();
432  enables->tcc = (sequence_config >> 4) & 0x1;
433  enables->dss = (sequence_config >> 3) & 0x1;
434  enables->msrc = (sequence_config >> 2) & 0x1;
435  enables->pre_range = (sequence_config >> 6) & 0x1;
436  enables->final_range = (sequence_config >> 7) & 0x1;
437 }
438 
441 
442  timeouts->msrc_dss_tcc_mclks = reg(0x46).get() + 1;
443  timeouts->msrc_dss_tcc_us =
445 
446  uint16_t value;
447  read_byte_16(0x51, &value);
448  timeouts->pre_range_mclks = decode_timeout_(value);
449  timeouts->pre_range_us =
451 
453 
454  read_byte_16(0x71, &value);
455  timeouts->final_range_mclks = decode_timeout_(value);
456 
457  if (enables->pre_range) {
458  timeouts->final_range_mclks -= timeouts->pre_range_mclks;
459  }
460 
461  timeouts->final_range_us =
463 }
464 
466  uint8_t vcsel;
467  if (type == VCSEL_PERIOD_PRE_RANGE) {
468  vcsel = reg(0x50).get();
469  } else if (type == VCSEL_PERIOD_FINAL_RANGE) {
470  vcsel = reg(0x70).get();
471  } else {
472  return 255;
473  }
474 
475  return (vcsel + 1) << 1;
476 }
477 
478 uint32_t VL53L0XSensor::get_macro_period_(uint8_t vcsel_period_pclks) {
479  return ((2304UL * vcsel_period_pclks * 1655UL) + 500UL) / 1000UL;
480 }
481 
482 uint32_t VL53L0XSensor::timeout_mclks_to_microseconds_(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks) {
483  uint32_t macro_period_ns = get_macro_period_(vcsel_period_pclks);
484  return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000;
485 }
486 
487 uint32_t VL53L0XSensor::timeout_microseconds_to_mclks_(uint32_t timeout_period_us, uint8_t vcsel_period_pclks) {
488  uint32_t macro_period_ns = get_macro_period_(vcsel_period_pclks);
489  return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
490 }
491 
492 uint16_t VL53L0XSensor::decode_timeout_(uint16_t reg_val) {
493  // format: "(LSByte * 2^MSByte) + 1"
494  uint8_t msb = (reg_val >> 8) & 0xFF;
495  uint8_t lsb = (reg_val >> 0) & 0xFF;
496  return (uint16_t(lsb) << msb) + 1;
497 }
498 
499 uint16_t VL53L0XSensor::encode_timeout_(uint16_t timeout_mclks) {
500  // format: "(LSByte * 2^MSByte) + 1"
501  uint32_t ls_byte = 0;
502  uint16_t ms_byte = 0;
503 
504  if (timeout_mclks <= 0)
505  return 0;
506 
507  ls_byte = timeout_mclks - 1;
508 
509  while ((ls_byte & 0xFFFFFF00) > 0) {
510  ls_byte >>= 1;
511  ms_byte++;
512  }
513 
514  return (ms_byte << 8) | (ls_byte & 0xFF);
515 }
516 
518  reg(0x00) = 0x01 | vhv_init_byte; // VL53L0X_REG_SYSRANGE_MODE_START_STOP
519 
520  uint32_t start = millis();
521  while ((reg(0x13).get() & 0x07) == 0) {
522  if (millis() - start > 1000)
523  return false;
524  yield();
525  }
526 
527  reg(0x0B) = 0x01;
528  reg(0x00) = 0x00;
529 
530  return true;
531 }
532 
533 } // namespace vl53l0x
534 } // namespace esphome
virtual void digital_write(bool value)=0
bool read_byte_16(uint8_t a_register, uint16_t *data)
Definition: i2c.h:107
uint32_t get_macro_period_(uint8_t vcsel_period_pclks)
I2CRegister reg(uint8_t a_register)
Definition: i2c.h:46
uint8_t get() const
Definition: i2c.cpp:33
uint32_t timeout_mclks_to_microseconds_(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
std::string name_
Definition: entity_base.h:54
void status_momentary_warning(const std::string &name, uint32_t length=5000)
Definition: component.cpp:151
bool read_bytes(uint8_t a_register, uint8_t *data, uint8_t len)
Definition: i2c.h:68
bool set_measurement_timing_budget_(uint32_t budget_us)
uint16_t decode_timeout_(uint16_t reg_val)
uint32_t IRAM_ATTR HOT micros()
Definition: core.cpp:28
uint32_t IRAM_ATTR HOT millis()
Definition: core.cpp:26
void publish_state(float state)
Publish a new state to the front-end.
Definition: sensor.cpp:72
uint8_t type
uint32_t timeout_microseconds_to_mclks_(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
void get_sequence_step_enables_(SequenceStepEnables *enables)
static std::list< VL53L0XSensor * > vl53_sensors
bool perform_single_ref_calibration_(uint8_t vhv_init_byte)
uint8_t address_
Definition: i2c.h:130
void IRAM_ATTR HOT yield()
Definition: core.cpp:25
virtual void mark_failed()
Mark this component as failed.
Definition: component.cpp:112
Definition: a4988.cpp:4
void IRAM_ATTR HOT delayMicroseconds(uint32_t us)
Definition: core.cpp:29
uint32_t val
Definition: datatypes.h:85
uint8_t get_vcsel_pulse_period_(VcselPeriodType type)
bool write_byte_16(uint8_t a_register, uint16_t data)
Definition: i2c.h:127
void set_i2c_address(uint8_t address)
Definition: i2c.h:43
uint16_t encode_timeout_(uint16_t timeout_mclks)
void get_sequence_step_timeouts_(SequenceStepEnables const *enables, SequenceStepTimeouts *timeouts)
bool write_bytes(uint8_t a_register, const uint8_t *data, uint8_t len, bool stop=true)
Definition: i2c.h:109