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

sf0x: move to PX4Rangefinder and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-01-03 11:01:48 -05:00
parent d5fb7f47c2
commit 0c9161f004
4 changed files with 119 additions and 556 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/sf0x/SF0X.cpp
+45 -284
View File
@@ -33,26 +33,16 @@
#include "SF0X.hpp"
#include <termios.h>
/* Configuration Constants */
#define SF0X_TAKE_RANGE_REG 'd'
SF0X::SF0X(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_rotation(rotation),
_min_distance(0.30f),
_max_distance(40.0f),
_conversion_interval(83334),
_reports(nullptr),
_measure_interval(0),
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(nullptr),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_com_err"))
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
@@ -60,22 +50,13 @@ SF0X::SF0X(const char *port, uint8_t rotation) :
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
}
SF0X::~SF0X()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
perf_free(_sample_perf);
perf_free(_comms_errors);
}
@@ -83,40 +64,39 @@ SF0X::~SF0X()
int
SF0X::init()
{
int hw_model;
int32_t hw_model = 0;
param_get(param_find("SENS_EN_SF0X"), &hw_model);
switch (hw_model) {
case 1: /* SF02 (40m, 12 Hz)*/
_min_distance = 0.3f;
_max_distance = 40.0f;
_conversion_interval = 83334;
_px4_rangefinder.set_min_distance(0.30f);
_px4_rangefinder.set_max_distance(40.0f);
_interval = 83334;
break;
case 2: /* SF10/a (25m 32Hz) */
_min_distance = 0.01f;
_max_distance = 25.0f;
_conversion_interval = 31250;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(25.0f);
_interval = 31250;
break;
case 3: /* SF10/b (50m 32Hz) */
_min_distance = 0.01f;
_max_distance = 50.0f;
_conversion_interval = 31250;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(50.0f);
_interval = 31250;
break;
case 4: /* SF10/c (100m 16Hz) */
_min_distance = 0.01f;
_max_distance = 100.0f;
_conversion_interval = 62500;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(100.0f);
_interval = 62500;
break;
case 5:
/* SF11/c (120m 20Hz) */
_min_distance = 0.01f;
_max_distance = 120.0f;
_conversion_interval = 50000;
_px4_rangefinder.set_min_distance(0.01f);
_px4_rangefinder.set_max_distance(120.0f);
_interval = 50000;
break;
default:
@@ -124,197 +104,16 @@ SF0X::init()
return -1;
}
/* status */
int ret = 0;
start();
do { /* create a scope to handle exit conditions using break */
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
PX4_ERR("alloc failed");
ret = -1;
break;
}
_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");
}
} while (0);
return ret;
return PX4_OK;
}
void
SF0X::set_minimum_distance(float min)
int SF0X::measure()
{
_min_distance = min;
}
void
SF0X::set_maximum_distance(float max)
{
_max_distance = max;
}
float
SF0X::get_minimum_distance()
{
return _min_distance;
}
float
SF0X::get_maximum_distance()
{
return _max_distance;
}
int
SF0X::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 CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
SF0X::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
SF0X::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
// Send the command to begin a measurement.
char cmd = SF0X_TAKE_RANGE_REG;
ret = ::write(_fd, &cmd, 1);
int ret = ::write(_fd, &cmd, 1);
if (ret != sizeof(cmd)) {
perf_count(_comms_errors);
@@ -322,13 +121,10 @@ SF0X::measure()
return ret;
}
ret = OK;
return ret;
return PX4_OK;
}
int
SF0X::collect()
int SF0X::collect()
{
perf_begin(_sample_perf);
@@ -340,6 +136,7 @@ SF0X::collect()
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
@@ -348,7 +145,7 @@ SF0X::collect()
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
if (read_elapsed > (_interval * 2)) {
return ret;
} else {
@@ -376,52 +173,28 @@ SF0X::collect()
PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
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;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
_px4_rangefinder.update(timestamp_sample, distance_m);
perf_end(_sample_perf);
return ret;
return PX4_OK;
}
void
SF0X::start()
void SF0X::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
}
void
SF0X::stop()
void SF0X::stop()
{
ScheduleClear();
}
void
SF0X::Run()
void SF0X::Run()
{
/* fds initialized? */
if (_fd < 0) {
@@ -447,7 +220,7 @@ SF0X::Run()
uart_config.c_cflag &= ~(CSTOPB | PARENB);
/* if distance sensor model is SF11/C, then set baudrate 115200, else 9600 */
int hw_model;
int32_t hw_model = 0;
param_get(param_find("SENS_EN_SF0X"), &hw_model);
@@ -507,17 +280,6 @@ SF0X::Run()
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_interval > (_conversion_interval)) {
/* schedule a fresh cycle call when we are ready to measure again */
ScheduleDelayed(_measure_interval - _conversion_interval);
return;
}
}
/* measurement phase */
@@ -529,14 +291,13 @@ SF0X::Run()
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_conversion_interval);
ScheduleDelayed(_interval);
}
void
SF0X::print_info()
void SF0X::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %d\n", _measure_interval);
_reports->print_info("report queue");
_px4_rangefinder.print_status();
}