fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 09:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: cleanup and create PX4Rangerfinder helper class (#12567)

Browse Source
This commit is contained in:
Daniel Agar
2019-07-31 15:17:35 -04:00
committed by GitHub
parent e9397eeb75
commit 29c50da1f6
13 changed files with 392 additions and 744 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+52 -223
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,37 +40,15 @@
*/
#include "LidarLiteI2C.h"
#include <px4_defines.h>
#include <semaphore.h>
#include <fcntl.h>
#include <errno.h>
#include <poll.h>
#include <string.h>
#include <stdio.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) :
I2C("LL40LS", path, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_rotation(rotation),
_reports(nullptr),
_sensor_ok(false),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
_sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")),
_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")),
_last_distance(0),
_zero_counter(0),
_acquire_time_usec(0),
_pause_measurements(false),
_hw_version(0),
_sw_version(0),
_unit_id(0)
LidarLiteI2C::LidarLiteI2C(int bus, uint8_t rotation, int address) :
LidarLite(rotation),
I2C("LL40LS", nullptr, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
{
// up the retries since the device misses the first measure attempts
_retries = 3;
@@ -78,30 +56,11 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr
LidarLiteI2C::~LidarLiteI2C()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_distance_sensor_topic != nullptr) {
orb_unadvertise(_distance_sensor_topic);
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_sensor_resets);
perf_free(_sensor_zero_resets);
}
int LidarLiteI2C::init()
int
LidarLiteI2C::init()
{
int ret = PX4_ERROR;
@@ -110,39 +69,22 @@ int LidarLiteI2C::init()
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return ret;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic == nullptr) {
PX4_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
return ret;
start();
return OK;
}
int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
int
LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val)
{
return lidar_transfer(&reg, 1, &val, 1);
}
int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
int
LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
{
const uint8_t cmd[2] = { reg, val };
return transfer(&cmd[0], 2, nullptr, 0);
@@ -152,7 +94,8 @@ int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val)
LidarLite specific transfer() function that avoids a stop condition
with SCL low
*/
int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
int
LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
if (send != nullptr && send_len > 0) {
int ret = transfer(send, send_len, nullptr, 0);
@@ -169,12 +112,14 @@ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t
return OK;
}
int LidarLiteI2C::probe()
int
LidarLiteI2C::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
const uint8_t addresses[2] = { LL40LS_BASEADDR, LL40LS_BASEADDR_OLD };
uint8_t id_high = 0, id_low = 0;
uint8_t id_high = 0;
uint8_t id_low = 0;
// more retries for detection
_retries = 10;
@@ -195,8 +140,7 @@ int LidarLiteI2C::probe()
goto ok;
}
PX4_DEBUG("probe failed unit_id=0x%02x\n",
(unsigned)_unit_id);
PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id);
} else {
/*
@@ -205,13 +149,12 @@ int LidarLiteI2C::probe()
*/
if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 &&
read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) {
set_maximum_distance(LL40LS_MAX_DISTANCE_V1);
_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V1);
goto ok;
}
PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n",
(unsigned)_hw_version,
(unsigned)_sw_version);
PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version);
}
}
@@ -224,84 +167,9 @@ ok:
return reset_sensor();
}
int LidarLiteI2C::ioctl(device::file_t *filp, int cmd, unsigned long arg)
int
LidarLiteI2C::measure()
{
switch (cmd) {
default: {
int result = LidarLite::ioctl(filp, cmd, arg);
if (result == -EINVAL) {
result = I2C::ioctl(filp, cmd, arg);
}
return result;
}
}
}
ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (getMeasureInterval() > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
px4_usleep(LL40LS_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int LidarLiteI2C::measure()
{
int ret;
if (_pause_measurements) {
// we are in print_registers() and need to avoid
// acquisition to keep the I2C peripheral on the
@@ -313,7 +181,7 @@ int LidarLiteI2C::measure()
* Send the command to begin a measurement.
*/
const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
int ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
@@ -332,18 +200,17 @@ int LidarLiteI2C::measure()
// remember when we sent the acquire so we can know when the
// acquisition has timed out
_acquire_time_usec = hrt_absolute_time();
ret = OK;
return ret;
return OK;
}
/*
reset the sensor to power on defaults plus additional configurations
*/
int LidarLiteI2C::reset_sensor()
int
LidarLiteI2C::reset_sensor()
{
int ret;
ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET);
if (ret != OK) {
return ret;
@@ -369,7 +236,7 @@ int LidarLiteI2C::reset_sensor()
void LidarLiteI2C::print_registers()
{
_pause_measurements = true;
printf("ll40ls registers\n");
PX4_INFO("registers");
// wait for a while to ensure the lidar is in a ready state
px4_usleep(50000);
@@ -395,16 +262,14 @@ void LidarLiteI2C::print_registers()
int LidarLiteI2C::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
uint8_t val[2] {};
perf_begin(_sample_perf);
// read the high and low byte distance registers
uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT;
ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val));
// if the transfer failed or if the high bit of distance is
// set then the distance is invalid
@@ -431,8 +296,7 @@ int LidarLiteI2C::collect()
}
uint16_t distance_cm = (val[0] << 8) | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
const float distance_m = float(distance_cm) * 1e-2f;
if (distance_cm == 0) {
_zero_counter++;
@@ -455,7 +319,8 @@ int LidarLiteI2C::collect()
_zero_counter = 0;
}
_last_distance = distance_cm;
// this should be fairly close to the end of the measurement, so the best approximation of the time
const hrt_abstime timestamp_sample = hrt_absolute_time();
/* Relative signal strength measurement, i.e. the strength of
* the main signal peak compared to the general noise level*/
@@ -464,7 +329,7 @@ int LidarLiteI2C::collect()
// check if the transfer failed
if (ret < 0) {
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
read before it is ready, so only consider it
@@ -496,7 +361,7 @@ int LidarLiteI2C::collect()
// check if the transfer failed
if (ret < 0) {
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) {
if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) {
/*
NACKs from the sensor are expected when we
read before it is ready, so only consider it
@@ -527,44 +392,25 @@ int LidarLiteI2C::collect()
0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
// Step 2: Also use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { signal_quality = 0; }
// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
if (distance_m < LL40LS_MIN_DISTANCE) { signal_quality = 0; }
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.variance = 0.0f;
report.signal_quality = signal_quality;
report.type =
distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; // the sensor is in fact a laser + sonar but there is no enum for this
report.orientation = _rotation;
report.id = 0; // TODO: set proper ID
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) {
signal_quality = 0;
}
_reports->force(&report);
// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
if (distance_m < LL40LS_MIN_DISTANCE) {
signal_quality = 0;
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
_px4_rangefinder.update(timestamp_sample, distance_m, signal_quality);
perf_end(_sample_perf);
return ret;
return OK;
}
void LidarLiteI2C::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
@@ -586,7 +432,7 @@ void LidarLiteI2C::Run()
/* if we've been waiting more than 200ms then
send a new acquire */
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {
if (hrt_elapsed_time(&_acquire_time_usec) > (LL40LS_CONVERSION_TIMEOUT * 2)) {
_collect_phase = false;
}
@@ -597,10 +443,10 @@ void LidarLiteI2C::Run()
/*
* Is there a collect->measure gap?
*/
if (getMeasureInterval() > (LL40LS_CONVERSION_INTERVAL)) {
if (get_measure_interval() > LL40LS_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(getMeasureInterval() - LL40LS_CONVERSION_INTERVAL);
ScheduleDelayed(get_measure_interval() - LL40LS_CONVERSION_INTERVAL);
return;
}
@@ -623,20 +469,3 @@ void LidarLiteI2C::Run()
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(LL40LS_CONVERSION_INTERVAL);
}
void LidarLiteI2C::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);
printf("poll interval: %u \n", getMeasureInterval());
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
const char *LidarLiteI2C::get_dev_name()
{
return get_devname();
}