PX4 Firmware
PX4 Autopilot Software http://px4.io
leddar_one.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2017 Intel Corporation. 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 #include <stdlib.h>
36 #include <string.h>
37 #include <termios.h>
38 
39 #include <arch/board/board.h>
40 #include <drivers/device/device.h>
42 #include <drivers/drv_hrt.h>
44 #include <perf/perf_counter.h>
45 #include <px4_platform_common/px4_config.h>
46 #include <px4_platform_common/defines.h>
47 #include <px4_platform_common/getopt.h>
48 #include <px4_platform_common/module.h>
49 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
51 
52 using namespace time_literals;
53 
54 #define DEVICE_PATH "/dev/LeddarOne"
55 #define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3"
56 
57 #define LEDDAR_ONE_FIELD_OF_VIEW (0.105f) // 6 deg cone angle.
58 
59 #define LEDDAR_ONE_MAX_DISTANCE 40.0f
60 #define LEDDAR_ONE_MIN_DISTANCE 0.01f
61 
62 #define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz
63 
64 #define MODBUS_SLAVE_ADDRESS 0x01
65 #define MODBUS_READING_FUNCTION 0x04
66 #define READING_START_ADDR 0x14
67 #define READING_LEN 0xA
68 
69 static const uint8_t request_reading_msg[] = {
72  0, /* starting addr high byte */
74  0, /* number of bytes to read high byte */
76  0x30, /* CRC low */
77  0x09 /* CRC high */
78 };
79 
80 struct __attribute__((__packed__)) reading_msg {
81  uint8_t slave_addr;
82  uint8_t function;
83  uint8_t len;
84  uint8_t low_timestamp_high_byte;
85  uint8_t low_timestamp_low_byte;
86  uint8_t high_timestamp_high_byte;
87  uint8_t high_timestamp_low_byte;
88  uint8_t temp_high;
89  uint8_t temp_low;
90  uint8_t num_detections_high_byte;
91  uint8_t num_detections_low_byte;
92  uint8_t first_dist_high_byte;
93  uint8_t first_dist_low_byte;
94  uint8_t first_amplitude_high_byte;
95  uint8_t first_amplitude_low_byte;
96  uint8_t second_dist_high_byte;
97  uint8_t second_dist_low_byte;
98  uint8_t second_amplitude_high_byte;
99  uint8_t second_amplitude_low_byte;
100  uint8_t third_dist_high_byte;
101  uint8_t third_dist_low_byte;
102  uint8_t third_amplitude_high_byte;
103  uint8_t third_amplitude_low_byte;
104  uint16_t crc; /* little-endian */
105 };
106 
107 class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem
108 {
109 public:
110  LeddarOne(const char *device_path,
111  const char *serial_port = LEDDAR_ONE_DEFAULT_SERIAL_PORT,
112  const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
113  virtual ~LeddarOne();
114 
115  virtual int init() override;
116 
117  /**
118  * Diagnostics - print some basic information about the driver.
119  */
120  void print_info();
121 
122  /**
123  * Initialise the automatic measurement state machine and start it.
124  */
125  void start();
126 
127  /**
128  * Stop the automatic measurement state machine.
129  */
130  void stop();
131 
132 private:
133 
134  /**
135  * Calculates the 16 byte crc value for the data frame.
136  * @param data_frame The data frame to compute a checksum for.
137  * @param crc16_length The length of the data frame.
138  */
139  uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length);
140 
141  /**
142  * Reads the data measrurement from serial UART.
143  */
144  int collect();
145 
146  /**
147  * Sends a data request message to the sensor.
148  */
149  int measure();
150 
151  /**
152  * Opens and configures the UART serial communications port.
153  * @param speed The baudrate (speed) to configure the serial UART port.
154  */
155  int open_serial_port(const speed_t speed = B115200);
156 
157  void Run() override;
158 
159  const char *_serial_port{nullptr};
160 
162 
163  int _file_descriptor{-1};
164  int _orb_class_instance{-1};
165 
166  uint8_t _buffer[sizeof(reading_msg)];
167  uint8_t _buffer_len{0};
168 
169  hrt_abstime _measurement_time{0};
170 
171  perf_counter_t _comms_error{perf_alloc(PC_COUNT, "leddar_one_comms_error")};
172  perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "leddar_one_sample")};
173 };
174 
175 LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation):
176  CDev(device_path),
177  ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)),
178  _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation)
179 {
180  _serial_port = strdup(serial_port);
181 
182  _px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
186  _px4_rangefinder.set_orientation(device_orientation);
187 }
188 
190 {
191  stop();
192  free((char *)_serial_port);
195 }
196 
197 uint16_t
198 LeddarOne::crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length)
199 {
200  uint16_t crc = 0xFFFF;
201 
202  for (uint8_t i = 0; i < crc16_length; i++) {
203  crc ^= data_frame[i];
204 
205  for (uint8_t j = 0; j < 8; j++) {
206  if (crc & 1) {
207  crc = (crc >> 1) ^ 0xA001;
208 
209  } else {
210  crc >>= 1;
211  }
212  }
213  }
214 
215  return crc;
216 }
217 
218 int
220 {
222 
223  const int buffer_size = sizeof(_buffer);
224  const int message_size = sizeof(reading_msg);
225 
226  int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, buffer_size - _buffer_len);
227 
228  if (bytes_read < 1) {
229  // Trigger a new measurement.
230  return measure();
231  }
232 
233  _buffer_len += bytes_read;
234 
235  if (_buffer_len < message_size) {
236  // Return on next scheduled cycle to collect remaining data.
237  return PX4_OK;
238  }
239 
240  reading_msg *msg {nullptr};
241  msg = (reading_msg *)_buffer;
242 
243  if (msg->slave_addr != MODBUS_SLAVE_ADDRESS ||
244  msg->function != MODBUS_READING_FUNCTION) {
245  PX4_ERR("slave address or function read error");
248  return measure();
249  }
250 
251  const uint16_t crc16 = crc16_calc(_buffer, buffer_size - 2);
252 
253  if (crc16 != msg->crc) {
254  PX4_ERR("crc error");
257  return measure();
258  }
259 
260  // NOTE: little-endian support only.
261  uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte);
262  float distance_m = static_cast<float>(distance_mm) / 1000.0f;
263 
264  // @TODO - implement a meaningful signal quality value.
265  int8_t signal_quality = -1;
266 
267  _px4_rangefinder.update(_measurement_time, distance_m, signal_quality);
268 
270 
271  // Trigger the next measurement.
272  return measure();;
273 }
274 
275 int
277 {
278  if (CDev::init()) {
279  PX4_ERR("Unable to initialize device");
280  return PX4_ERROR;
281  }
282 
283  if (open_serial_port() != PX4_OK) {
284  return PX4_ERROR;
285  }
286 
287  hrt_abstime time_now = hrt_absolute_time();
288 
289  const hrt_abstime timeout_usec = time_now + 500000_us; // 0.5sec
290 
291  while (time_now < timeout_usec) {
292  if (measure() == PX4_OK) {
293  px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL);
294 
295  if (collect() == PX4_OK) {
296  // The file descriptor can only be accessed by the process that opened it,
297  // so closing here allows the port to be opened from scheduled work queue.
298  stop();
299  return PX4_OK;
300  }
301  }
302 
303  px4_usleep(1000);
304  time_now = hrt_absolute_time();
305  }
306 
307  PX4_ERR("No readings from LeddarOne");
308  return PX4_ERROR;
309 }
310 
311 int
313 {
314  // Flush the receive buffer.
315  tcflush(_file_descriptor, TCIFLUSH);
316 
318 
319  if (num_bytes != sizeof(request_reading_msg)) {
320  PX4_INFO("measurement error: %i, errno: %i", num_bytes, errno);
321  return PX4_ERROR;
322  }
323 
325  _buffer_len = 0;
326  return PX4_OK;
327 }
328 
329 int
330 LeddarOne::open_serial_port(const speed_t speed)
331 {
332  // File descriptor already initialized?
333  if (_file_descriptor > 0) {
334  // PX4_INFO("serial port already open");
335  return PX4_OK;
336  }
337 
338  // Configure port flags for read/write, non-controlling, non-blocking.
339  int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
340 
341  // Open the serial port.
343 
344  if (_file_descriptor < 0) {
345  PX4_ERR("open failed (%i)", errno);
346  return PX4_ERROR;
347  }
348 
349  termios uart_config = {};
350 
351  // Store the current port configuration. attributes.
352  if (tcgetattr(_file_descriptor, &uart_config)) {
353  PX4_ERR("Unable to get termios from %s.", _serial_port);
355  _file_descriptor = -1;
356  return PX4_ERROR;
357  }
358 
359  // Clear: data bit size, two stop bits, parity, hardware flow control.
360  uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB | CRTSCTS);
361 
362  // Set: 8 data bits, enable receiver, ignore modem status lines.
363  uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
364 
365  // Clear: echo, echo new line, canonical input and extended input.
366  uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);
367 
368  // Clear ONLCR flag (which appends a CR for every LF).
369  uart_config.c_oflag &= ~ONLCR;
370 
371  // Set the input baud rate in the uart_config struct.
372  int termios_state = cfsetispeed(&uart_config, speed);
373 
374  if (termios_state < 0) {
375  PX4_ERR("CFG: %d ISPD", termios_state);
377  return PX4_ERROR;
378  }
379 
380  // Set the output baud rate in the uart_config struct.
381  termios_state = cfsetospeed(&uart_config, speed);
382 
383  if (termios_state < 0) {
384  PX4_ERR("CFG: %d OSPD", termios_state);
386  return PX4_ERROR;
387  }
388 
389  // Apply the modified port attributes.
390  termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
391 
392  if (termios_state < 0) {
393  PX4_ERR("baud %d ATTR", termios_state);
395  return PX4_ERROR;
396  }
397 
398  // Flush the hardware buffers.
399  tcflush(_file_descriptor, TCIOFLUSH);
400 
401  PX4_INFO("opened UART port %s", _serial_port);
402  return PX4_OK;
403 }
404 
405 void
407 {
410  PX4_INFO("measure interval: %u msec", static_cast<uint16_t>(LEDDAR_ONE_MEASURE_INTERVAL));
411 }
412 
413 void
415 {
416  // Ensure the serial port is open.
418 
419  collect();
420 }
421 
422 void
424 {
425  // Schedule the driver at regular intervals.
427  PX4_INFO("driver started");
428 }
429 
430 void
432 {
433  // Ensure the serial port is closed.
435  _file_descriptor = -1;
436 
437  // Clear the work queue schedule.
438  ScheduleClear();
439 }
440 
441 
442 /**
443  * Local functions in support of the shell command.
444  */
445 namespace leddar_one
446 {
447 
449 
450 int start(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT,
451  const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
452 int status();
453 int stop();
454 int test(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT);
455 int usage();
456 
457 int start(const char *port, const uint8_t rotation)
458 {
459  if (g_dev != nullptr) {
460  PX4_ERR("already started");
461  return PX4_ERROR;
462  }
463 
464  g_dev = new LeddarOne(DEVICE_PATH, port, rotation);
465 
466  if (g_dev == nullptr) {
467  PX4_ERR("object instantiate failed");
468  delete g_dev;
469  return PX4_ERROR;
470  }
471 
472  // Initialize the sensor.
473  if (g_dev->init() != PX4_OK) {
474  PX4_ERR("driver start failed");
475  delete g_dev;
476  g_dev = nullptr;
477  return PX4_ERROR;
478  }
479 
480  // Start the driver.
481  g_dev->start();
482  return PX4_OK;
483 }
484 
485 /**
486  * Print the driver status.
487  */
488 int
490 {
491  if (g_dev == nullptr) {
492  PX4_ERR("driver not running");
493  return PX4_ERROR;
494  }
495 
496  g_dev->print_info();
497 
498  return PX4_OK;
499 }
500 
501 /**
502  * Stop the driver
503  */
504 int
506 {
507  if (g_dev != nullptr) {
508  delete g_dev;
509  g_dev = nullptr;
510 
511  }
512 
513  PX4_INFO("driver stopped");
514  return PX4_OK;
515 }
516 
517 /**
518  * Perform some basic functional tests on the driver;
519  * make sure we can collect data from the sensor in polled
520  * and automatic modes.
521  */
522 int
523 test(const char *port)
524 {
525  // Configure port flags for read/write, non-controlling, non-blocking.
526  int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
527 
528  // Open the serial port.
529  int fd = ::open(port, flags);
530 
531  if (fd < 0) {
532  PX4_ERR("Unable to open %s", port);
533  return PX4_ERROR;
534  }
535 
536  ssize_t num_bytes = ::write(fd, request_reading_msg, sizeof(request_reading_msg));
537 
538  if (num_bytes != sizeof(request_reading_msg)) {
539  PX4_INFO("serial port write failed: %i, errno: %i", num_bytes, errno);
540  return PX4_ERROR;
541  }
542 
543  px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL);
544 
545  uint8_t buffer[sizeof(reading_msg)];
546 
547  num_bytes = ::read(fd, buffer, sizeof(reading_msg));
548 
549  if (num_bytes != sizeof(reading_msg)) {
550  PX4_ERR("Data not available at %s", port);
551  return PX4_ERROR;
552  }
553 
554  close(fd);
555 
556  PX4_INFO("PASS");
557  return PX4_OK;
558 }
559 
560 int
562 {
563  PRINT_MODULE_DESCRIPTION(
564  R"DESCR_STR(
565 ### Description
566 
567 Serial bus driver for the LeddarOne LiDAR.
568 
569 Most boards are configured to enable/start the driver on a specified UART using the SENS_LEDDAR1_CFG parameter.
570 
571 Setup/usage information: https://docs.px4.io/en/sensor/leddar_one.html
572 
573 ### Examples
574 
575 Attempt to start driver on a specified serial device.
576 $ leddar_one start -d /dev/ttyS1
577 Stop driver
578 $ leddar_one stop
579 )DESCR_STR");
580 
581  PRINT_MODULE_USAGE_NAME("leddar_one", "driver");
582  PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
583  PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
584  PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
585  PRINT_MODULE_USAGE_PARAM_INT('r', 25, 1, 25, "Sensor rotation - downward facing by default", true);
586  PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
587  PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
588  return PX4_OK;
589 }
590 
591 } // namespace
592 
593 extern "C" __EXPORT int leddar_one_main(int argc, char *argv[])
594 {
595  const char *myoptarg = nullptr;
596 
597  int ch = 0;
598  int myoptind = 1;
599 
600  const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT;
601  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
602 
603  while ((ch = px4_getopt(argc, argv, "d:r", &myoptind, &myoptarg)) != EOF) {
604  switch (ch) {
605  case 'd':
606  port = myoptarg;
607  break;
608 
609  case 'r':
610  rotation = (uint8_t)atoi(myoptarg);
611  break;
612 
613  default:
614  PX4_WARN("Unknown option!");
615  return leddar_one::usage();
616  }
617  }
618 
619  if (myoptind >= argc) {
620  return leddar_one::usage();
621  }
622 
623  // Start/load the driver.
624  if (!strcmp(argv[myoptind], "start")) {
625  return leddar_one::start(port, rotation);
626  }
627 
628  // Print the driver status.
629  if (!strcmp(argv[myoptind], "status")) {
630  return leddar_one::status();
631  }
632 
633  // Stop the driver.
634  if (!strcmp(argv[myoptind], "stop")) {
635  return leddar_one::stop();
636  }
637 
638  // Test the driver/device.
639  if (!strcmp(argv[myoptind], "test")) {
640  return leddar_one::test(port);
641  }
642 
643  // Print driver usage information.
644  return leddar_one::usage();
645 }
void set_max_distance(const float distance)
#define MODBUS_SLAVE_ADDRESS
Definition: leddar_one.cpp:64
int start(const char *port=LEDDAR_ONE_DEFAULT_SERIAL_PORT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Definition: leddar_one.cpp:457
hrt_abstime _measurement_time
Definition: leddar_one.cpp:169
virtual int open(file_t *filep)
Handle an open of the device.
Definition: CDev.cpp:141
uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length)
Calculates the 16 byte crc value for the data frame.
Definition: leddar_one.cpp:198
#define MODBUS_READING_FUNCTION
Definition: leddar_one.cpp:65
virtual int init() override
Definition: leddar_one.cpp:276
measure the time elapsed performing an event
Definition: perf_counter.h:56
void set_fov(const float fov)
const char * _serial_port
Definition: leddar_one.cpp:159
#define LEDDAR_ONE_MIN_DISTANCE
Definition: leddar_one.cpp:60
void Run() override
Definition: leddar_one.cpp:414
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
perf_counter_t _comms_error
Definition: leddar_one.cpp:171
Definition: I2C.hpp:51
void print_info()
Diagnostics - print some basic information about the driver.
Definition: leddar_one.cpp:406
void start()
Initialise the automatic measurement state machine and start it.
Definition: leddar_one.cpp:423
static void stop()
Definition: dataman.cpp:1491
LeddarOne * g_dev
Definition: leddar_one.cpp:448
uint8_t _buffer_len
Definition: leddar_one.cpp:167
count the number of times an event occurs
Definition: perf_counter.h:55
int _file_descriptor
Definition: leddar_one.cpp:163
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen)
Perform a read from the device.
Definition: CDev.hpp:111
virtual ~LeddarOne()
Definition: leddar_one.cpp:189
int status()
Print the driver status.
Definition: leddar_one.cpp:489
High-resolution timer with callouts and timekeeping.
virtual int close(file_t *filep)
Handle a close of the device.
Definition: CDev.cpp:166
void set_device_type(uint8_t device_type)
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
uint8_t _buffer[sizeof(reading_msg)]
Definition: leddar_one.cpp:166
void update(const hrt_abstime timestamp, const float distance, const int8_t quality=-1)
void perf_count(perf_counter_t handle)
Count a performance event.
Abstract class for any character device.
Definition: CDev.hpp:58
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
perf_counter_t _sample_perf
Definition: leddar_one.cpp:172
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
#define LEDDAR_ONE_MAX_DISTANCE
Definition: leddar_one.cpp:59
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void set_min_distance(const float distance)
int usage()
Prints info about the driver argument usage.
Definition: leddar_one.cpp:561
uint16_t crc16(const uint8_t *data_p, uint32_t length)
Calculate buffer CRC16.
Definition: sbf.cpp:368
void perf_end(perf_counter_t handle)
End a performance event.
struct __attribute__((__packed__)) reading_msg
Definition: leddar_one.cpp:80
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
virtual ssize_t write(file_t *filep, const char *buffer, size_t buflen)
Perform a write to the device.
Definition: CDev.hpp:123
int stop()
Stop the driver.
Definition: leddar_one.cpp:505
int fd
Definition: dataman.cpp:146
int test(const char *port=LEDDAR_ONE_DEFAULT_SERIAL_PORT)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: leddar_one.cpp:523
int measure()
Sends a data request message to the sensor.
Definition: leddar_one.cpp:312
#define READING_START_ADDR
Definition: leddar_one.cpp:66
static int start()
Definition: dataman.cpp:1452
void stop()
Stop the automatic measurement state machine.
Definition: leddar_one.cpp:431
__EXPORT int leddar_one_main(int argc, char *argv[])
Definition: leddar_one.cpp:593
static const uint8_t request_reading_msg[]
Definition: leddar_one.cpp:69
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define LEDDAR_ONE_MEASURE_INTERVAL
Definition: leddar_one.cpp:62
Definition: bst.cpp:62
#define READING_LEN
Definition: leddar_one.cpp:67
Local functions in support of the shell command.
Definition: leddar_one.cpp:445
PX4Rangefinder _px4_rangefinder
Definition: leddar_one.cpp:161
void set_orientation(const uint8_t device_orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
#define LEDDAR_ONE_DEFAULT_SERIAL_PORT
Definition: leddar_one.cpp:55
#define DEVICE_PATH
Definition: leddar_one.cpp:54
int open_serial_port(const speed_t speed=B115200)
Opens and configures the UART serial communications port.
Definition: leddar_one.cpp:330
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define LEDDAR_ONE_FIELD_OF_VIEW
Definition: leddar_one.cpp:57
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
LeddarOne(const char *device_path, const char *serial_port=LEDDAR_ONE_DEFAULT_SERIAL_PORT, const uint8_t device_orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Definition: leddar_one.cpp:175
int collect()
Reads the data measrurement from serial UART.
Definition: leddar_one.cpp:219
Performance measuring tools.