dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ll40ls: first stab at adapting ll40ls to the new distance_sensor msg

This commit is contained in:
Ban Siesta 2015-05-24 15:59:29 +01:00
parent cf39e8c721
commit a729d6f301
5 changed files with 58 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/drivers/ll40ls/LidarLiteI2C.cpp
View File

@ -61,7 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
_sensor_ok(false), _sensor_ok(false),
_collect_phase(false), _collect_phase(false),
_class_instance(-1), _class_instance(-1),
_range_finder_topic(-1), _distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")),
@ -115,7 +115,7 @@ int LidarLiteI2C::init()
} }
/* allocate basic report buffers */ /* 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) { if (_reports == nullptr) {
goto out; goto out;
@ -125,13 +125,13 @@ int LidarLiteI2C::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) { if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
struct range_finder_report rf_report; struct distance_sensor_s ds_report;
measure(); measure();
_reports->get(&rf_report); _reports->get(&ds_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
if (_range_finder_topic < 0) { if (_distance_sensor_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?"); 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) ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct range_finder_report); unsigned count = buflen / sizeof(struct distance_sensor_s);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -400,11 +400,11 @@ int LidarLiteI2C::collect()
return ret; return ret;
} }
uint16_t distance = (val[0] << 8) | val[1]; uint16_t distance_cm = (val[0] << 8) | val[1];
float si_units = distance * 0.01f; /* cm to m */ float distance_m = float(distance_cm) * 1e-2f;
struct range_finder_report report; struct distance_sensor_s report;
if (distance == 0) { if (distance_cm == 0) {
_zero_counter++; _zero_counter++;
if (_zero_counter == 20) { if (_zero_counter == 20) {
@ -425,25 +425,23 @@ int LidarLiteI2C::collect()
_zero_counter = 0; _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 */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors); report.current_distance = distance_m;
report.distance = si_units; report.min_distance = get_minimum_distance();
report.minimum_distance = get_minimum_distance(); report.max_distance = get_maximum_distance();
report.maximum_distance = get_maximum_distance(); report.covariance = 0.0f;
/* the sensor is in fact a laser + sonar but there is no enum for this */
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.valid = 1; report.orientation = 8;
/* TODO: set proper ID */
} else { report.id = 0;
report.valid = 0;
}
/* publish it, if we are the primary */ /* publish it, if we are the primary */
if (_range_finder_topic >= 0) { if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
} }
if (_reports->force(&report)) { if (_reports->force(&report)) {

3
src/drivers/ll40ls/LidarLiteI2C.h
View File

@ -50,6 +50,7 @@
#include <drivers/device/ringbuffer.h> #include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/subsystem_info.h> #include <uORB/topics/subsystem_info.h>
/* Configuration Constants */ /* Configuration Constants */
@ -103,7 +104,7 @@ private:
bool _collect_phase; bool _collect_phase;
int _class_instance; int _class_instance;
orb_advert_t _range_finder_topic; orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;

52
src/drivers/ll40ls/LidarLitePWM.cpp
View File

@ -60,7 +60,7 @@ LidarLitePWM::LidarLitePWM(const char *path) :
_class_instance(-1), _class_instance(-1),
_pwmSub(-1), _pwmSub(-1),
_pwm{}, _pwm{},
_range_finder_topic(-1), _distance_sensor_topic(-1),
_range{}, _range{},
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")),
_read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), _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); unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
} }
// free perf counters /* free perf counters */
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_buffer_overflows); perf_free(_buffer_overflows);
perf_free(_sensor_zero_resets); perf_free(_sensor_zero_resets);
@ -91,7 +91,6 @@ LidarLitePWM::~LidarLitePWM()
int LidarLitePWM::init() int LidarLitePWM::init()
{ {
/* do regular cdev init */ /* do regular cdev init */
int ret = CDev::init(); int ret = CDev::init();
@ -100,7 +99,7 @@ int LidarLitePWM::init()
} }
/* allocate basic report buffers */ /* 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) { if (_reports == nullptr) {
return ERROR; return ERROR;
@ -109,14 +108,14 @@ int LidarLitePWM::init()
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) { if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */ /* get a publish handle on the distance_sensor topic */
struct range_finder_report rf_report; struct distance_sensor_s ds_report;
measure(); measure();
_reports->get(&rf_report); _reports->get(&ds_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report);
if (_range_finder_topic < 0) { if (_distance_sensor_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?"); 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(_buffer_overflows);
perf_print_counter(_sensor_zero_resets); perf_print_counter(_sensor_zero_resets);
warnx("poll interval: %u ticks", getMeasureTicks()); warnx("poll interval: %u ticks", getMeasureTicks());
warnx("distance: %.3fm", (double)_range.distance); warnx("distance: %.3fm", (double)_range.current_distance);
} }
void LidarLitePWM::print_registers() void LidarLitePWM::print_registers()
@ -180,26 +179,25 @@ int LidarLitePWM::measure()
return ERROR; return ERROR;
} }
_range.type = RANGE_FINDER_TYPE_LASER; _range.timestamp = hrt_absolute_time();
_range.error_count = _pwm.error_count; _range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
_range.maximum_distance = get_maximum_distance(); _range.max_distance = get_maximum_distance();
_range.minimum_distance = get_minimum_distance(); _range.min_distance = get_minimum_distance();
_range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */
_range.distance_vector[0] = _range.distance; _range.covariance = 0.0f;
_range.just_updated = 0; _range.orientation = 8;
_range.valid = true; /* 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) /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
if (_range.distance <= 0.0f) { if (_range.current_distance <= 0.0f) {
_range.valid = false;
_range.error_count++;
perf_count(_sensor_zero_resets); perf_count(_sensor_zero_resets);
perf_end(_sample_perf); perf_end(_sample_perf);
return reset_sensor(); return reset_sensor();
} }
if (_range_finder_topic >= 0) { if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_range); orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range);
} }
if (_reports->force(&_range)) { if (_reports->force(&_range)) {
@ -213,8 +211,8 @@ int LidarLitePWM::measure()
ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct range_finder_report); unsigned count = buflen / sizeof(struct distance_sensor_s);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */

5
src/drivers/ll40ls/LidarLitePWM.h
View File

@ -53,6 +53,7 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h> #include <uORB/topics/pwm_input.h>
#include <uORB/topics/distance_sensor.h>
@ -109,8 +110,8 @@ private:
int _class_instance; int _class_instance;
int _pwmSub; int _pwmSub;
struct pwm_input_s _pwm; struct pwm_input_s _pwm;
orb_advert_t _range_finder_topic; orb_advert_t _distance_sensor_topic;
range_finder_report _range; struct distance_sensor_s _range;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _read_errors; perf_counter_t _read_errors;

6
src/drivers/ll40ls/ll40ls.cpp
View File

@ -239,7 +239,7 @@ void stop(const bool use_i2c, const int bus)
void void
test(const bool use_i2c, const int bus) test(const bool use_i2c, const int bus)
{ {
struct range_finder_report report; struct distance_sensor_s report;
ssize_t sz; ssize_t sz;
int ret; int ret;
@ -266,7 +266,7 @@ test(const bool use_i2c, const int bus)
} }
warnx("single read"); 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); warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */ /* start the sensor polling at 2Hz */
@ -295,7 +295,7 @@ test(const bool use_i2c, const int bus)
} }
warnx("periodic read %u", i); 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); warnx("time: %lld", report.timestamp);
} }