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

sf1xx: move to PX4Rangerfinder and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-04-15 23:33:41 -04:00
parent f51715c7ab
commit 9f7c3b9f06
1 changed files with 41 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/sf1xx/sf1xx.cpp
+41 -313
View File
@@ -46,32 +46,11 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <lib/parameters/param.h>
#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
#include <drivers/rangefinder/PX4Rangefinder.hpp>
/* Configuration Constants */
#define SF1XX_BASEADDR 0x66
@@ -82,7 +61,7 @@ public:
SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency,
int address = SF1XX_BASEADDR);
virtual ~SF1XX() override;
~SF1XX() override;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
@@ -90,9 +69,6 @@ public:
int init() override;
ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
*/
@@ -104,10 +80,9 @@ public:
*/
void RunImpl();
protected:
private:
int probe() override;
private:
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -125,76 +100,38 @@ private:
*/
void start();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults SF1XX_MIN_DISTANCE
* and SF1XX_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
int measure();
int collect();
PX4Rangefinder _px4_rangefinder;
bool _sensor_ok{false};
int _class_instance{-1};
int _conversion_interval{-1};
int _measure_interval{0};
int _orb_class_instance{-1};
float _max_distance{-1.0f};
float _min_distance{-1.0f};
uint8_t _rotation{0};
ringbuffer::RingBuffer *_reports{nullptr};
orb_advert_t _distance_sensor_topic{nullptr};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "sf1xx_read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "sf1xx_com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]);
SF1XX::SF1XX(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
I2C(DRV_DIST_DEVTYPE_SF1XX, MODULE_NAME, bus, address, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_rotation(rotation)
_px4_rangefinder(DRV_DIST_DEVTYPE_SF1XX, rotation)
{
}
SF1XX::~SF1XX()
{
/* 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);
}
int
SF1XX::init()
int SF1XX::init()
{
int ret = PX4_ERROR;
int hw_model;
int32_t hw_model = 0;
param_get(param_find("SENS_EN_SF1XX"), &hw_model);
switch (hw_model) {
@@ -203,41 +140,41 @@ SF1XX::init()
return ret;
case 1: /* SF10/a (25m 32Hz) */
_min_distance = 0.01f;
_max_distance = 25.0f;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(25.0f);
_conversion_interval = 31250;
break;
case 2: /* SF10/b (50m 32Hz) */
_min_distance = 0.01f;
_max_distance = 50.0f;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(50.0f);
_conversion_interval = 31250;
break;
case 3: /* SF10/c (100m 16Hz) */
_min_distance = 0.01f;
_max_distance = 100.0f;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(100.0f);
_conversion_interval = 62500;
break;
case 4:
/* SF11/c (120m 20Hz) */
_min_distance = 0.01f;
_max_distance = 120.0f;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(120.0f);
_conversion_interval = 50000;
break;
case 5:
/* SF/LW20/b (50m 48-388Hz) */
_min_distance = 0.001f;
_max_distance = 50.0f;
_px4_rangefinder.set_min_distance(0.001f);
_px4_rangefinder.set_max_distance(50.0f);
_conversion_interval = 20834;
break;
case 6:
/* SF/LW20/c (100m 48-388Hz) */
_min_distance = 0.001f;
_max_distance = 100.0f;
_px4_rangefinder.set_min_distance(0.001f);
_px4_rangefinder.set_max_distance(100.0f);
_conversion_interval = 20834;
break;
@@ -251,202 +188,29 @@ SF1XX::init()
return ret;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
set_device_address(SF1XX_BASEADDR);
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 = {};
_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");
}
// Select altitude register
int ret2 = measure();
if (ret2 == 0) {
ret = OK;
_sensor_ok = true;
PX4_INFO("(%dm %dHz) with address %d found", (int)_max_distance,
(int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
}
return ret;
}
int
SF1XX::probe()
int SF1XX::probe()
{
return measure();
}
void
SF1XX::set_minimum_distance(float min)
int SF1XX::measure()
{
_min_distance = min;
}
void
SF1XX::set_maximum_distance(float max)
{
_max_distance = max;
}
float
SF1XX::get_minimum_distance()
{
return _min_distance;
}
float
SF1XX::get_maximum_distance()
{
return _max_distance;
}
int
SF1XX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = (_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
int interval = (1000000 / arg);
/* check against maximum rate */
if (interval < _conversion_interval) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
SF1XX::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 (_measure_interval > 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(_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
SF1XX::measure()
{
int ret;
/*
* Send the command '0' -- read altitude
*/
uint8_t cmd = 0;
ret = transfer(&cmd, 1, nullptr, 0);
int ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
@@ -459,65 +223,35 @@ SF1XX::measure()
return ret;
}
int
SF1XX::collect()
int SF1XX::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
uint8_t val[2] {};
const hrt_abstime timestamp_sample = hrt_absolute_time();
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
PX4_DEBUG("error reading from sensor: %d", ret);
if (transfer(nullptr, 0, &val[0], 2) < 0) {
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
return PX4_ERROR;
}
perf_end(_sample_perf);
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.variance = 0.0f;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;
_px4_rangefinder.update(timestamp_sample, distance_m);
/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
return PX4_OK;
}
void
SF1XX::start()
void SF1XX::start()
{
if (_measure_interval == 0) {
_measure_interval = _conversion_interval;
}
/* reset the report ring and state machine */
_reports->flush();
/* set register to '0' */
measure();
@@ -525,8 +259,7 @@ SF1XX::start()
ScheduleDelayed(_conversion_interval);
}
void
SF1XX::RunImpl()
void SF1XX::RunImpl()
{
/* Collect results */
if (OK != collect()) {
@@ -540,18 +273,15 @@ SF1XX::RunImpl()
ScheduleDelayed(_conversion_interval);
}
void
SF1XX::print_status()
void SF1XX::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u\n", _measure_interval);
_reports->print_info("report queue");
_px4_rangefinder.print_status();
}
void
SF1XX::print_usage()
void SF1XX::print_usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
@@ -589,9 +319,7 @@ I2CSPIDriverBase *SF1XX::instantiate(const BusCLIArguments &cli, const BusInstan
return instance;
}
int
sf1xx_main(int argc, char *argv[])
extern "C" __EXPORT int sf1xx_main(int argc, char *argv[])
{
int ch;
using ThisDriver = SF1XX;