mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:38:52 +08:00
leddar_one: split into separate header, implementation, and main
- remove default serial port
This commit is contained in:
Daniel Agar
@@ -1,6 +1,6 @@
|
|||||||
############################################################################
|
############################################################################
|
||||||
#
|
#
|
||||||
# Copyright (C) 2017 Intel Corporation. All rights reserved.
|
# Copyright (C) 2017-2019 Intel Corporation. All rights reserved.
|
||||||
#
|
#
|
||||||
# Redistribution and use in source and binary forms, with or without
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
@@ -31,12 +31,14 @@
|
|||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE drivers__leddar_one
|
MODULE drivers__distance_sensor__leddar_one
|
||||||
MAIN leddar_one
|
MAIN leddar_one
|
||||||
COMPILE_FLAGS
|
COMPILE_FLAGS
|
||||||
-Wno-cast-align # TODO: fix and enable
|
-Wno-cast-align # TODO: fix and enable
|
||||||
SRCS
|
SRCS
|
||||||
leddar_one.cpp
|
LeddarOne.cpp
|
||||||
|
LeddarOne.hpp
|
||||||
|
leddar_one_main.cpp
|
||||||
MODULE_CONFIG
|
MODULE_CONFIG
|
||||||
module.yaml
|
module.yaml
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -0,0 +1,300 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2017-2019 Intel Corporation. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "LeddarOne.hpp"
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
LeddarOne::LeddarOne(const char *serial_port, uint8_t device_orientation):
|
||||||
|
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)),
|
||||||
|
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation)
|
||||||
|
{
|
||||||
|
_serial_port = strdup(serial_port);
|
||||||
|
|
||||||
|
_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
|
||||||
|
_px4_rangefinder.set_max_distance(LEDDAR_ONE_MAX_DISTANCE);
|
||||||
|
_px4_rangefinder.set_min_distance(LEDDAR_ONE_MIN_DISTANCE);
|
||||||
|
_px4_rangefinder.set_fov(LEDDAR_ONE_FIELD_OF_VIEW);
|
||||||
|
_px4_rangefinder.set_orientation(device_orientation);
|
||||||
|
}
|
||||||
|
|
||||||
|
LeddarOne::~LeddarOne()
|
||||||
|
{
|
||||||
|
stop();
|
||||||
|
|
||||||
|
free((char *)_serial_port);
|
||||||
|
perf_free(_comms_error);
|
||||||
|
perf_free(_sample_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t
|
||||||
|
LeddarOne::crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length)
|
||||||
|
{
|
||||||
|
uint16_t crc = 0xFFFF;
|
||||||
|
|
||||||
|
for (uint8_t i = 0; i < crc16_length; i++) {
|
||||||
|
crc ^= data_frame[i];
|
||||||
|
|
||||||
|
for (uint8_t j = 0; j < 8; j++) {
|
||||||
|
if (crc & 1) {
|
||||||
|
crc = (crc >> 1) ^ 0xA001;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
crc >>= 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LeddarOne::collect()
|
||||||
|
{
|
||||||
|
perf_begin(_sample_perf);
|
||||||
|
|
||||||
|
const int buffer_size = sizeof(_buffer);
|
||||||
|
const int message_size = sizeof(reading_msg);
|
||||||
|
|
||||||
|
int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, buffer_size - _buffer_len);
|
||||||
|
|
||||||
|
if (bytes_read < 1) {
|
||||||
|
// Trigger a new measurement.
|
||||||
|
return measure();
|
||||||
|
}
|
||||||
|
|
||||||
|
_buffer_len += bytes_read;
|
||||||
|
|
||||||
|
if (_buffer_len < message_size) {
|
||||||
|
// Return on next scheduled cycle to collect remaining data.
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
reading_msg *msg {nullptr};
|
||||||
|
msg = (reading_msg *)_buffer;
|
||||||
|
|
||||||
|
if (msg->slave_addr != MODBUS_SLAVE_ADDRESS ||
|
||||||
|
msg->function != MODBUS_READING_FUNCTION) {
|
||||||
|
|
||||||
|
PX4_ERR("slave address or function read error");
|
||||||
|
perf_count(_comms_error);
|
||||||
|
perf_end(_sample_perf);
|
||||||
|
return measure();
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint16_t crc16 = crc16_calc(_buffer, buffer_size - 2);
|
||||||
|
|
||||||
|
if (crc16 != msg->crc) {
|
||||||
|
PX4_ERR("crc error");
|
||||||
|
perf_count(_comms_error);
|
||||||
|
perf_end(_sample_perf);
|
||||||
|
return measure();
|
||||||
|
}
|
||||||
|
|
||||||
|
// NOTE: little-endian support only.
|
||||||
|
uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte);
|
||||||
|
float distance_m = static_cast<float>(distance_mm) / 1000.0f;
|
||||||
|
|
||||||
|
// @TODO - implement a meaningful signal quality value.
|
||||||
|
int8_t signal_quality = -1;
|
||||||
|
|
||||||
|
_px4_rangefinder.update(_measurement_time, distance_m, signal_quality);
|
||||||
|
|
||||||
|
perf_end(_sample_perf);
|
||||||
|
|
||||||
|
// Trigger the next measurement.
|
||||||
|
return measure();
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LeddarOne::init()
|
||||||
|
{
|
||||||
|
if (open_serial_port() != PX4_OK) {
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
hrt_abstime time_now = hrt_absolute_time();
|
||||||
|
|
||||||
|
const hrt_abstime timeout_usec = time_now + 500000_us; // 0.5sec
|
||||||
|
|
||||||
|
while (time_now < timeout_usec) {
|
||||||
|
if (measure() == PX4_OK) {
|
||||||
|
px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL);
|
||||||
|
|
||||||
|
if (collect() == PX4_OK) {
|
||||||
|
// The file descriptor can only be accessed by the process that opened it,
|
||||||
|
// so closing here allows the port to be opened from scheduled work queue.
|
||||||
|
stop();
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
px4_usleep(1000);
|
||||||
|
time_now = hrt_absolute_time();
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("No readings from LeddarOne");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LeddarOne::measure()
|
||||||
|
{
|
||||||
|
// Flush the receive buffer.
|
||||||
|
tcflush(_file_descriptor, TCIFLUSH);
|
||||||
|
|
||||||
|
int num_bytes = ::write(_file_descriptor, request_reading_msg, sizeof(request_reading_msg));
|
||||||
|
|
||||||
|
if (num_bytes != sizeof(request_reading_msg)) {
|
||||||
|
PX4_INFO("measurement error: %i, errno: %i", num_bytes, errno);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
_measurement_time = hrt_absolute_time();
|
||||||
|
_buffer_len = 0;
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LeddarOne::open_serial_port(const speed_t speed)
|
||||||
|
{
|
||||||
|
// File descriptor already initialized?
|
||||||
|
if (_file_descriptor > 0) {
|
||||||
|
// PX4_INFO("serial port already open");
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Configure port flags for read/write, non-controlling, non-blocking.
|
||||||
|
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||||
|
|
||||||
|
// Open the serial port.
|
||||||
|
_file_descriptor = ::open(_serial_port, flags);
|
||||||
|
|
||||||
|
if (_file_descriptor < 0) {
|
||||||
|
PX4_ERR("open failed (%i)", errno);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
termios uart_config = {};
|
||||||
|
|
||||||
|
// Store the current port configuration. attributes.
|
||||||
|
if (tcgetattr(_file_descriptor, &uart_config)) {
|
||||||
|
PX4_ERR("Unable to get termios from %s.", _serial_port);
|
||||||
|
::close(_file_descriptor);
|
||||||
|
_file_descriptor = -1;
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clear: data bit size, two stop bits, parity, hardware flow control.
|
||||||
|
uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB | CRTSCTS);
|
||||||
|
|
||||||
|
// Set: 8 data bits, enable receiver, ignore modem status lines.
|
||||||
|
uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
|
||||||
|
|
||||||
|
// Clear: echo, echo new line, canonical input and extended input.
|
||||||
|
uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);
|
||||||
|
|
||||||
|
// Clear ONLCR flag (which appends a CR for every LF).
|
||||||
|
uart_config.c_oflag &= ~ONLCR;
|
||||||
|
|
||||||
|
// Set the input baud rate in the uart_config struct.
|
||||||
|
int termios_state = cfsetispeed(&uart_config, speed);
|
||||||
|
|
||||||
|
if (termios_state < 0) {
|
||||||
|
PX4_ERR("CFG: %d ISPD", termios_state);
|
||||||
|
::close(_file_descriptor);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the output baud rate in the uart_config struct.
|
||||||
|
termios_state = cfsetospeed(&uart_config, speed);
|
||||||
|
|
||||||
|
if (termios_state < 0) {
|
||||||
|
PX4_ERR("CFG: %d OSPD", termios_state);
|
||||||
|
::close(_file_descriptor);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply the modified port attributes.
|
||||||
|
termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
|
||||||
|
|
||||||
|
if (termios_state < 0) {
|
||||||
|
PX4_ERR("baud %d ATTR", termios_state);
|
||||||
|
::close(_file_descriptor);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Flush the hardware buffers.
|
||||||
|
tcflush(_file_descriptor, TCIOFLUSH);
|
||||||
|
|
||||||
|
PX4_DEBUG("opened UART port %s", _serial_port);
|
||||||
|
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
LeddarOne::print_info()
|
||||||
|
{
|
||||||
|
perf_print_counter(_comms_error);
|
||||||
|
perf_print_counter(_sample_perf);
|
||||||
|
|
||||||
|
_px4_rangefinder.print_status();
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
LeddarOne::Run()
|
||||||
|
{
|
||||||
|
// Ensure the serial port is open.
|
||||||
|
open_serial_port();
|
||||||
|
|
||||||
|
collect();
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
LeddarOne::start()
|
||||||
|
{
|
||||||
|
// Schedule the driver at regular intervals.
|
||||||
|
ScheduleOnInterval(LEDDAR_ONE_MEASURE_INTERVAL, LEDDAR_ONE_MEASURE_INTERVAL);
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
LeddarOne::stop()
|
||||||
|
{
|
||||||
|
// Ensure the serial port is closed.
|
||||||
|
::close(_file_descriptor);
|
||||||
|
_file_descriptor = -1;
|
||||||
|
|
||||||
|
// Clear the work queue schedule.
|
||||||
|
ScheduleClear();
|
||||||
|
}
|
||||||
@@ -0,0 +1,160 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2017-2019 Intel Corporation. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <termios.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
|
||||||
|
#include <lib/perf/perf_counter.h>
|
||||||
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
|
||||||
|
using namespace time_literals;
|
||||||
|
|
||||||
|
#define LEDDAR_ONE_FIELD_OF_VIEW (0.105f) // 6 deg cone angle.
|
||||||
|
|
||||||
|
#define LEDDAR_ONE_MAX_DISTANCE 40.0f
|
||||||
|
#define LEDDAR_ONE_MIN_DISTANCE 0.01f
|
||||||
|
|
||||||
|
#define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz
|
||||||
|
|
||||||
|
#define MODBUS_SLAVE_ADDRESS 0x01
|
||||||
|
#define MODBUS_READING_FUNCTION 0x04
|
||||||
|
#define READING_START_ADDR 0x14
|
||||||
|
#define READING_LEN 0xA
|
||||||
|
|
||||||
|
static const uint8_t request_reading_msg[] = {
|
||||||
|
MODBUS_SLAVE_ADDRESS,
|
||||||
|
MODBUS_READING_FUNCTION,
|
||||||
|
0, /* starting addr high byte */
|
||||||
|
READING_START_ADDR,
|
||||||
|
0, /* number of bytes to read high byte */
|
||||||
|
READING_LEN,
|
||||||
|
0x30, /* CRC low */
|
||||||
|
0x09 /* CRC high */
|
||||||
|
};
|
||||||
|
|
||||||
|
struct __attribute__((__packed__)) reading_msg {
|
||||||
|
uint8_t slave_addr;
|
||||||
|
uint8_t function;
|
||||||
|
uint8_t len;
|
||||||
|
uint8_t low_timestamp_high_byte;
|
||||||
|
uint8_t low_timestamp_low_byte;
|
||||||
|
uint8_t high_timestamp_high_byte;
|
||||||
|
uint8_t high_timestamp_low_byte;
|
||||||
|
uint8_t temp_high;
|
||||||
|
uint8_t temp_low;
|
||||||
|
uint8_t num_detections_high_byte;
|
||||||
|
uint8_t num_detections_low_byte;
|
||||||
|
uint8_t first_dist_high_byte;
|
||||||
|
uint8_t first_dist_low_byte;
|
||||||
|
uint8_t first_amplitude_high_byte;
|
||||||
|
uint8_t first_amplitude_low_byte;
|
||||||
|
uint8_t second_dist_high_byte;
|
||||||
|
uint8_t second_dist_low_byte;
|
||||||
|
uint8_t second_amplitude_high_byte;
|
||||||
|
uint8_t second_amplitude_low_byte;
|
||||||
|
uint8_t third_dist_high_byte;
|
||||||
|
uint8_t third_dist_low_byte;
|
||||||
|
uint8_t third_amplitude_high_byte;
|
||||||
|
uint8_t third_amplitude_low_byte;
|
||||||
|
uint16_t crc; /* little-endian */
|
||||||
|
};
|
||||||
|
|
||||||
|
class LeddarOne : public px4::ScheduledWorkItem
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LeddarOne(const char *serial_port, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||||
|
~LeddarOne() override;
|
||||||
|
|
||||||
|
int init();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Diagnostics - print some basic information about the driver.
|
||||||
|
*/
|
||||||
|
void print_info();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Initialise the automatic measurement state machine and start it.
|
||||||
|
*/
|
||||||
|
void start();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Stop the automatic measurement state machine.
|
||||||
|
*/
|
||||||
|
void stop();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Calculates the 16 byte crc value for the data frame.
|
||||||
|
* @param data_frame The data frame to compute a checksum for.
|
||||||
|
* @param crc16_length The length of the data frame.
|
||||||
|
*/
|
||||||
|
uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Reads the data measrurement from serial UART.
|
||||||
|
*/
|
||||||
|
int collect();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Sends a data request message to the sensor.
|
||||||
|
*/
|
||||||
|
int measure();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Opens and configures the UART serial communications port.
|
||||||
|
* @param speed The baudrate (speed) to configure the serial UART port.
|
||||||
|
*/
|
||||||
|
int open_serial_port(const speed_t speed = B115200);
|
||||||
|
|
||||||
|
void Run() override;
|
||||||
|
|
||||||
|
const char *_serial_port{nullptr};
|
||||||
|
|
||||||
|
PX4Rangefinder _px4_rangefinder;
|
||||||
|
|
||||||
|
int _file_descriptor{-1};
|
||||||
|
|
||||||
|
uint8_t _buffer[sizeof(reading_msg)];
|
||||||
|
uint8_t _buffer_len{0};
|
||||||
|
|
||||||
|
hrt_abstime _measurement_time{0};
|
||||||
|
|
||||||
|
perf_counter_t _comms_error{perf_alloc(PC_COUNT, MODULE_NAME": comms_error")};
|
||||||
|
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": sample")};
|
||||||
|
};
|
||||||
@@ -1,645 +0,0 @@
|
|||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* Copyright (C) 2017 Intel Corporation. All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* 1. Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* 2. Redistributions in binary form must reproduce the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer in
|
|
||||||
* the documentation and/or other materials provided with the
|
|
||||||
* distribution.
|
|
||||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
||||||
* used to endorse or promote products derived from this software
|
|
||||||
* without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
||||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
||||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*
|
|
||||||
****************************************************************************/
|
|
||||||
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <termios.h>
|
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
#include <drivers/device/device.h>
|
|
||||||
#include <drivers/device/ringbuffer.h>
|
|
||||||
#include <drivers/drv_hrt.h>
|
|
||||||
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
|
|
||||||
#include <perf/perf_counter.h>
|
|
||||||
#include <px4_platform_common/px4_config.h>
|
|
||||||
#include <px4_platform_common/defines.h>
|
|
||||||
#include <px4_platform_common/getopt.h>
|
|
||||||
#include <px4_platform_common/module.h>
|
|
||||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
|
||||||
#include <uORB/topics/distance_sensor.h>
|
|
||||||
|
|
||||||
using namespace time_literals;
|
|
||||||
|
|
||||||
#define DEVICE_PATH "/dev/LeddarOne"
|
|
||||||
#define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3"
|
|
||||||
|
|
||||||
#define LEDDAR_ONE_FIELD_OF_VIEW (0.105f) // 6 deg cone angle.
|
|
||||||
|
|
||||||
#define LEDDAR_ONE_MAX_DISTANCE 40.0f
|
|
||||||
#define LEDDAR_ONE_MIN_DISTANCE 0.01f
|
|
||||||
|
|
||||||
#define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz
|
|
||||||
|
|
||||||
#define MODBUS_SLAVE_ADDRESS 0x01
|
|
||||||
#define MODBUS_READING_FUNCTION 0x04
|
|
||||||
#define READING_START_ADDR 0x14
|
|
||||||
#define READING_LEN 0xA
|
|
||||||
|
|
||||||
static const uint8_t request_reading_msg[] = {
|
|
||||||
MODBUS_SLAVE_ADDRESS,
|
|
||||||
MODBUS_READING_FUNCTION,
|
|
||||||
0, /* starting addr high byte */
|
|
||||||
READING_START_ADDR,
|
|
||||||
0, /* number of bytes to read high byte */
|
|
||||||
READING_LEN,
|
|
||||||
0x30, /* CRC low */
|
|
||||||
0x09 /* CRC high */
|
|
||||||
};
|
|
||||||
|
|
||||||
struct __attribute__((__packed__)) reading_msg {
|
|
||||||
uint8_t slave_addr;
|
|
||||||
uint8_t function;
|
|
||||||
uint8_t len;
|
|
||||||
uint8_t low_timestamp_high_byte;
|
|
||||||
uint8_t low_timestamp_low_byte;
|
|
||||||
uint8_t high_timestamp_high_byte;
|
|
||||||
uint8_t high_timestamp_low_byte;
|
|
||||||
uint8_t temp_high;
|
|
||||||
uint8_t temp_low;
|
|
||||||
uint8_t num_detections_high_byte;
|
|
||||||
uint8_t num_detections_low_byte;
|
|
||||||
uint8_t first_dist_high_byte;
|
|
||||||
uint8_t first_dist_low_byte;
|
|
||||||
uint8_t first_amplitude_high_byte;
|
|
||||||
uint8_t first_amplitude_low_byte;
|
|
||||||
uint8_t second_dist_high_byte;
|
|
||||||
uint8_t second_dist_low_byte;
|
|
||||||
uint8_t second_amplitude_high_byte;
|
|
||||||
uint8_t second_amplitude_low_byte;
|
|
||||||
uint8_t third_dist_high_byte;
|
|
||||||
uint8_t third_dist_low_byte;
|
|
||||||
uint8_t third_amplitude_high_byte;
|
|
||||||
uint8_t third_amplitude_low_byte;
|
|
||||||
uint16_t crc; /* little-endian */
|
|
||||||
};
|
|
||||||
|
|
||||||
class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
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);
|
|
||||||
virtual ~LeddarOne();
|
|
||||||
|
|
||||||
virtual int init() override;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Diagnostics - print some basic information about the driver.
|
|
||||||
*/
|
|
||||||
void print_info();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Initialise the automatic measurement state machine and start it.
|
|
||||||
*/
|
|
||||||
void start();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop the automatic measurement state machine.
|
|
||||||
*/
|
|
||||||
void stop();
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Calculates the 16 byte crc value for the data frame.
|
|
||||||
* @param data_frame The data frame to compute a checksum for.
|
|
||||||
* @param crc16_length The length of the data frame.
|
|
||||||
*/
|
|
||||||
uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Reads the data measrurement from serial UART.
|
|
||||||
*/
|
|
||||||
int collect();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sends a data request message to the sensor.
|
|
||||||
*/
|
|
||||||
int measure();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Opens and configures the UART serial communications port.
|
|
||||||
* @param speed The baudrate (speed) to configure the serial UART port.
|
|
||||||
*/
|
|
||||||
int open_serial_port(const speed_t speed = B115200);
|
|
||||||
|
|
||||||
void Run() override;
|
|
||||||
|
|
||||||
const char *_serial_port{nullptr};
|
|
||||||
|
|
||||||
PX4Rangefinder _px4_rangefinder;
|
|
||||||
|
|
||||||
int _file_descriptor{-1};
|
|
||||||
int _orb_class_instance{-1};
|
|
||||||
|
|
||||||
uint8_t _buffer[sizeof(reading_msg)];
|
|
||||||
uint8_t _buffer_len{0};
|
|
||||||
|
|
||||||
hrt_abstime _measurement_time{0};
|
|
||||||
|
|
||||||
perf_counter_t _comms_error{perf_alloc(PC_COUNT, "leddar_one_comms_error")};
|
|
||||||
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "leddar_one_sample")};
|
|
||||||
};
|
|
||||||
|
|
||||||
LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation):
|
|
||||||
CDev(device_path),
|
|
||||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)),
|
|
||||||
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation)
|
|
||||||
{
|
|
||||||
_serial_port = strdup(serial_port);
|
|
||||||
|
|
||||||
_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
|
|
||||||
_px4_rangefinder.set_max_distance(LEDDAR_ONE_MAX_DISTANCE);
|
|
||||||
_px4_rangefinder.set_min_distance(LEDDAR_ONE_MIN_DISTANCE);
|
|
||||||
_px4_rangefinder.set_fov(LEDDAR_ONE_FIELD_OF_VIEW);
|
|
||||||
_px4_rangefinder.set_orientation(device_orientation);
|
|
||||||
}
|
|
||||||
|
|
||||||
LeddarOne::~LeddarOne()
|
|
||||||
{
|
|
||||||
stop();
|
|
||||||
free((char *)_serial_port);
|
|
||||||
perf_free(_comms_error);
|
|
||||||
perf_free(_sample_perf);
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t
|
|
||||||
LeddarOne::crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length)
|
|
||||||
{
|
|
||||||
uint16_t crc = 0xFFFF;
|
|
||||||
|
|
||||||
for (uint8_t i = 0; i < crc16_length; i++) {
|
|
||||||
crc ^= data_frame[i];
|
|
||||||
|
|
||||||
for (uint8_t j = 0; j < 8; j++) {
|
|
||||||
if (crc & 1) {
|
|
||||||
crc = (crc >> 1) ^ 0xA001;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
crc >>= 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return crc;
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
LeddarOne::collect()
|
|
||||||
{
|
|
||||||
perf_begin(_sample_perf);
|
|
||||||
|
|
||||||
const int buffer_size = sizeof(_buffer);
|
|
||||||
const int message_size = sizeof(reading_msg);
|
|
||||||
|
|
||||||
int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, buffer_size - _buffer_len);
|
|
||||||
|
|
||||||
if (bytes_read < 1) {
|
|
||||||
// Trigger a new measurement.
|
|
||||||
return measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
_buffer_len += bytes_read;
|
|
||||||
|
|
||||||
if (_buffer_len < message_size) {
|
|
||||||
// Return on next scheduled cycle to collect remaining data.
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
reading_msg *msg {nullptr};
|
|
||||||
msg = (reading_msg *)_buffer;
|
|
||||||
|
|
||||||
if (msg->slave_addr != MODBUS_SLAVE_ADDRESS ||
|
|
||||||
msg->function != MODBUS_READING_FUNCTION) {
|
|
||||||
PX4_ERR("slave address or function read error");
|
|
||||||
perf_count(_comms_error);
|
|
||||||
perf_end(_sample_perf);
|
|
||||||
return measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
const uint16_t crc16 = crc16_calc(_buffer, buffer_size - 2);
|
|
||||||
|
|
||||||
if (crc16 != msg->crc) {
|
|
||||||
PX4_ERR("crc error");
|
|
||||||
perf_count(_comms_error);
|
|
||||||
perf_end(_sample_perf);
|
|
||||||
return measure();
|
|
||||||
}
|
|
||||||
|
|
||||||
// NOTE: little-endian support only.
|
|
||||||
uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte);
|
|
||||||
float distance_m = static_cast<float>(distance_mm) / 1000.0f;
|
|
||||||
|
|
||||||
// @TODO - implement a meaningful signal quality value.
|
|
||||||
int8_t signal_quality = -1;
|
|
||||||
|
|
||||||
_px4_rangefinder.update(_measurement_time, distance_m, signal_quality);
|
|
||||||
|
|
||||||
perf_end(_sample_perf);
|
|
||||||
|
|
||||||
// Trigger the next measurement.
|
|
||||||
return measure();;
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
LeddarOne::init()
|
|
||||||
{
|
|
||||||
if (CDev::init()) {
|
|
||||||
PX4_ERR("Unable to initialize device");
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (open_serial_port() != PX4_OK) {
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
hrt_abstime time_now = hrt_absolute_time();
|
|
||||||
|
|
||||||
const hrt_abstime timeout_usec = time_now + 500000_us; // 0.5sec
|
|
||||||
|
|
||||||
while (time_now < timeout_usec) {
|
|
||||||
if (measure() == PX4_OK) {
|
|
||||||
px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL);
|
|
||||||
|
|
||||||
if (collect() == PX4_OK) {
|
|
||||||
// The file descriptor can only be accessed by the process that opened it,
|
|
||||||
// so closing here allows the port to be opened from scheduled work queue.
|
|
||||||
stop();
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
px4_usleep(1000);
|
|
||||||
time_now = hrt_absolute_time();
|
|
||||||
}
|
|
||||||
|
|
||||||
PX4_ERR("No readings from LeddarOne");
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
LeddarOne::measure()
|
|
||||||
{
|
|
||||||
// Flush the receive buffer.
|
|
||||||
tcflush(_file_descriptor, TCIFLUSH);
|
|
||||||
|
|
||||||
int num_bytes = ::write(_file_descriptor, request_reading_msg, sizeof(request_reading_msg));
|
|
||||||
|
|
||||||
if (num_bytes != sizeof(request_reading_msg)) {
|
|
||||||
PX4_INFO("measurement error: %i, errno: %i", num_bytes, errno);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
_measurement_time = hrt_absolute_time();
|
|
||||||
_buffer_len = 0;
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
LeddarOne::open_serial_port(const speed_t speed)
|
|
||||||
{
|
|
||||||
// File descriptor already initialized?
|
|
||||||
if (_file_descriptor > 0) {
|
|
||||||
// PX4_INFO("serial port already open");
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Configure port flags for read/write, non-controlling, non-blocking.
|
|
||||||
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
|
|
||||||
|
|
||||||
// Open the serial port.
|
|
||||||
_file_descriptor = ::open(_serial_port, flags);
|
|
||||||
|
|
||||||
if (_file_descriptor < 0) {
|
|
||||||
PX4_ERR("open failed (%i)", errno);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
termios uart_config = {};
|
|
||||||
|
|
||||||
// Store the current port configuration. attributes.
|
|
||||||
if (tcgetattr(_file_descriptor, &uart_config)) {
|
|
||||||
PX4_ERR("Unable to get termios from %s.", _serial_port);
|
|
||||||
::close(_file_descriptor);
|
|
||||||
_file_descriptor = -1;
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Clear: data bit size, two stop bits, parity, hardware flow control.
|
|
||||||
uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB | CRTSCTS);
|
|
||||||
|
|
||||||
// Set: 8 data bits, enable receiver, ignore modem status lines.
|
|
||||||
uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);
|
|
||||||
|
|
||||||
// Clear: echo, echo new line, canonical input and extended input.
|
|
||||||
uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);
|
|
||||||
|
|
||||||
// Clear ONLCR flag (which appends a CR for every LF).
|
|
||||||
uart_config.c_oflag &= ~ONLCR;
|
|
||||||
|
|
||||||
// Set the input baud rate in the uart_config struct.
|
|
||||||
int termios_state = cfsetispeed(&uart_config, speed);
|
|
||||||
|
|
||||||
if (termios_state < 0) {
|
|
||||||
PX4_ERR("CFG: %d ISPD", termios_state);
|
|
||||||
::close(_file_descriptor);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the output baud rate in the uart_config struct.
|
|
||||||
termios_state = cfsetospeed(&uart_config, speed);
|
|
||||||
|
|
||||||
if (termios_state < 0) {
|
|
||||||
PX4_ERR("CFG: %d OSPD", termios_state);
|
|
||||||
::close(_file_descriptor);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Apply the modified port attributes.
|
|
||||||
termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
|
|
||||||
|
|
||||||
if (termios_state < 0) {
|
|
||||||
PX4_ERR("baud %d ATTR", termios_state);
|
|
||||||
::close(_file_descriptor);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Flush the hardware buffers.
|
|
||||||
tcflush(_file_descriptor, TCIOFLUSH);
|
|
||||||
|
|
||||||
PX4_INFO("opened UART port %s", _serial_port);
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
LeddarOne::print_info()
|
|
||||||
{
|
|
||||||
perf_print_counter(_comms_error);
|
|
||||||
perf_print_counter(_sample_perf);
|
|
||||||
PX4_INFO("measure interval: %u msec", static_cast<uint16_t>(LEDDAR_ONE_MEASURE_INTERVAL));
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
LeddarOne::Run()
|
|
||||||
{
|
|
||||||
// Ensure the serial port is open.
|
|
||||||
open_serial_port();
|
|
||||||
|
|
||||||
collect();
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
LeddarOne::start()
|
|
||||||
{
|
|
||||||
// Schedule the driver at regular intervals.
|
|
||||||
ScheduleOnInterval(LEDDAR_ONE_MEASURE_INTERVAL, LEDDAR_ONE_MEASURE_INTERVAL);
|
|
||||||
PX4_INFO("driver started");
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
LeddarOne::stop()
|
|
||||||
{
|
|
||||||
// Ensure the serial port is closed.
|
|
||||||
::close(_file_descriptor);
|
|
||||||
_file_descriptor = -1;
|
|
||||||
|
|
||||||
// Clear the work queue schedule.
|
|
||||||
ScheduleClear();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Local functions in support of the shell command.
|
|
||||||
*/
|
|
||||||
namespace leddar_one
|
|
||||||
{
|
|
||||||
|
|
||||||
LeddarOne *g_dev;
|
|
||||||
|
|
||||||
int start(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT,
|
|
||||||
const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
|
||||||
int status();
|
|
||||||
int stop();
|
|
||||||
int test(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT);
|
|
||||||
int usage();
|
|
||||||
|
|
||||||
int start(const char *port, const uint8_t rotation)
|
|
||||||
{
|
|
||||||
if (g_dev != nullptr) {
|
|
||||||
PX4_ERR("already started");
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
g_dev = new LeddarOne(DEVICE_PATH, port, rotation);
|
|
||||||
|
|
||||||
if (g_dev == nullptr) {
|
|
||||||
PX4_ERR("object instantiate failed");
|
|
||||||
delete g_dev;
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Initialize the sensor.
|
|
||||||
if (g_dev->init() != PX4_OK) {
|
|
||||||
PX4_ERR("driver start failed");
|
|
||||||
delete g_dev;
|
|
||||||
g_dev = nullptr;
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Start the driver.
|
|
||||||
g_dev->start();
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Print the driver status.
|
|
||||||
*/
|
|
||||||
int
|
|
||||||
status()
|
|
||||||
{
|
|
||||||
if (g_dev == nullptr) {
|
|
||||||
PX4_ERR("driver not running");
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
g_dev->print_info();
|
|
||||||
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop the driver
|
|
||||||
*/
|
|
||||||
int
|
|
||||||
stop()
|
|
||||||
{
|
|
||||||
if (g_dev != nullptr) {
|
|
||||||
delete g_dev;
|
|
||||||
g_dev = nullptr;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
PX4_INFO("driver stopped");
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Perform some basic functional tests on the driver;
|
|
||||||
* make sure we can collect data from the sensor in polled
|
|
||||||
* and automatic modes.
|
|
||||||
*/
|
|
||||||
int
|
|
||||||
test(const char *port)
|
|
||||||
{
|
|
||||||
// Configure port flags for read/write, non-controlling, non-blocking.
|
|
||||||
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
|
|
||||||
|
|
||||||
// Open the serial port.
|
|
||||||
int fd = ::open(port, flags);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
PX4_ERR("Unable to open %s", port);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
ssize_t num_bytes = ::write(fd, request_reading_msg, sizeof(request_reading_msg));
|
|
||||||
|
|
||||||
if (num_bytes != sizeof(request_reading_msg)) {
|
|
||||||
PX4_INFO("serial port write failed: %i, errno: %i", num_bytes, errno);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL);
|
|
||||||
|
|
||||||
uint8_t buffer[sizeof(reading_msg)];
|
|
||||||
|
|
||||||
num_bytes = ::read(fd, buffer, sizeof(reading_msg));
|
|
||||||
|
|
||||||
if (num_bytes != sizeof(reading_msg)) {
|
|
||||||
PX4_ERR("Data not available at %s", port);
|
|
||||||
return PX4_ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
PX4_INFO("PASS");
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
usage()
|
|
||||||
{
|
|
||||||
PRINT_MODULE_DESCRIPTION(
|
|
||||||
R"DESCR_STR(
|
|
||||||
### Description
|
|
||||||
|
|
||||||
Serial bus driver for the LeddarOne LiDAR.
|
|
||||||
|
|
||||||
Most boards are configured to enable/start the driver on a specified UART using the SENS_LEDDAR1_CFG parameter.
|
|
||||||
|
|
||||||
Setup/usage information: https://docs.px4.io/en/sensor/leddar_one.html
|
|
||||||
|
|
||||||
### Examples
|
|
||||||
|
|
||||||
Attempt to start driver on a specified serial device.
|
|
||||||
$ leddar_one start -d /dev/ttyS1
|
|
||||||
Stop driver
|
|
||||||
$ leddar_one stop
|
|
||||||
)DESCR_STR");
|
|
||||||
|
|
||||||
PRINT_MODULE_USAGE_NAME("leddar_one", "driver");
|
|
||||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
|
|
||||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
|
||||||
PRINT_MODULE_USAGE_PARAM_INT('r', 25, 1, 25, "Sensor rotation - downward facing by default", true);
|
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
|
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
|
|
||||||
return PX4_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
extern "C" __EXPORT int leddar_one_main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
const char *myoptarg = nullptr;
|
|
||||||
|
|
||||||
int ch = 0;
|
|
||||||
int myoptind = 1;
|
|
||||||
|
|
||||||
const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT;
|
|
||||||
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "d:r", &myoptind, &myoptarg)) != EOF) {
|
|
||||||
switch (ch) {
|
|
||||||
case 'd':
|
|
||||||
port = myoptarg;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'r':
|
|
||||||
rotation = (uint8_t)atoi(myoptarg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
PX4_WARN("Unknown option!");
|
|
||||||
return leddar_one::usage();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (myoptind >= argc) {
|
|
||||||
return leddar_one::usage();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Start/load the driver.
|
|
||||||
if (!strcmp(argv[myoptind], "start")) {
|
|
||||||
return leddar_one::start(port, rotation);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Print the driver status.
|
|
||||||
if (!strcmp(argv[myoptind], "status")) {
|
|
||||||
return leddar_one::status();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Stop the driver.
|
|
||||||
if (!strcmp(argv[myoptind], "stop")) {
|
|
||||||
return leddar_one::stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Test the driver/device.
|
|
||||||
if (!strcmp(argv[myoptind], "test")) {
|
|
||||||
return leddar_one::test(port);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Print driver usage information.
|
|
||||||
return leddar_one::usage();
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,168 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2017-2019 Intel Corporation. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "LeddarOne.hpp"
|
||||||
|
|
||||||
|
#include <px4_platform_common/getopt.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
|
||||||
|
namespace leddar_one
|
||||||
|
{
|
||||||
|
|
||||||
|
LeddarOne *g_dev{nullptr};
|
||||||
|
|
||||||
|
static int start(const char *port, const uint8_t rotation)
|
||||||
|
{
|
||||||
|
if (g_dev != nullptr) {
|
||||||
|
PX4_ERR("already started");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
g_dev = new LeddarOne(port, rotation);
|
||||||
|
|
||||||
|
if (g_dev == nullptr) {
|
||||||
|
PX4_ERR("object instantiate failed");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the sensor.
|
||||||
|
if (g_dev->init() != PX4_OK) {
|
||||||
|
PX4_ERR("driver start failed");
|
||||||
|
delete g_dev;
|
||||||
|
g_dev = nullptr;
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Start the driver.
|
||||||
|
g_dev->start();
|
||||||
|
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int status()
|
||||||
|
{
|
||||||
|
if (g_dev == nullptr) {
|
||||||
|
PX4_ERR("driver not running");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
g_dev->print_info();
|
||||||
|
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int stop()
|
||||||
|
{
|
||||||
|
if (g_dev != nullptr) {
|
||||||
|
delete g_dev;
|
||||||
|
g_dev = nullptr;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_INFO("driver stopped");
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int usage()
|
||||||
|
{
|
||||||
|
PRINT_MODULE_DESCRIPTION(
|
||||||
|
R"DESCR_STR(
|
||||||
|
### Description
|
||||||
|
|
||||||
|
Serial bus driver for the LeddarOne LiDAR.
|
||||||
|
|
||||||
|
Most boards are configured to enable/start the driver on a specified UART using the SENS_LEDDAR1_CFG parameter.
|
||||||
|
|
||||||
|
Setup/usage information: https://docs.px4.io/en/sensor/leddar_one.html
|
||||||
|
|
||||||
|
### Examples
|
||||||
|
|
||||||
|
Attempt to start driver on a specified serial device.
|
||||||
|
$ leddar_one start -d /dev/ttyS1
|
||||||
|
Stop driver
|
||||||
|
$ leddar_one stop
|
||||||
|
)DESCR_STR");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_NAME("leddar_one", "driver");
|
||||||
|
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
||||||
|
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
||||||
|
PRINT_MODULE_USAGE_PARAM_INT('r', 25, 1, 25, "Sensor rotation - downward facing by default", true);
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
extern "C" __EXPORT int leddar_one_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
const char *myoptarg = nullptr;
|
||||||
|
|
||||||
|
int ch = 0;
|
||||||
|
int myoptind = 1;
|
||||||
|
|
||||||
|
const char *port = nullptr;
|
||||||
|
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||||
|
|
||||||
|
while ((ch = px4_getopt(argc, argv, "d:r", &myoptind, &myoptarg)) != EOF) {
|
||||||
|
switch (ch) {
|
||||||
|
case 'd':
|
||||||
|
port = myoptarg;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'r':
|
||||||
|
rotation = (uint8_t)atoi(myoptarg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
PX4_WARN("Unknown option");
|
||||||
|
return leddar_one::usage();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (myoptind >= argc) {
|
||||||
|
return leddar_one::usage();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[myoptind], "start")) {
|
||||||
|
return leddar_one::start(port, rotation);
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[myoptind], "status")) {
|
||||||
|
return leddar_one::status();
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[myoptind], "stop")) {
|
||||||
|
return leddar_one::stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
return leddar_one::usage();
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user