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

sf0x: split into separate header, implementation, and main

Browse Source
This commit is contained in:
Daniel Agar 2019-12-29 12:02:20 -05:00
parent 156cf16265
commit d5fb7f47c2
4 changed files with 459 additions and 386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/distance_sensor/sf0x/CMakeLists.txt
View File

@ -36,7 +36,9 @@ px4_add_module(
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
sf0x.cpp
SF0X.cpp
SF0X.hpp
sf0x_main.cpp
sf0x_parser.cpp
MODULE_CONFIG
module.yaml

387
src/drivers/distance_sensor/sf0x/sf0x.cpp → src/drivers/distance_sensor/sf0x/SF0X.cpp
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
@ -31,130 +31,7 @@
*
****************************************************************************/
/**
* @file sf0x.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Greg Hulands
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.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 <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 "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
{
public:
SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~SF0X();
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.
*/
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;
orb_advert_t _distance_sensor_topic;
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();
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
#include "SF0X.hpp"
SF0X::SF0X(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
@ -663,263 +540,3 @@ SF0X::print_info()
printf("poll interval: %d\n", _measure_interval);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace sf0x
{
SF0X *g_dev;
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)
{
int fd;
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
}
/* create the driver */
g_dev = new SF0X(port, rotation);
if (g_dev == nullptr) {
goto fail;
}
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;
}
return 0;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return -1;
}
/**
* Stop the driver
*/
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
return -1;
}
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()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return 0;
}
} // namespace
int
sf0x_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = SF0X_DEFAULT_PORT;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
PX4_WARN("Unknown option!");
return -1;
}
}
if (myoptind >= argc) {
goto out_error;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
return sf0x::start(device_path, rotation);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return sf0x::stop();
}
/*
* 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'");
return -1;
}

153
src/drivers/distance_sensor/sf0x/SF0X.hpp Normal file
View File

@ -0,0 +1,153 @@
/****************************************************************************
*
* 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SF0X.hpp
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Greg Hulands
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#pragma once
#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 <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 "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
{
public:
SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~SF0X();
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.
*/
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;
orb_advert_t _distance_sensor_topic;
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();
};

301
src/drivers/distance_sensor/sf0x/sf0x_main.cpp Normal file
View File

@ -0,0 +1,301 @@
/****************************************************************************
*
* 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "SF0X.hpp"
#include <px4_platform_common/getopt.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;
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)
{
int fd;
if (g_dev != nullptr) {
PX4_WARN("already started");
return -1;
}
/* create the driver */
g_dev = new SF0X(port, rotation);
if (g_dev == nullptr) {
goto fail;
}
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;
}
return 0;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
return -1;
}
/**
* Stop the driver
*/
int stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
return -1;
}
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()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return -1;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return 0;
}
} // namespace
int
sf0x_main(int argc, char *argv[])
{
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = SF0X_DEFAULT_PORT;
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
PX4_WARN("Unknown option!");
return -1;
}
}
if (myoptind >= argc) {
goto out_error;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
return sf0x::start(device_path, rotation);
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return sf0x::stop();
}
/*
* 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'");
return -1;
}