mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
sf0x: move to PX4Rangefinder and cleanup
This commit is contained in:
Daniel Agar
parent
d5fb7f47c2
commit
0c9161f004
@ -40,6 +40,9 @@ px4_add_module(
|
||||
SF0X.hpp
|
||||
sf0x_main.cpp
|
||||
sf0x_parser.cpp
|
||||
DEPENDS
|
||||
drivers_rangefinder
|
||||
px4_work_queue
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -43,111 +43,47 @@
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#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 <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <lib/parameters/param.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include "sf0x_parser.h"
|
||||
|
||||
/* Configuration Constants */
|
||||
|
||||
#define SF0X_TAKE_RANGE_REG 'd'
|
||||
|
||||
// designated SERIAL4/5 on Pixhawk
|
||||
#define SF0X_DEFAULT_PORT "/dev/ttyS6"
|
||||
|
||||
class SF0X : public cdev::CDev, public px4::ScheduledWorkItem
|
||||
class SF0X : public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
virtual ~SF0X();
|
||||
SF0X(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
|
||||
~SF0X() override;
|
||||
|
||||
virtual int init() override;
|
||||
|
||||
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
int init();
|
||||
void print_info();
|
||||
|
||||
private:
|
||||
char _port[20];
|
||||
uint8_t _rotation;
|
||||
float _min_distance;
|
||||
float _max_distance;
|
||||
int _conversion_interval;
|
||||
ringbuffer::RingBuffer *_reports;
|
||||
int _measure_interval;
|
||||
bool _collect_phase;
|
||||
int _fd;
|
||||
char _linebuf[10];
|
||||
unsigned _linebuf_index;
|
||||
enum SF0X_PARSE_STATE _parse_state;
|
||||
hrt_abstime _last_read;
|
||||
|
||||
int _class_instance;
|
||||
int _orb_class_instance;
|
||||
void start();
|
||||
void stop();
|
||||
void Run() override;
|
||||
int measure();
|
||||
int collect();
|
||||
|
||||
orb_advert_t _distance_sensor_topic;
|
||||
|
||||
PX4Rangefinder _px4_rangefinder;
|
||||
|
||||
char _port[20] {};
|
||||
int _interval{100000};
|
||||
bool _collect_phase{false};
|
||||
int _fd{-1};
|
||||
char _linebuf[10] {};
|
||||
unsigned _linebuf_index{0};
|
||||
enum SF0X_PARSE_STATE _parse_state {SF0X_PARSE_STATE0_UNSYNC};
|
||||
hrt_abstime _last_read{0};
|
||||
|
||||
unsigned _consecutive_fail_count;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* 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 SF0X_MIN_DISTANCE
|
||||
* and SF0X_MAX_DISTANCE
|
||||
*/
|
||||
void set_minimum_distance(float min);
|
||||
void set_maximum_distance(float max);
|
||||
float get_minimum_distance();
|
||||
float get_maximum_distance();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void Run() override;
|
||||
int measure();
|
||||
int collect();
|
||||
|
||||
};
|
||||
|
||||
@ -34,34 +34,15 @@
|
||||
#include "SF0X.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace sf0x
|
||||
{
|
||||
|
||||
SF0X *g_dev;
|
||||
SF0X *g_dev{nullptr};
|
||||
|
||||
int start(const char *port, uint8_t rotation);
|
||||
int stop();
|
||||
int test();
|
||||
int reset();
|
||||
int info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
int
|
||||
start(const char *port, uint8_t rotation)
|
||||
static int start(const char *port, uint8_t rotation)
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
PX4_WARN("already started");
|
||||
return -1;
|
||||
@ -71,41 +52,19 @@ start(const char *port, uint8_t rotation)
|
||||
g_dev = new SF0X(port, rotation);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(RANGE_FINDER0_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("device open fail (%i)", errno);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
if (g_dev->init() != PX4_OK) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
int stop()
|
||||
static int stop()
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
@ -118,124 +77,53 @@ int stop()
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform some basic functional tests on the driver;
|
||||
* make sure we can collect data from the sensor in polled
|
||||
* and automatic modes.
|
||||
*/
|
||||
int
|
||||
test()
|
||||
{
|
||||
struct distance_sensor_s report;
|
||||
ssize_t sz;
|
||||
|
||||
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_ERR("immediate read failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
print_message(report);
|
||||
|
||||
/* start the sensor polling at 2 Hz rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
PX4_ERR("failed to set 2Hz poll rate");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
int ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1) {
|
||||
PX4_ERR("timed out");
|
||||
break;
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report)) {
|
||||
PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report));
|
||||
break;
|
||||
}
|
||||
|
||||
print_message(report);
|
||||
}
|
||||
|
||||
/* reset the sensor polling to the default rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
|
||||
PX4_ERR("ioctl SENSORIOCSPOLLRATE failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
int
|
||||
reset()
|
||||
{
|
||||
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
PX4_ERR("open failed (%i)", errno);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
PX4_ERR("driver reset failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
PX4_ERR("driver poll restart failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
int
|
||||
info()
|
||||
static int status()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("driver not running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
Serial bus driver for the LightWare SF02/F, SF10/a, SF10/b, SF10/c, SF11/c Laser rangefinders.
|
||||
|
||||
Most boards are configured to enable/start the driver on a specified UART using the SENS_SF0X_CFG parameter.
|
||||
|
||||
Setup/usage information: https://docs.px4.io/en/sensor/sfxx_lidar.html
|
||||
|
||||
### Examples
|
||||
|
||||
Attempt to start driver on a specified serial device.
|
||||
$ sf0x start -d /dev/ttyS1
|
||||
Stop driver
|
||||
$ sf0x stop
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("sf0x", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
sf0x_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int sf0x_main(int argc, char *argv[])
|
||||
{
|
||||
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
|
||||
const char *device_path = SF0X_DEFAULT_PORT;
|
||||
const char *device_path = nullptr;
|
||||
int ch;
|
||||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
@ -251,51 +139,26 @@ sf0x_main(int argc, char *argv[])
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("Unknown option!");
|
||||
sf0x::usage();
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (myoptind >= argc) {
|
||||
goto out_error;
|
||||
sf0x::usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[myoptind], "start")) {
|
||||
return sf0x::start(device_path, rotation);
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[myoptind], "stop")) {
|
||||
} else if (!strcmp(argv[myoptind], "stop")) {
|
||||
return sf0x::stop();
|
||||
|
||||
} else if (!strcmp(argv[myoptind], "status")) {
|
||||
return sf0x::status();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[myoptind], "test")) {
|
||||
return sf0x::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[myoptind], "reset")) {
|
||||
return sf0x::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
|
||||
return sf0x::info();
|
||||
}
|
||||
|
||||
out_error:
|
||||
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
sf0x::usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user