PX4 Firmware
PX4 Autopilot Software http://px4.io
teraranger.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2017 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 teraranger.cpp
36  * @author Luis Rodrigues
37  *
38  * Driver for the TeraRanger One range finders connected via I2C.
39  */
40 
41 #include <drivers/device/i2c.h>
42 #include <lib/parameters/param.h>
43 #include <perf/perf_counter.h>
44 #include <px4_platform_common/px4_config.h>
45 #include <px4_platform_common/defines.h>
46 #include <px4_platform_common/getopt.h>
47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
48 #include <px4_platform_common/module.h>
50 #include <uORB/uORB.h>
51 
52 using namespace time_literals;
53 
54 /* Configuration Constants */
55 #define TERARANGER_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
56 #define TERARANGER_DEVICE_PATH "/dev/teraranger"
57 #define TERARANGER_ONE_BASEADDR 0x30 // 7-bit address.
58 #define TERARANGER_EVO_BASEADDR 0x31 // 7-bit address.
59 
60 /* TERARANGER Registers addresses */
61 #define TERARANGER_MEASURE_REG 0x00 // Measure range register.
62 #define TERARANGER_WHO_AM_I_REG 0x01 // Who am I test register.
63 #define TERARANGER_WHO_AM_I_REG_VAL 0xA1
64 
65 /* Device limits */
66 #define TERARANGER_ONE_MAX_DISTANCE (14.00f)
67 #define TERARANGER_ONE_MIN_DISTANCE (0.20f)
68 
69 #define TERARANGER_EVO_3M_MAX_DISTANCE (3.0f)
70 #define TERARANGER_EVO_3M_MIN_DISTANCE (0.10f)
71 
72 #define TERARANGER_EVO_60M_MAX_DISTANCE (60.0f)
73 #define TERARANGER_EVO_60M_MIN_DISTANCE (0.50f)
74 
75 #define TERARANGER_EVO_600HZ_MAX_DISTANCE (8.0f)
76 #define TERARANGER_EVO_600HZ_MIN_DISTANCE (0.75f)
77 
78 #define TERARANGER_MEASUREMENT_INTERVAL 50_us
79 
80 class TERARANGER : public device::I2C, public px4::ScheduledWorkItem
81 {
82 public:
83  TERARANGER(const int bus = TERARANGER_BUS_DEFAULT,
84  const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
85  const int address = TERARANGER_ONE_BASEADDR);
86 
87  virtual ~TERARANGER();
88 
89  virtual int init() override;
90 
91  /**
92  * Diagnostics - print some basic information about the driver.
93  */
94  void print_info();
95 
96  /**
97  * Initialise the automatic measurement state machine and start it.
98  *
99  * @note This function is called at open and error time. It might make sense
100  * to make it more aggressive about resetting the bus in case of errors.
101  */
102  void start();
103 
104  /**
105  * Stop the automatic measurement state machine.
106  */
107  void stop();
108 
109 protected:
110 
111  virtual int probe() override;
112 
113 private:
114 
115  /**
116  * Collects the most recent sensor measurement data from the i2c bus.
117  */
118  int collect();
119 
120  /**
121  * Sends an i2c measure command to the sensors.
122  */
123  int measure();
124 
125  /**
126  * Test whether the device supported by the driver is present at a
127  * specific address.
128  *
129  * @param address The I2C bus address to probe.
130  * @return True if the device is present.
131  */
132  int probe_address(const uint8_t address);
133 
134  /**
135  * Perform a poll cycle; collect from the previous measurement
136  * and start a new one.
137  */
138  void Run() override;
139 
140  bool _collect_phase{false};
141 
142  int _orb_class_instance{-1};
143 
144  uint8_t _orientation{0};
145 
146  float _max_distance{0};
147  float _min_distance{0};
148 
149  orb_advert_t _distance_sensor_topic{nullptr};
150 
151  perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "tr1_comm_err")};
152  perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "tr1_read")};
153 };
154 
155 static const uint8_t crc_table[] = {
156  0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
157  0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
158  0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
159  0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
160  0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
161  0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
162  0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
163  0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
164  0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
165  0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
166  0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
167  0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
168  0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
169  0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
170  0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
171  0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
172  0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
173  0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
174  0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
175  0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
176  0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
177  0xfa, 0xfd, 0xf4, 0xf3
178 };
179 
180 static uint8_t crc8(uint8_t *p, uint8_t len)
181 {
182  uint16_t i;
183  uint16_t crc = 0x0;
184 
185  while (len--) {
186  i = (crc ^ *p++) & 0xFF;
187  crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
188  }
189 
190  return crc & 0xFF;
191 }
192 
193 
194 TERARANGER::TERARANGER(const int bus, const uint8_t orientation, const int address) :
195  I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000),
196  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
197  _orientation(orientation)
198 {
199  // up the retries since the device misses the first measure attempts
200  I2C::_retries = 3;
201 }
202 
204 {
205  // Ensure we are truly inactive.
206  stop();
207 
208  // Unadvertise the distance sensor topic.
209  if (_distance_sensor_topic != nullptr) {
211  }
212 
213  // Free perf counters.
216 }
217 
218 int
220 {
221  if (!_collect_phase) {
222  return measure();
223  }
224 
226 
227  uint8_t val[3] = {};
228 
229  // Transfer data from the bus.
230  int ret_val = transfer(nullptr, 0, &val[0], 3);
231 
232  if (ret_val < 0) {
233  PX4_ERR("error reading from sensor: %d", ret_val);
236  return ret_val;
237  }
238 
239  uint16_t distance_mm = (val[0] << 8) | val[1];
240  float distance_m = static_cast<float>(distance_mm) * 1e-3f;
241 
242  distance_sensor_s report {};
243  report.current_distance = distance_m;
244  report.id = 0; // TODO: set proper ID.
245  report.min_distance = _min_distance;
246  report.max_distance = _max_distance;
247  report.orientation = _orientation;
248  report.signal_quality = -1;
249  report.timestamp = hrt_absolute_time();
250  // NOTE: There is no enum item for a combined LASER and ULTRASOUND which this should be.
251  report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
252  report.variance = 0.0f;
253 
254  if (crc8(val, 2) == val[2] &&
255  (float)report.current_distance > report.min_distance &&
256  (float)report.current_distance < report.max_distance) {
257  int instance_id;
258  orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id, ORB_PRIO_DEFAULT);
259  }
260 
261  // Next phase is measurement.
262  _collect_phase = false;
263 
266  return PX4_OK;
267 }
268 
269 int
271 {
272  int hw_model = 0;
273  param_get(param_find("SENS_EN_TRANGER"), &hw_model);
274 
275  switch (hw_model) {
276  case 0: // Disabled
277  PX4_WARN("Disabled");
278  return PX4_ERROR;
279 
280  case 1: // Autodetect - assume default Teraranger One
281  set_device_address(TERARANGER_ONE_BASEADDR);
282 
283  if (I2C::init() != OK) {
284  set_device_address(TERARANGER_EVO_BASEADDR);
285 
286  if (I2C::init() != OK) {
287  return PX4_ERROR;
288 
289  } else {
290  // Assume minimum and maximum possible distances acros Evo family
293  }
294 
295  } else {
298  }
299 
300  break;
301 
302  case 2: // Teraranger One.
303  set_device_address(TERARANGER_ONE_BASEADDR);
304 
305  if (I2C::init() != OK) {
306  return PX4_ERROR;
307  }
308 
311  break;
312 
313  case 3: // Teraranger Evo60m.
314  set_device_address(TERARANGER_EVO_BASEADDR);
315 
316  // I2C init (and probe) first.
317  if (I2C::init() != OK) {
318  return PX4_ERROR;
319  }
320 
323  break;
324 
325  case 4: // Teraranger Evo600Hz.
326  set_device_address(TERARANGER_EVO_BASEADDR);
327 
328  // I2C init (and probe) first.
329  if (I2C::init() != OK) {
330  return PX4_ERROR;
331  }
332 
335  break;
336 
337  case 5: // Teraranger Evo3m.
338  set_device_address(TERARANGER_EVO_BASEADDR);
339 
340  // I2C init (and probe) first.
341  if (I2C::init() != OK) {
342  return PX4_ERROR;
343  }
344 
347  break;
348 
349  default:
350  PX4_ERR("invalid HW model %d.", hw_model);
351  return PX4_ERROR;
352  }
353 
354  return PX4_OK;
355 }
356 
357 int
359 {
360  // Send the command to begin a measurement.
361  const uint8_t cmd = TERARANGER_MEASURE_REG;
362  int ret_val = transfer(&cmd, sizeof(cmd), nullptr, 0);
363 
364  if (ret_val != PX4_OK) {
366  PX4_DEBUG("i2c::transfer returned %d", ret_val);
367  return ret_val;
368  }
369 
370  _collect_phase = true;
371  return PX4_OK;
372 }
373 
374 int
376 {
377  uint8_t who_am_i = 0;
378 
379  const uint8_t cmd = TERARANGER_WHO_AM_I_REG;
380 
381  // Can't use a single transfer as Teraranger needs a bit of time for internal processing.
382  if (transfer(&cmd, 1, nullptr, 0) == OK) {
383  if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TERARANGER_WHO_AM_I_REG_VAL) {
384  return measure();
385  }
386  }
387 
388  PX4_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n",
389  (unsigned)who_am_i,
391 
392  // Not found on any address.
393  return -EIO;
394 }
395 
396 void
398 {
401  printf("poll interval: %u\n", TERARANGER_MEASUREMENT_INTERVAL);
402 }
403 
404 void
406 {
407  // Perform data collection.
408  collect();
409 }
410 
411 void
413 {
414  _collect_phase = false;
415 
416  // Schedule the driver to run on a set interval
417  ScheduleOnInterval(TERARANGER_MEASUREMENT_INTERVAL);
418 }
419 
420 void
422 {
423  ScheduleClear();
424 }
425 
426 /**
427  * Local functions in support of the shell command.
428  */
429 namespace teraranger
430 {
431 
433 
434 int start(const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
435 int start_bus(const int i2c_bus = TERARANGER_BUS_DEFAULT,
436  const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
437 int status();
438 int stop();
439 int usage();
440 
441 /**
442  *
443  * Attempt to start driver on all available I2C busses.
444  *
445  * This function will return as soon as the first sensor
446  * is detected on one of the available busses or if no
447  * sensors are detected.
448  *
449  */
450 int
451 start(const uint8_t orientation)
452 {
453  if (g_dev != nullptr) {
454  PX4_ERR("already started");
455  return PX4_ERROR;
456  }
457 
458  for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
459  if (start_bus(i2c_bus_options[i], orientation) == PX4_OK) {
460  return PX4_OK;
461  }
462  }
463 
464  return PX4_ERROR;
465 }
466 
467 /**
468  * Start the driver on a specific bus.
469  *
470  * This function only returns if the sensor is up and running
471  * or could not be detected successfully.
472  */
473 int
474 start_bus(const int i2c_bus, const uint8_t orientation)
475 {
476  if (g_dev != nullptr) {
477  PX4_ERR("already started");
478  return PX4_OK;
479  }
480 
481  // Instantiate the driver.
482  g_dev = new TERARANGER(i2c_bus, orientation);
483 
484  if (g_dev == nullptr) {
485  delete g_dev;
486  return PX4_ERROR;
487  }
488 
489  // Initialize the sensor.
490  if (g_dev->init() != PX4_OK) {
491  delete g_dev;
492  g_dev = nullptr;
493  return PX4_ERROR;
494  }
495 
496  // Start the driver.
497  g_dev->start();
498 
499  PX4_INFO("driver started");
500  return PX4_OK;
501 }
502 
503 /**
504  * Stop the driver
505  */
506 int
508 {
509  if (g_dev != nullptr) {
510  delete g_dev;
511  g_dev = nullptr;
512 
513  }
514 
515  PX4_INFO("driver stopped");
516  return PX4_OK;
517 }
518 
519 /**
520  * Print the driver status.
521  */
522 int
524 {
525  if (g_dev == nullptr) {
526  PX4_ERR("driver not running");
527  return PX4_ERROR;
528  }
529 
530  g_dev->print_info();
531 
532  return PX4_OK;
533 }
534 
535 int
537 {
538  PRINT_MODULE_DESCRIPTION(
539  R"DESCR_STR(
540 ### Description
541 
542 I2C bus driver for TeraRanger rangefinders.
543 
544 The sensor/driver must be enabled using the parameter SENS_EN_TRANGER.
545 
546 Setup/usage information: https://docs.px4.io/en/sensor/rangefinders.html#teraranger-rangefinders
547 
548 ### Examples
549 Start driver on any bus (start on bus where first sensor found).
550 $ teraranger start -a
551 Start driver on specified bus
552 $ teraranger start -b 1
553 Stop driver
554 $ teraranger stop
555 )DESCR_STR");
556 
557  PRINT_MODULE_USAGE_NAME("teraranger", "driver");
558  PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
559  PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
560  PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true);
561  PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
562  PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
563  PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
564  PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver information");
565 
566  return PX4_OK;
567 }
568 
569 } // namespace
570 
571 /**
572  * Driver 'main' command.
573  */
574 extern "C" __EXPORT int teraranger_main(int argc, char *argv[])
575 {
576  const char *myoptarg = nullptr;
577 
578  int ch;
579  int i2c_bus = TERARANGER_BUS_DEFAULT;
580  int myoptind = 1;
581 
582  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
583 
584  bool start_all = false;
585 
586  while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
587  switch (ch) {
588  case 'a':
589  start_all = true;
590  break;
591 
592  case 'b':
593  i2c_bus = atoi(myoptarg);
594  break;
595 
596  case 'R':
597  rotation = (uint8_t)atoi(myoptarg);
598  break;
599 
600  default:
601  PX4_WARN("Unknown option!");
602  return teraranger::usage();
603  }
604  }
605 
606  if (myoptind >= argc) {
607  return teraranger::usage();
608  }
609 
610  // Start/load the driver.
611  if (!strcmp(argv[myoptind], "start")) {
612 
613  if (start_all) {
614  return teraranger::start(rotation);
615 
616  } else {
617  return teraranger::start_bus(i2c_bus, rotation);
618  }
619  }
620 
621  // Print the driver status.
622  if (!strcmp(argv[myoptind], "status")) {
623  return teraranger::status();
624  }
625 
626  // Stop the driver.
627  if (!strcmp(argv[myoptind], "stop")) {
628  return teraranger::stop();
629  }
630 
631  return teraranger::usage();
632 }
#define TERARANGER_WHO_AM_I_REG_VAL
Definition: teraranger.cpp:63
virtual int probe() override
Definition: teraranger.cpp:375
measure the time elapsed performing an event
Definition: perf_counter.h:56
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
API for the uORB lightweight object broker.
#define TERARANGER_EVO_60M_MAX_DISTANCE
Definition: teraranger.cpp:72
#define TERARANGER_WHO_AM_I_REG
Definition: teraranger.cpp:62
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
#define TERARANGER_EVO_BASEADDR
Definition: teraranger.cpp:58
#define TERARANGER_EVO_3M_MIN_DISTANCE
Definition: teraranger.cpp:70
Definition: I2C.hpp:51
TERARANGER(const int bus=TERARANGER_BUS_DEFAULT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, const int address=TERARANGER_ONE_BASEADDR)
Definition: teraranger.cpp:194
Local functions in support of the shell command.
Definition: teraranger.cpp:429
int status()
Print the driver status.
Definition: teraranger.cpp:523
void stop()
Stop the automatic measurement state machine.
Definition: teraranger.cpp:421
void print_info()
Diagnostics - print some basic information about the driver.
Definition: teraranger.cpp:397
static void stop()
Definition: dataman.cpp:1491
#define TERARANGER_MEASUREMENT_INTERVAL
Definition: teraranger.cpp:78
count the number of times an event occurs
Definition: perf_counter.h:55
float _min_distance
Definition: teraranger.cpp:147
#define TERARANGER_EVO_600HZ_MAX_DISTANCE
Definition: teraranger.cpp:75
__EXPORT int teraranger_main(int argc, char *argv[])
Driver &#39;main&#39; command.
Definition: teraranger.cpp:574
Global flash based parameter store.
#define TERARANGER_ONE_MIN_DISTANCE
Definition: teraranger.cpp:67
perf_counter_t _sample_perf
Definition: teraranger.cpp:152
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int stop()
Stop the driver.
Definition: teraranger.cpp:507
void perf_count(perf_counter_t handle)
Count a performance event.
virtual int init() override
Definition: teraranger.cpp:270
perf_counter_t _comms_errors
Definition: teraranger.cpp:151
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
#define TERARANGER_EVO_600HZ_MIN_DISTANCE
Definition: teraranger.cpp:76
void init()
Activates/configures the hardware registers.
float _max_distance
Definition: teraranger.cpp:146
#define perf_alloc(a, b)
Definition: px4io.h:59
uint8_t _orientation
Definition: teraranger.cpp:144
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
static uint8_t crc8(uint8_t *p, uint8_t len)
Definition: teraranger.cpp:180
virtual ~TERARANGER()
Definition: teraranger.cpp:203
#define TERARANGER_EVO_3M_MAX_DISTANCE
Definition: teraranger.cpp:69
void perf_end(perf_counter_t handle)
End a performance event.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: teraranger.cpp:405
#define TERARANGER_BUS_DEFAULT
Definition: teraranger.cpp:55
int start_bus(const int i2c_bus=TERARANGER_BUS_DEFAULT, const uint8_t orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the driver on a specific bus.
Definition: teraranger.cpp:474
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
static const int i2c_bus_options[]
Definition: i2c.h:46
void start()
Initialise the automatic measurement state machine and start it.
Definition: teraranger.cpp:412
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static int start()
Definition: dataman.cpp:1452
int usage()
Prints info about the driver argument usage.
Definition: teraranger.cpp:536
#define TERARANGER_ONE_MAX_DISTANCE
Definition: teraranger.cpp:66
bool _collect_phase
Definition: teraranger.cpp:140
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
orb_advert_t _distance_sensor_topic
Definition: teraranger.cpp:149
#define TERARANGER_MEASURE_REG
Definition: teraranger.cpp:61
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
static const uint8_t crc_table[]
Definition: teraranger.cpp:155
Definition: bst.cpp:62
TERARANGER * g_dev
Definition: teraranger.cpp:432
int start(const uint8_t orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Attempt to start driver on all available I2C busses.
Definition: teraranger.cpp:451
#define TERARANGER_EVO_60M_MIN_DISTANCE
Definition: teraranger.cpp:73
#define OK
Definition: uavcan_main.cpp:71
#define NUM_I2C_BUS_OPTIONS
Definition: i2c.h:61
#define TERARANGER_ONE_BASEADDR
Definition: teraranger.cpp:57
void perf_begin(perf_counter_t handle)
Begin a performance event.
int collect()
Collects the most recent sensor measurement data from the i2c bus.
Definition: teraranger.cpp:219
__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
int measure()
Sends an i2c measure command to the sensors.
Definition: teraranger.cpp:358
#define TERARANGER_DEVICE_PATH
Definition: teraranger.cpp:56
Performance measuring tools.
Base class for devices connected via I2C.