From a729d6f3014b5fedb2458d6e63d08d06ffd6d44d Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 15:59:29 +0100 Subject: [PATCH] ll40ls: first stab at adapting ll40ls to the new distance_sensor msg --- src/drivers/ll40ls/LidarLiteI2C.cpp | 52 ++++++++++++++--------------- src/drivers/ll40ls/LidarLiteI2C.h | 3 +- src/drivers/ll40ls/LidarLitePWM.cpp | 52 ++++++++++++++--------------- src/drivers/ll40ls/LidarLitePWM.h | 5 +-- src/drivers/ll40ls/ll40ls.cpp | 6 ++-- 5 files changed, 58 insertions(+), 60 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index e2195ae029..4a2922f7df 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -61,7 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _sensor_ok(false), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), @@ -115,7 +115,7 @@ int LidarLiteI2C::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -125,13 +125,13 @@ int LidarLiteI2C::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -229,8 +229,8 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -400,11 +400,11 @@ int LidarLiteI2C::collect() return ret; } - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + uint16_t distance_cm = (val[0] << 8) | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + struct distance_sensor_s report; - if (distance == 0) { + if (distance_cm == 0) { _zero_counter++; if (_zero_counter == 20) { @@ -425,25 +425,23 @@ int LidarLiteI2C::collect() _zero_counter = 0; } - _last_distance = distance; + _last_distance = distance_m; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - - } else { - report.valid = 0; - } + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* the sensor is in fact a laser + sonar but there is no enum for this */ + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.orientation = 8; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index cd80b162cb..9f58475b98 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -50,6 +50,7 @@ #include #include +#include #include /* Configuration Constants */ @@ -103,7 +104,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 2ebdc6fd7e..2adb9e8ed3 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -60,7 +60,7 @@ LidarLitePWM::LidarLitePWM(const char *path) : _class_instance(-1), _pwmSub(-1), _pwm{}, - _range_finder_topic(-1), + _distance_sensor_topic(-1), _range{}, _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), _read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), @@ -82,7 +82,7 @@ LidarLitePWM::~LidarLitePWM() unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_buffer_overflows); perf_free(_sensor_zero_resets); @@ -91,7 +91,6 @@ LidarLitePWM::~LidarLitePWM() int LidarLitePWM::init() { - /* do regular cdev init */ int ret = CDev::init(); @@ -100,7 +99,7 @@ int LidarLitePWM::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { return ERROR; @@ -109,14 +108,14 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + /* get a publish handle on the distance_sensor topic */ + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -130,7 +129,7 @@ void LidarLitePWM::print_info() perf_print_counter(_buffer_overflows); perf_print_counter(_sensor_zero_resets); warnx("poll interval: %u ticks", getMeasureTicks()); - warnx("distance: %.3fm", (double)_range.distance); + warnx("distance: %.3fm", (double)_range.current_distance); } void LidarLitePWM::print_registers() @@ -180,26 +179,25 @@ int LidarLitePWM::measure() return ERROR; } - _range.type = RANGE_FINDER_TYPE_LASER; - _range.error_count = _pwm.error_count; - _range.maximum_distance = get_maximum_distance(); - _range.minimum_distance = get_minimum_distance(); - _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite - _range.distance_vector[0] = _range.distance; - _range.just_updated = 0; - _range.valid = true; + _range.timestamp = hrt_absolute_time(); + _range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _range.max_distance = get_maximum_distance(); + _range.min_distance = get_minimum_distance(); + _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + _range.covariance = 0.0f; + _range.orientation = 8; + /* TODO: set proper ID */ + _range.id = 0; - // Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) - if (_range.distance <= 0.0f) { - _range.valid = false; - _range.error_count++; + /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ + if (_range.current_distance <= 0.0f) { perf_count(_sensor_zero_resets); perf_end(_sample_perf); return reset_sensor(); } - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_range); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range); } if (_reports->force(&_range)) { @@ -213,8 +211,8 @@ int LidarLitePWM::measure() ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index d2cc7f36e4..93cb80e29a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -53,6 +53,7 @@ #include #include +#include @@ -109,8 +110,8 @@ private: int _class_instance; int _pwmSub; struct pwm_input_s _pwm; - orb_advert_t _range_finder_topic; - range_finder_report _range; + orb_advert_t _distance_sensor_topic; + struct distance_sensor_s _range; perf_counter_t _sample_perf; perf_counter_t _read_errors; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 9c52ce6e9a..8984220625 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -239,7 +239,7 @@ void stop(const bool use_i2c, const int bus) void test(const bool use_i2c, const int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -266,7 +266,7 @@ test(const bool use_i2c, const int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -295,7 +295,7 @@ test(const bool use_i2c, const int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f m", (double)report.distance); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); }