39 #include <arch/board/board.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> 54 #define DEVICE_PATH "/dev/LeddarOne" 55 #define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3" 57 #define LEDDAR_ONE_FIELD_OF_VIEW (0.105f) // 6 deg cone angle. 59 #define LEDDAR_ONE_MAX_DISTANCE 40.0f 60 #define LEDDAR_ONE_MIN_DISTANCE 0.01f 62 #define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz 64 #define MODBUS_SLAVE_ADDRESS 0x01 65 #define MODBUS_READING_FUNCTION 0x04 66 #define READING_START_ADDR 0x14 67 #define READING_LEN 0xA 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;
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;
112 const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
115 virtual int init()
override;
139 uint16_t crc16_calc(
const unsigned char *data_frame,
const uint8_t crc16_length);
155 int open_serial_port(
const speed_t speed = B115200);
159 const char *_serial_port{
nullptr};
163 int _file_descriptor{-1};
164 int _orb_class_instance{-1};
166 uint8_t _buffer[
sizeof(reading_msg)];
167 uint8_t _buffer_len{0};
177 ScheduledWorkItem(MODULE_NAME,
px4::serial_port_to_wq(serial_port)),
200 uint16_t crc = 0xFFFF;
202 for (uint8_t i = 0; i < crc16_length; i++) {
203 crc ^= data_frame[i];
205 for (uint8_t j = 0; j < 8; j++) {
207 crc = (crc >> 1) ^ 0xA001;
223 const int buffer_size =
sizeof(
_buffer);
224 const int message_size =
sizeof(reading_msg);
228 if (bytes_read < 1) {
240 reading_msg *
msg {
nullptr};
245 PX4_ERR(
"slave address or function read error");
253 if (crc16 !=
msg->crc) {
254 PX4_ERR(
"crc error");
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.0
f;
265 int8_t signal_quality = -1;
279 PX4_ERR(
"Unable to initialize device");
289 const hrt_abstime timeout_usec = time_now + 500000_us;
291 while (time_now < timeout_usec) {
307 PX4_ERR(
"No readings from LeddarOne");
320 PX4_INFO(
"measurement error: %i, errno: %i", num_bytes, errno);
339 int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
345 PX4_ERR(
"open failed (%i)", errno);
349 termios uart_config = {};
353 PX4_ERR(
"Unable to get termios from %s.",
_serial_port);
360 uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB | CRTSCTS);
363 uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
366 uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);
369 uart_config.c_oflag &= ~ONLCR;
372 int termios_state = cfsetispeed(&uart_config, speed);
374 if (termios_state < 0) {
375 PX4_ERR(
"CFG: %d ISPD", termios_state);
381 termios_state = cfsetospeed(&uart_config, speed);
383 if (termios_state < 0) {
384 PX4_ERR(
"CFG: %d OSPD", termios_state);
392 if (termios_state < 0) {
393 PX4_ERR(
"baud %d ATTR", termios_state);
427 PX4_INFO(
"driver started");
451 const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
457 int start(
const char *port,
const uint8_t rotation)
459 if (g_dev !=
nullptr) {
460 PX4_ERR(
"already started");
466 if (g_dev ==
nullptr) {
467 PX4_ERR(
"object instantiate failed");
473 if (g_dev->
init() != PX4_OK) {
474 PX4_ERR(
"driver start failed");
491 if (g_dev ==
nullptr) {
492 PX4_ERR(
"driver not running");
507 if (g_dev !=
nullptr) {
513 PX4_INFO(
"driver stopped");
526 int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
532 PX4_ERR(
"Unable to open %s", port);
539 PX4_INFO(
"serial port write failed: %i, errno: %i", num_bytes, errno);
545 uint8_t buffer[
sizeof(reading_msg)];
547 num_bytes =
::read(fd, buffer,
sizeof(reading_msg));
549 if (num_bytes !=
sizeof(reading_msg)) {
550 PX4_ERR(
"Data not available at %s", port);
563 PRINT_MODULE_DESCRIPTION(
567 Serial bus driver for the LeddarOne LiDAR. 569 Most boards are configured to enable/start the driver on a specified UART using the SENS_LEDDAR1_CFG parameter. 571 Setup/usage information: https://docs.px4.io/en/sensor/leddar_one.html 575 Attempt to start driver on a specified serial device. 576 $ leddar_one start -d /dev/ttyS1 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)");
595 const char *myoptarg =
nullptr;
601 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
603 while ((ch = px4_getopt(argc, argv,
"d:r", &myoptind, &myoptarg)) != EOF) {
610 rotation = (uint8_t)atoi(myoptarg);
614 PX4_WARN(
"Unknown option!");
619 if (myoptind >= argc) {
624 if (!strcmp(argv[myoptind],
"start")) {
629 if (!strcmp(argv[myoptind],
"status")) {
634 if (!strcmp(argv[myoptind],
"stop")) {
639 if (!strcmp(argv[myoptind],
"test")) {
void set_max_distance(const float distance)
#define MODBUS_SLAVE_ADDRESS
int start(const char *port=LEDDAR_ONE_DEFAULT_SERIAL_PORT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
hrt_abstime _measurement_time
virtual int open(file_t *filep)
Handle an open of the device.
uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length)
Calculates the 16 byte crc value for the data frame.
#define MODBUS_READING_FUNCTION
virtual int init() override
measure the time elapsed performing an event
void set_fov(const float fov)
const char * _serial_port
#define LEDDAR_ONE_MIN_DISTANCE
void usage(const char *reason)
Print the correct usage.
perf_counter_t _comms_error
void print_info()
Diagnostics - print some basic information about the driver.
void start()
Initialise the automatic measurement state machine and start it.
count the number of times an event occurs
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen)
Perform a read from the device.
int status()
Print the driver status.
High-resolution timer with callouts and timekeeping.
virtual int close(file_t *filep)
Handle a close of the device.
void set_device_type(uint8_t device_type)
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
uint8_t _buffer[sizeof(reading_msg)]
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.
void perf_free(perf_counter_t handle)
Free a counter.
perf_counter_t _sample_perf
void init()
Activates/configures the hardware registers.
#define LEDDAR_ONE_MAX_DISTANCE
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void set_min_distance(const float distance)
int usage()
Prints info about the driver argument usage.
uint16_t crc16(const uint8_t *data_p, uint32_t length)
Calculate buffer CRC16.
void perf_end(perf_counter_t handle)
End a performance event.
struct __attribute__((__packed__)) reading_msg
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
virtual ssize_t write(file_t *filep, const char *buffer, size_t buflen)
Perform a write to the device.
int stop()
Stop the driver.
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...
int measure()
Sends a data request message to the sensor.
#define READING_START_ADDR
void stop()
Stop the automatic measurement state machine.
__EXPORT int leddar_one_main(int argc, char *argv[])
static const uint8_t request_reading_msg[]
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define LEDDAR_ONE_MEASURE_INTERVAL
Local functions in support of the shell command.
PX4Rangefinder _px4_rangefinder
void set_orientation(const uint8_t device_orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
#define LEDDAR_ONE_DEFAULT_SERIAL_PORT
int open_serial_port(const speed_t speed=B115200)
Opens and configures the UART serial communications port.
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define LEDDAR_ONE_FIELD_OF_VIEW
__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)
int collect()
Reads the data measrurement from serial UART.
Performance measuring tools.