PX4 Firmware
PX4 Autopilot Software http://px4.io
vl53lxx.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file vl53lxx.cpp
36  * @author Daniele Pettenuzzo
37  *
38  * Driver for the vl53lxx ToF Sensor from ST Microelectronics connected via I2C.
39  */
40 
41 #include <stdlib.h>
42 #include <string.h>
43 
44 #include <containers/Array.hpp>
45 #include <drivers/device/i2c.h>
47 #include <drivers/drv_hrt.h>
49 #include <perf/perf_counter.h>
50 #include <px4_platform_common/getopt.h>
51 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
53 
54 /* Configuration Constants */
55 #define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
56 
57 #define VL53LXX_BASEADDR 0x29
58 #define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
59 
60 /* VL53LXX Registers addresses */
61 #define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89
62 #define MSRC_CONFIG_CONTROL_REG 0x60
63 #define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44
64 #define SYSTEM_SEQUENCE_CONFIG_REG 0x01
65 #define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F
66 #define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E
67 #define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6
68 #define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0
69 #define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A
70 #define SYSTEM_SEQUENCE_CONFIG_REG 0x01
71 #define SYSRANGE_START_REG 0x00
72 #define RESULT_INTERRUPT_STATUS_REG 0x13
73 #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
74 #define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0
75 #define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84
76 #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
77 #define RESULT_RANGE_STATUS_REG 0x14
78 #define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0
79 #define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA
80 
81 #define VL53LXX_US 1000 // 1ms
82 #define VL53LXX_SAMPLE_RATE 50000 // 50ms
83 
84 #define VL53LXX_MAX_RANGING_DISTANCE 2.0f
85 #define VL53LXX_MIN_RANGING_DISTANCE 0.0f
86 
87 #define VL53LXX_BUS_CLOCK 400000 // 400kHz bus clock speed
88 
89 class VL53LXX : public device::I2C, public px4::ScheduledWorkItem
90 {
91 public:
92  VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
93  int bus = VL53LXX_BUS_DEFAULT, int address = VL53LXX_BASEADDR);
94 
95  virtual ~VL53LXX();
96 
97  virtual int init() override;
98 
99  /**
100  * Diagnostics - print some basic information about the driver.
101  */
102  void print_info();
103 
104  virtual ssize_t read(device::file_t *file_pointer, char *buffer, size_t buflen);
105 
106  /**
107  * Initialise the automatic measurement state machine and start it.
108  */
109  void start();
110 
111  /**
112  * Stop the automatic measurement state machine.
113  */
114  void stop();
115 
116 protected:
117  virtual int probe() override;
118 
119 private:
120 
121  /**
122  * Collects the most recent sensor measurement data from the i2c bus.
123  */
124  int collect();
125 
126  /**
127  * Sends an i2c measure command to the sensor.
128  */
129  int measure();
130 
131  /**
132  * Perform a poll cycle; collect from the previous measurement
133  * and start a new one.
134  */
135  void Run() override;
136 
137  int readRegister(const uint8_t reg_address, uint8_t &value);
138  int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length);
139 
140  int writeRegister(const uint8_t reg_address, const uint8_t value);
141  int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length);
142 
143  int sensorInit();
144  int sensorTuning();
145  int singleRefCalibration(const uint8_t byte);
146  int spadCalculations();
147 
148  bool _collect_phase{false};
149  bool _measurement_started{false};
150  bool _new_measurement{true};
151 
155 
156  uint8_t _rotation{0};
157  uint8_t _stop_variable{0};
158 
160 
163 
164  ringbuffer::RingBuffer *_reports{nullptr};
165 };
166 
167 
168 VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
169  I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000),
170  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
171  _rotation(rotation)
172 {
173  // Allow 3 retries as the device typically misses the first measure attempts.
174  I2C::_retries = 3;
175 }
176 
178 {
179  // Ensure we are truly inactive.
180  stop();
181 
182  // Free any existing reports.
183  if (_reports != nullptr) {
184  delete _reports;
185  }
186 
187  if (_class_instance != -1) {
188  unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
189  }
190 
191  // Unadvertise uORB topics.
192  if (_distance_sensor_topic != nullptr) {
194  }
195 
196  // free perf counters
199 }
200 
201 int
203 {
204  // Read from the sensor.
205  uint8_t val[2] = {};
207 
208  _collect_phase = false;
209 
210  int ret = transfer(nullptr, 0, &val[0], 2);
211 
212  if (ret != PX4_OK) {
213  DEVICE_LOG("error reading from sensor: %d", ret);
216  return ret;
217  }
218 
219  uint16_t distance_mm = (val[0] << 8) | val[1];
220  float distance_m = distance_mm / 1000.f;
221 
222  struct distance_sensor_s report;
223  report.timestamp = hrt_absolute_time();
224  report.current_distance = distance_m;
225  report.id = 0; // TODO: set propoer ID
228  report.orientation = _rotation;
229  report.signal_quality = -1;
230  report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
231  report.variance = 0.0f;
232 
233  // Publish the report data if we have a valid topic.
235  orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report,
237  }
238 
239  _reports->force(&report);
240 
241  // Notify anyone waiting for data.
242  poll_notify(POLLIN);
244 
245  return PX4_OK;
246 }
247 
248 int
250 {
251  set_device_address(VL53LXX_BASEADDR);
252 
253  // Initialize I2C (and probe) first.
254  if (I2C::init() != OK) {
255  return PX4_ERROR;
256  }
257 
258  /* allocate basic report buffers */
259  _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
260 
261  if (_reports == nullptr) {
262  return PX4_ERROR;
263  }
264 
265  _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
266 
267  // Get a publish handle on the obstacle distance topic.
268  distance_sensor_s report {};
269  _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &report,
271 
272  if (_distance_sensor_topic == nullptr) {
273  PX4_ERR("failed to create distance_sensor object");
274  return PX4_ERROR;
275  }
276 
277  return PX4_OK;
278 }
279 
280 int
282 {
283  uint8_t wait_for_measurement = 0;
284  uint8_t system_start = 0;
285 
286  // Send the command to begin a measurement.
287  const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10;
288 
289  if (_new_measurement) {
290 
291  _new_measurement = false;
292 
293  writeRegister(0x80, 0x01);
294  writeRegister(0xFF, 0x01);
295  writeRegister(0x00, 0x00);
297  writeRegister(0x00, 0x01);
298  writeRegister(0xFF, 0x00);
299  writeRegister(0x80, 0x00);
300 
302 
303  readRegister(SYSRANGE_START_REG, system_start);
304 
305  if ((system_start & 0x01) == 1) {
306  ScheduleDelayed(VL53LXX_US);
307  return PX4_OK;
308 
309  } else {
310  _measurement_started = true;
311  }
312 
313  }
314 
316 
317  readRegister(SYSRANGE_START_REG, system_start);
318 
319  if ((system_start & 0x01) == 1) {
320  ScheduleDelayed(VL53LXX_US);
321  return PX4_OK;
322 
323  } else {
324  _measurement_started = true;
325  }
326  }
327 
328  readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
329 
330  if ((wait_for_measurement & 0x07) == 0) {
331  ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready
332  return PX4_OK;
333  }
334 
335  _collect_phase = true;
336 
337  int ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
338 
339  if (ret != PX4_OK) {
341  DEVICE_LOG("i2c::transfer returned %d", ret);
342  return ret;
343  }
344 
345  return PX4_OK;
346 }
347 
348 void
350 {
353  PX4_INFO("measure interval: %u msec", _measure_interval / 1000);
354  _reports->print_info("report queue");
355 }
356 
357 int
359 {
360  if (sensorInit() == PX4_OK) {
361  return PX4_OK;
362  }
363 
364  // Device not found on any address.
365  return -EIO;
366 }
367 
368 ssize_t
369 VL53LXX::read(device::file_t *file_pointer, char *buffer, size_t buffer_length)
370 {
371  size_t buffer_size = 0;
372  unsigned int count = buffer_length / sizeof(struct distance_sensor_s);
373  struct distance_sensor_s *read_buffer = reinterpret_cast<distance_sensor_s *>(buffer);
374 
375  // Buffer must be large enough.
376  if (count < 1) {
377  return -ENOSPC;
378  }
379 
380  // If automatic measurement is enabled.
381  if (_measure_interval > 0) {
382 
383  /*
384  * While there is space in the caller's buffer, and reports, copy them.
385  * Note that we may be pre-empted by the workq thread while we are doing this;
386  * we are careful to avoid racing with them.
387  */
388  while (count--) {
389  if (_reports->get(read_buffer)) {
390  buffer_size += sizeof(*read_buffer);
391  read_buffer++;
392  }
393  }
394 
395  // If there was no data, warn the caller.
396  if (buffer_size == 0) {
397  return -EAGAIN;
398  }
399  }
400 
401  // Flush the report ring buffer.
402  _reports->flush();
403 
404  // Trigger a measurement.
405  if (measure() != PX4_OK) {
406  return -EIO;
407  }
408 
409  // Wait for the measurement to complete.
410  usleep(_measure_interval);
411 
412  // Run the collection phase.
413  if (collect() != PX4_OK) {
414  return -EIO;
415  }
416 
417  // State machine will have generated a report, copy it out.
418  if (_reports->get(read_buffer)) {
419  buffer_size = sizeof(*read_buffer);
420  }
421 
422  return buffer_size;
423 }
424 
425 int
426 VL53LXX::readRegister(const uint8_t reg_address, uint8_t &value)
427 {
428  // Write register address to the sensor.
429  int ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0);
430 
431  if (ret != PX4_OK) {
433  return ret;
434  }
435 
436  // Read from the sensor.
437  ret = transfer(nullptr, 0, &value, 1);
438 
439  if (ret != PX4_OK) {
441  return ret;
442  }
443 
444  return PX4_OK;
445 }
446 
447 
448 int
449 VL53LXX::readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length)
450 {
451  // Write register address to the sensor.
452  int ret = transfer(&reg_address, 1, nullptr, 0);
453 
454  if (ret != PX4_OK) {
456  return ret;
457  }
458 
459  // Read from the sensor.
460  ret = transfer(nullptr, 0, &value[0], length);
461 
462  if (ret != PX4_OK) {
464  return ret;
465  }
466 
467  return PX4_OK;
468 }
469 
470 void
472 {
473  measure();
474 
475  if (_collect_phase) {
476 
477  _collect_phase = false;
478  _new_measurement = true;
479 
480  collect();
481 
482  ScheduleDelayed(_measure_interval);
483  }
484 }
485 
486 int
488 {
489  uint8_t val = 0;
490  uint8_t spad_count = 0;
491  uint8_t ref_spad_map[6] = {};
492 
493  bool spad_type_is_aperture = false;
494 
495  writeRegister(0x80, 0x01);
496  writeRegister(0xFF, 0x01);
497  writeRegister(0x00, 0x00);
498  writeRegister(0xFF, 0x06);
499 
500  readRegister(0x83, val);
501  writeRegister(0x83, val | 0x04);
502 
503  writeRegister(0xFF, 0x07);
504  writeRegister(0x81, 0x01);
505  writeRegister(0x80, 0x01);
506  writeRegister(0x94, 0x6b);
507  writeRegister(0x83, 0x00);
508 
509  readRegister(0x83, val);
510 
511  while (val == 0x00) {
512  readRegister(0x83, val);
513  }
514 
515  writeRegister(0x83, 0x01);
516  readRegister(0x92, val);
517 
518  spad_count = val & 0x7f;
519  spad_type_is_aperture = (val >> 7) & 0x01;
520 
521  writeRegister(0x81, 0x00);
522  writeRegister(0xFF, 0x06);
523 
524  readRegister(0x83, val);
525  writeRegister(0x83, val & ~0x04);
526 
527  writeRegister(0xFF, 0x01);
528  writeRegister(0x00, 0x01);
529  writeRegister(0xFF, 0x00);
530  writeRegister(0x80, 0x00);
531 
533 
534  writeRegister(0xFF, 0x01);
537  writeRegister(0xFF, 0x00);
539 
540  uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
541  uint8_t spads_enabled = 0;
542 
543  for (uint8_t i = 0; i < 48; i++) {
544  if (i < first_spad_to_enable || spads_enabled == spad_count) {
545  ref_spad_map[i / 8] &= ~(1 << (i % 8));
546 
547  } else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) {
548  spads_enabled++;
549  }
550  }
551 
553 
554  sensorTuning();
555 
556  writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data.
558 
559  writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // Active low.
563 
564  singleRefCalibration(0x40);
565 
567 
568  singleRefCalibration(0x00);
569 
570  writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // Restore config.
571 
572  return OK;
573 }
574 
575 int
577 {
578  uint8_t val = 0;
579 
580  // I2C at 2.8V on sensor side of level shifter
581  int ret = PX4_OK;
583 
584  if (ret != PX4_OK) {
585  return PX4_ERROR;
586  }
587 
589 
590  // set I2C to standard mode
591  ret |= writeRegister(0x88, 0x00);
592  ret |= writeRegister(0x80, 0x01);
593  ret |= writeRegister(0xFF, 0x01);
594  ret |= writeRegister(0x00, 0x00);
595  ret |= readRegister(0x91, val);
596  ret |= writeRegister(0x00, 0x01);
597  ret |= writeRegister(0xFF, 0x00);
598  ret |= writeRegister(0x80, 0x00);
599 
600  if (ret != PX4_OK) {
601  return PX4_ERROR;
602  }
603 
604  _stop_variable = val;
605 
606  // Disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
609 
610  // Set signal rate limit to 0.1
611  float rate_limit = 0.1 * 65536;
612  uint8_t rate_limit_split[2] = {};
613 
614  rate_limit_split[0] = (((uint16_t)rate_limit) >> 8);
615  rate_limit_split[1] = (uint16_t)rate_limit;
616 
619 
621 
622  return PX4_OK;
623 }
624 
625 int
627 {
628  // Magic register settings taken from the ST Micro API.
629  writeRegister(0xFF, 0x01);
630  writeRegister(0x00, 0x00);
631  writeRegister(0xFF, 0x00);
632  writeRegister(0x09, 0x00);
633  writeRegister(0x10, 0x00);
634  writeRegister(0x11, 0x00);
635  writeRegister(0x24, 0x01);
636  writeRegister(0x25, 0xFF);
637  writeRegister(0x75, 0x00);
638  writeRegister(0xFF, 0x01);
639  writeRegister(0x4E, 0x2C);
640  writeRegister(0x48, 0x00);
641  writeRegister(0x30, 0x20);
642  writeRegister(0xFF, 0x00);
643  writeRegister(0x30, 0x09);
644  writeRegister(0x54, 0x00);
645  writeRegister(0x31, 0x04);
646  writeRegister(0x32, 0x03);
647  writeRegister(0x40, 0x83);
648  writeRegister(0x46, 0x25);
649  writeRegister(0x60, 0x00);
650  writeRegister(0x27, 0x00);
651  writeRegister(0x50, 0x06);
652  writeRegister(0x51, 0x00);
653  writeRegister(0x52, 0x96);
654  writeRegister(0x56, 0x08);
655  writeRegister(0x57, 0x30);
656  writeRegister(0x61, 0x00);
657  writeRegister(0x62, 0x00);
658  writeRegister(0x64, 0x00);
659  writeRegister(0x65, 0x00);
660  writeRegister(0x66, 0xA0);
661  writeRegister(0xFF, 0x01);
662  writeRegister(0x22, 0x32);
663  writeRegister(0x47, 0x14);
664  writeRegister(0x49, 0xFF);
665  writeRegister(0x4A, 0x00);
666  writeRegister(0xFF, 0x00);
667  writeRegister(0x7A, 0x0A);
668  writeRegister(0x7B, 0x00);
669  writeRegister(0x78, 0x21);
670  writeRegister(0xFF, 0x01);
671  writeRegister(0x23, 0x34);
672  writeRegister(0x42, 0x00);
673  writeRegister(0x44, 0xFF);
674  writeRegister(0x45, 0x26);
675  writeRegister(0x46, 0x05);
676  writeRegister(0x40, 0x40);
677  writeRegister(0x0E, 0x06);
678  writeRegister(0x20, 0x1A);
679  writeRegister(0x43, 0x40);
680  writeRegister(0xFF, 0x00);
681  writeRegister(0x34, 0x03);
682  writeRegister(0x35, 0x44);
683  writeRegister(0xFF, 0x01);
684  writeRegister(0x31, 0x04);
685  writeRegister(0x4B, 0x09);
686  writeRegister(0x4C, 0x05);
687  writeRegister(0x4D, 0x04);
688  writeRegister(0xFF, 0x00);
689  writeRegister(0x44, 0x00);
690  writeRegister(0x45, 0x20);
691  writeRegister(0x47, 0x08);
692  writeRegister(0x48, 0x28);
693  writeRegister(0x67, 0x00);
694  writeRegister(0x70, 0x04);
695  writeRegister(0x71, 0x01);
696  writeRegister(0x72, 0xFE);
697  writeRegister(0x76, 0x00);
698  writeRegister(0x77, 0x00);
699  writeRegister(0xFF, 0x01);
700  writeRegister(0x0D, 0x01);
701  writeRegister(0xFF, 0x00);
702  writeRegister(0x80, 0x01);
703  writeRegister(0x01, 0xF8);
704  writeRegister(0xFF, 0x01);
705  writeRegister(0x8E, 0x01);
706  writeRegister(0x00, 0x01);
707  writeRegister(0xFF, 0x00);
708  writeRegister(0x80, 0x00);
709 
710  return PX4_OK;
711 }
712 
713 int
714 VL53LXX::singleRefCalibration(const uint8_t byte)
715 {
716  uint8_t val = 0;
717 
718  writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
720 
721  while ((val & 0x07) == 0) {
723  }
724 
727 
728  return PX4_OK;
729 }
730 
731 void
733 {
734  // Flush the report ring buffer.
735  _reports->flush();
736 
737  // Schedule the first cycle.
738  ScheduleNow();
739 }
740 
741 void
743 {
744  ScheduleClear();
745 }
746 
747 int
748 VL53LXX::writeRegister(const uint8_t reg_address, const uint8_t value)
749 {
750  uint8_t cmd[2] = {};
751  cmd[0] = reg_address;
752  cmd[1] = value;
753 
754  int ret = transfer(&cmd[0], 2, nullptr, 0);
755 
756  if (ret != PX4_OK) {
758  return ret;
759  }
760 
761  return PX4_OK;
762 }
763 
764 
765 int
766 VL53LXX::writeRegisterMulti(const uint8_t reg_address, const uint8_t *value,
767  const uint8_t length) /* bytes are send in order as they are in the array */
768 {
769  if (length > 6 || length < 1) {
770  DEVICE_LOG("VL53LXX::writeRegisterMulti length out of range");
771  return PX4_ERROR;
772  }
773 
774  /* be careful: for uint16_t to send first higher byte */
775  uint8_t cmd[7] = {};
776  cmd[0] = reg_address;
777 
778  memcpy(&cmd[1], &value[0], length);
779 
780  int ret = transfer(&cmd[0], length + 1, nullptr, 0);
781 
782  if (ret != PX4_OK) {
784  return ret;
785  }
786 
787  return PX4_OK;
788 }
789 
790 
791 /**
792  * Local functions in support of the shell command.
793  */
794 namespace vl53lxx
795 {
796 
798 
799 int reset();
800 int start(const uint8_t rotation);
801 int start_bus(const uint8_t rotation = 0, const int i2c_bus = VL53LXX_BUS_DEFAULT);
802 int status();
803 int stop();
804 int test();
805 int usage();
806 
807 /**
808  * Reset the driver.
809  */
810 int
812 {
813  if (g_dev == nullptr) {
814  PX4_ERR("driver not running");
815  return PX4_ERROR;
816  }
817 
818  g_dev->stop();
819  g_dev->start();
820  PX4_INFO("driver reset");
821  return PX4_OK;
822 }
823 
824 /**
825  * Attempt to start driver on all available I2C busses.
826  *
827  * This function will return as soon as the first sensor
828  * is detected on one of the available busses or if no
829  * sensors are detected.
830  */
831 int
832 start(const uint8_t rotation)
833 {
834  for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
835  if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) {
836  return PX4_OK;
837  }
838  }
839 
840  return PX4_ERROR;
841 }
842 
843 /**
844  * Start the driver on a specific bus.
845  *
846  * This function only returns if the sensor is up and running
847  * or could not be detected successfully.
848  */
849 int
850 start_bus(const uint8_t rotation, const int i2c_bus)
851 {
852  if (g_dev != nullptr) {
853  PX4_ERR("already started");
854  return PX4_OK;
855  }
856 
857  // Instantiate the driver.
858  g_dev = new VL53LXX(rotation, i2c_bus);
859 
860  if (g_dev == nullptr) {
861  delete g_dev;
862  return PX4_ERROR;
863  }
864 
865  // Initialize the sensor.
866  if (g_dev->init() != PX4_OK) {
867  delete g_dev;
868  g_dev = nullptr;
869  return PX4_ERROR;
870  }
871 
872  // Start the driver.
873  g_dev->start();
874  PX4_INFO("driver started");
875  return PX4_OK;
876 }
877 
878 /**
879  * Print the driver status.
880  */
881 int
883 {
884  if (g_dev == nullptr) {
885  PX4_ERR("driver not running");
886  return PX4_ERROR;
887  }
888 
889  g_dev->print_info();
890 
891  return PX4_OK;
892 }
893 
894 /**
895  * Stop the driver
896  */
897 int
899 {
900  if (g_dev != nullptr) {
901  delete g_dev;
902  g_dev = nullptr;
903 
904  }
905 
906  PX4_INFO("driver stopped");
907  return PX4_OK;
908 }
909 
910 /**
911  * Perform some basic functional tests on the driver;
912  * make sure we can collect data from the sensor in polled
913  * and automatic modes.
914  */
915 int
917 {
918  int fd = px4_open(VL53LXX_DEVICE_PATH, O_RDONLY);
919 
920  if (fd < 0) {
921  PX4_ERR("%s open failed (try 'vl53lxx start' if the driver is not running)", VL53LXX_DEVICE_PATH);
922  return PX4_ERROR;
923  }
924 
925  // Perform a simple demand read.
926  distance_sensor_s report {};
927  ssize_t num_bytes = read(fd, &report, sizeof(report));
928 
929  if (num_bytes != sizeof(report)) {
930  PX4_ERR("immediate read failed");
931  return PX4_ERROR;
932  }
933 
934  print_message(report);
935 
936  px4_close(fd);
937 
938  PX4_INFO("PASS");
939  return PX4_OK;
940 }
941 
942 int
944 {
945  PX4_INFO("usage: vl53lxx command [options]");
946  PX4_INFO("options:");
947  PX4_INFO("\t-a --all available busses");
948  PX4_INFO("\t-b --bus i2cbus (%d)", VL53LXX_BUS_DEFAULT);
949  PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
950  PX4_INFO("command:");
951  PX4_INFO("\treset|start|status|stop|test|usage");
952  return PX4_OK;
953 }
954 
955 } // namespace vl53lxx
956 
957 
958 /**
959  * Driver 'main' command.
960  */
961 extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[])
962 {
963  const char *myoptarg = nullptr;
964 
965  int ch = 0;
966  int i2c_bus = VL53LXX_BUS_DEFAULT;
967  int myoptind = 1;
968 
969  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
970 
971  bool start_all = false;
972 
973  while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
974  switch (ch) {
975  case 'a':
976  start_all = true;
977  break;
978 
979  case 'b':
980  i2c_bus = atoi(myoptarg);
981  break;
982 
983  case 'R':
984  rotation = (uint8_t)atoi(myoptarg);
985  break;
986 
987  default:
988  PX4_WARN("Unknown option!");
989  return vl53lxx::usage();
990  }
991  }
992 
993  if (myoptind >= argc) {
994  return vl53lxx::usage();
995  }
996 
997  // Reset the driver.
998  if (!strcmp(argv[myoptind], "reset")) {
999  return vl53lxx::reset();
1000  }
1001 
1002  // Start/load the driver.
1003  if (!strcmp(argv[myoptind], "start")) {
1004  if (start_all) {
1005  return vl53lxx::start(rotation);
1006 
1007  } else {
1008  return vl53lxx::start_bus(rotation, i2c_bus);
1009  }
1010  }
1011 
1012  // Print the driver status.
1013  if (!strcmp(argv[myoptind], "status")) {
1014  return vl53lxx::status();
1015  }
1016 
1017  // Stop the driver.
1018  if (!strcmp(argv[myoptind], "stop")) {
1019  return vl53lxx::stop();
1020  }
1021 
1022  // Test the driver/device.
1023  if (!strcmp(argv[myoptind], "test")) {
1024  return vl53lxx::test();
1025  }
1026 
1027  // Print driver usage information.
1028  return vl53lxx::usage();
1029 }
bool _new_measurement
Definition: vl53lxx.cpp:150
#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG
Definition: vl53lxx.cpp:65
#define VL53LXX_MIN_RANGING_DISTANCE
Definition: vl53lxx.cpp:85
#define VL53LXX_MAX_RANGING_DISTANCE
Definition: vl53lxx.cpp:84
#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG
Definition: vl53lxx.cpp:69
int status()
Print the driver status.
Definition: vl53lxx.cpp:882
#define RANGE_FINDER_BASE_DEVICE_PATH
measure the time elapsed performing an event
Definition: perf_counter.h:56
int start_bus(const uint8_t rotation=0, const int i2c_bus=VL53LXX_BUS_DEFAULT)
Start the driver on a specific bus.
Definition: vl53lxx.cpp:850
int spadCalculations()
Definition: vl53lxx.cpp:487
#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG
Definition: vl53lxx.cpp:74
#define RESULT_INTERRUPT_STATUS_REG
Definition: vl53lxx.cpp:72
perf_counter_t _sample_perf
Definition: vl53lxx.cpp:162
#define RESULT_RANGE_STATUS_REG
Definition: vl53lxx.cpp:77
int stop()
Stop the driver.
Definition: vl53lxx.cpp:898
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length)
Definition: vl53lxx.cpp:449
virtual int init() override
Definition: vl53lxx.cpp:249
Definition: I2C.hpp:51
perf_counter_t _comms_errors
Definition: vl53lxx.cpp:161
int start(const uint8_t rotation)
Attempt to start driver on all available I2C busses.
Definition: vl53lxx.cpp:832
virtual ssize_t read(device::file_t *file_pointer, char *buffer, size_t buflen)
Definition: vl53lxx.cpp:369
#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG
Definition: vl53lxx.cpp:67
uint8_t _rotation
Definition: vl53lxx.cpp:156
count the number of times an event occurs
Definition: perf_counter.h:55
High-resolution timer with callouts and timekeeping.
#define VL53LXX_BUS_DEFAULT
Definition: vl53lxx.cpp:55
int reset()
Reset the driver.
Definition: vl53lxx.cpp:811
void stop()
Stop the automatic measurement state machine.
Definition: vl53lxx.cpp:742
int _measure_interval
Definition: vl53lxx.cpp:153
bool _collect_phase
Definition: vl53lxx.cpp:148
#define GPIO_HV_MUX_ACTIVE_HIGH_REG
Definition: vl53lxx.cpp:75
#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG
Definition: vl53lxx.cpp:61
int usage()
Prints info about the driver argument usage.
Definition: vl53lxx.cpp:943
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void perf_count(perf_counter_t handle)
Count a performance event.
int sensorInit()
Definition: vl53lxx.cpp:576
bool _measurement_started
Definition: vl53lxx.cpp:149
#define VL53LXX_BASEADDR
Definition: vl53lxx.cpp:57
Header common to all counters.
int collect()
Collects the most recent sensor measurement data from the i2c bus.
Definition: vl53lxx.cpp:202
void perf_free(perf_counter_t handle)
Free a counter.
#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG
Definition: vl53lxx.cpp:66
#define MSRC_CONFIG_CONTROL_REG
Definition: vl53lxx.cpp:62
void init()
Activates/configures the hardware registers.
#define DEVICE_LOG(FMT,...)
Definition: Device.hpp:51
Local functions in support of the shell command.
Definition: vl53lxx.cpp:794
#define perf_alloc(a, b)
Definition: px4io.h:59
ringbuffer::RingBuffer * _reports
Definition: vl53lxx.cpp:164
#define VL53LXX_DEVICE_PATH
Definition: vl53lxx.cpp:58
int _class_instance
Definition: vl53lxx.cpp:152
void perf_end(perf_counter_t handle)
End a performance event.
int readRegister(const uint8_t reg_address, uint8_t &value)
Definition: vl53lxx.cpp:426
int measure()
Sends an i2c measure command to the sensor.
Definition: vl53lxx.cpp:281
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
static const int i2c_bus_options[]
Definition: i2c.h:46
int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length)
Definition: vl53lxx.cpp:766
void start()
Initialise the automatic measurement state machine and start it.
Definition: vl53lxx.cpp:732
orb_advert_t _distance_sensor_topic
Definition: vl53lxx.cpp:159
uint8_t _stop_variable
Definition: vl53lxx.cpp:157
#define SYSTEM_SEQUENCE_CONFIG_REG
Definition: vl53lxx.cpp:70
int px4_open(const char *path, int flags,...)
#define SYSTEM_INTERRUPT_CLEAR_REG
Definition: vl53lxx.cpp:76
virtual ~VL53LXX()
Definition: vl53lxx.cpp:177
#define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG
Definition: vl53lxx.cpp:63
VL53LXX * g_dev
Definition: vl53lxx.cpp:797
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
void print_info()
Diagnostics - print some basic information about the driver.
Definition: vl53lxx.cpp:349
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
Definition: bst.cpp:62
int writeRegister(const uint8_t reg_address, const uint8_t value)
Definition: vl53lxx.cpp:748
VL53LXX(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus=VL53LXX_BUS_DEFAULT, int address=VL53LXX_BASEADDR)
Definition: vl53lxx.cpp:168
#define OK
Definition: uavcan_main.cpp:71
int _orb_class_instance
Definition: vl53lxx.cpp:154
__EXPORT int vl53lxx_main(int argc, char *argv[])
Driver &#39;main&#39; command.
Definition: vl53lxx.cpp:961
#define VL53LXX_SAMPLE_RATE
Definition: vl53lxx.cpp:82
#define NUM_I2C_BUS_OPTIONS
Definition: i2c.h:61
#define VL53LXX_US
Definition: vl53lxx.cpp:81
#define SYSRANGE_START_REG
Definition: vl53lxx.cpp:71
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: vl53lxx.cpp:916
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
int sensorTuning()
Definition: vl53lxx.cpp:626
void perf_begin(perf_counter_t handle)
Begin a performance event.
int singleRefCalibration(const uint8_t byte)
Definition: vl53lxx.cpp:714
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
virtual int probe() override
Definition: vl53lxx.cpp:358
int px4_close(int fd)
Performance measuring tools.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: vl53lxx.cpp:471
Base class for devices connected via I2C.