From 04073f2c71238a383d05324d0570cf3e15a856c9 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Wed, 28 Aug 2019 11:21:00 -0600 Subject: [PATCH] Deprecate the LeddarOne request() and publish() methods to match other distance sensor driver classes and fix perf counter errata. --- .../distance_sensor/leddar_one/leddar_one.cpp | 191 +++++++++--------- 1 file changed, 91 insertions(+), 100 deletions(-) diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp index 0e35de6f52..0b87d1b76f 100644 --- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -56,32 +56,32 @@ using namespace time_literals; #define DEVICE_PATH "/dev/LeddarOne" #define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3" -#define MAX_DISTANCE 40.0f -#define MIN_DISTANCE 0.01f +#define MAX_DISTANCE 40.0f +#define MIN_DISTANCE 0.01f -#define SENSOR_READING_FREQ 10.0f -#define READING_USEC_PERIOD (unsigned long)(1000000.0f / SENSOR_READING_FREQ) -#define OVERSAMPLE 6 -#define WORK_USEC_INTERVAL READING_USEC_PERIOD / OVERSAMPLE -#define COLLECT_USEC_TIMEOUT READING_USEC_PERIOD / (OVERSAMPLE / 2) +#define SENSOR_READING_FREQ 10.0f +#define READING_USEC_PERIOD (unsigned long)(1000000.0f / SENSOR_READING_FREQ) +#define OVERSAMPLE 6 -/* 0.5sec */ -#define PROBE_USEC_TIMEOUT 500000_us +#define COLLECT_USEC_TIMEOUT READING_USEC_PERIOD / (OVERSAMPLE / 2) +#define LEDDAR_ONE_MEASURE_INTERVAL READING_USEC_PERIOD / OVERSAMPLE -#define MODBUS_SLAVE_ADDRESS 0x01 -#define MODBUS_READING_FUNCTION 0x04 -#define READING_START_ADDR 0x14 -#define READING_LEN 0xA +#define PROBE_USEC_TIMEOUT 500_ms + +#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 */ + 0, /* starting addr high byte */ READING_START_ADDR, - 0, /* number of bytes to read high byte */ + 0, /* number of bytes to read high byte */ READING_LEN, - 0x30, /* CRC low */ - 0x09 /* CRC high */ + 0x30, /* CRC low */ + 0x09 /* CRC high */ }; struct __attribute__((__packed__)) reading_msg { @@ -156,8 +156,6 @@ private: */ int open_serial_port(const speed_t speed = B115200); - void publish(uint16_t distance_cm); - bool request(); void Run() override; @@ -167,6 +165,7 @@ private: bool _collect_phase{false}; int _file_descriptor{-1}; + int _orb_class_instance{-1}; uint8_t _buffer[sizeof(struct reading_msg)]; uint8_t _buffer_len{0}; @@ -178,7 +177,7 @@ private: perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "leddar_one_comms_errors")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "leddar_one_sample")}; - orb_advert_t _topic{nullptr}; + orb_advert_t _distance_sensor_topic{nullptr}; }; LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t rotation): @@ -195,8 +194,8 @@ LeddarOne::~LeddarOne() free((char *)_serial_port); - if (_topic) { - orb_unadvertise(_topic); + if (_distance_sensor_topic) { + orb_unadvertise(_distance_sensor_topic); } perf_free(_collect_timeout_perf); @@ -232,12 +231,10 @@ LeddarOne::collect() return measure(); } - perf_end(_sample_perf); + perf_begin(_sample_perf); const hrt_abstime time_now = hrt_absolute_time(); - struct reading_msg *msg; - int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, sizeof(_buffer) - _buffer_len); if (bytes_read < 1) { @@ -252,15 +249,18 @@ LeddarOne::collect() _buffer_len += bytes_read; - if (_buffer_len < sizeof(struct reading_msg)) { + if (_buffer_len < sizeof(reading_msg)) { perf_count(_comms_errors); + perf_end(_sample_perf); return PX4_ERROR; } - msg = (struct reading_msg *)_buffer; + reading_msg *msg {nullptr}; + msg = (reading_msg *)_buffer; if (msg->slave_addr != MODBUS_SLAVE_ADDRESS || msg->function != MODBUS_READING_FUNCTION) { perf_count(_comms_errors); + perf_end(_sample_perf); return PX4_ERROR; } @@ -268,11 +268,25 @@ LeddarOne::collect() if (crc16 != msg->crc) { perf_count(_comms_errors); + perf_end(_sample_perf); return PX4_ERROR; } // NOTE: little-endian support only. - publish(msg->first_dist_high_byte << 8 | msg->first_dist_low_byte); + uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte); + + distance_sensor_s report = {}; + report.current_distance = static_cast(distance_mm) / 1000.0f; + report.id = 0; + report.max_distance = MAX_DISTANCE; + report.min_distance = MIN_DISTANCE; + report.orientation = _rotation; + report.signal_quality = -1; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.variance = 0.0f; + + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); _collect_phase = false; _timeout_usec = time_now + READING_USEC_PERIOD; @@ -281,32 +295,6 @@ LeddarOne::collect() return PX4_OK; } -int -LeddarOne::measure() -{ - const hrt_abstime time_now = hrt_absolute_time(); - - if (time_now > _timeout_usec) { - if (request()) { - _buffer_len = 0; - _timeout_usec = time_now + COLLECT_USEC_TIMEOUT; - _collect_phase = true; - return PX4_OK; - } - } - - return PX4_ERROR; -} - -void -LeddarOne::Run() -{ - // Ensure the serial port is open. - open_serial_port(); - - collect(); -} - int LeddarOne::init() { @@ -315,12 +303,21 @@ LeddarOne::init() return -1; } - if (open_serial_port()) { + if (open_serial_port() != PX4_OK) { return PX4_ERROR; } + // Get a publish handle on the range finder topic. + distance_sensor_s ds_report = {}; + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_distance_sensor_topic == nullptr) { + PX4_ERR("failed to create distance_sensor object"); + } + hrt_abstime time_now = hrt_absolute_time(); - hrt_abstime timeout_usec = time_now + PROBE_USEC_TIMEOUT; + const hrt_abstime timeout_usec = time_now + PROBE_USEC_TIMEOUT; while (time_now < timeout_usec) { if (measure() == PX4_OK) { @@ -336,6 +333,29 @@ LeddarOne::init() return PX4_ERROR; } +int +LeddarOne::measure() +{ + const hrt_abstime time_now = hrt_absolute_time(); + + if (time_now > _timeout_usec) { + // Flush anything in RX buffer. + tcflush(_file_descriptor, TCIFLUSH); + + int message_size = sizeof(request_reading_msg); + int ret_val = ::write(_file_descriptor, request_reading_msg, message_size); + + if (ret_val != message_size) { + _buffer_len = 0; + _timeout_usec = time_now + COLLECT_USEC_TIMEOUT; + _collect_phase = true; + return PX4_OK; + } + } + + return PX4_ERROR; +} + int LeddarOne::open_serial_port(const speed_t speed) { @@ -411,67 +431,36 @@ LeddarOne::open_serial_port(const speed_t speed) void LeddarOne::print_info() { - perf_print_counter(_collect_timeout_perf); perf_print_counter(_comms_errors); perf_print_counter(_sample_perf); - PX4_INFO("measure interval: %u msec", static_cast(WORK_USEC_INTERVAL) / 1000); + PX4_INFO("measure interval: %u msec", static_cast(LEDDAR_ONE_MEASURE_INTERVAL) / 1000); } void -LeddarOne::publish(uint16_t distance_mm) +LeddarOne::Run() { - struct distance_sensor_s report; + // Ensure the serial port is open. + open_serial_port(); - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - report.orientation = _rotation; - report.current_distance = ((float)distance_mm / 1000.0f); - report.min_distance = MIN_DISTANCE; - report.max_distance = MAX_DISTANCE; - report.variance = 0.0f; - report.signal_quality = -1; - report.id = 0; - - if (_topic == nullptr) { - _topic = orb_advertise(ORB_ID(distance_sensor), &report); - - } else { - orb_publish(ORB_ID(distance_sensor), _topic, &report); - } -} - -bool -LeddarOne::request() -{ - /* flush anything in RX buffer */ - tcflush(_file_descriptor, TCIFLUSH); - - int r = ::write(_file_descriptor, request_reading_msg, sizeof(request_reading_msg)); - return r == sizeof(request_reading_msg); + collect(); } void LeddarOne::start() { - /* - * file descriptor can only be accessed by the process that opened it - * so closing here and it will be opened from the High priority kernel - * process - */ - ::close(_file_descriptor); - _file_descriptor = -1; - - ScheduleOnInterval(WORK_USEC_INTERVAL); + // Schedule the driver at regular intervals. + ScheduleOnInterval(LEDDAR_ONE_MEASURE_INTERVAL, 0); + PX4_INFO("driver started"); } void LeddarOne::stop() { - if (_file_descriptor > -1) { - ::close(_file_descriptor); - } + // Ensure the serial port is closed. + ::close(_file_descriptor); + // Clear the work queue schedule. ScheduleClear(); } @@ -555,7 +544,8 @@ stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -int test() +int +test() { int fd = open(DEVICE_PATH, O_RDONLY); @@ -580,7 +570,8 @@ int test() return PX4_OK; } -int usage() +int +usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR(