mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 10:33:35 +08:00
sf0x: split into separate header, implementation, and main
This commit is contained in:
Daniel Agar
@@ -0,0 +1,542 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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"
|
||||
|
||||
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"))
|
||||
{
|
||||
/* store port name */
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
|
||||
/* enforce null termination */
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int
|
||||
SF0X::init()
|
||||
{
|
||||
int hw_model;
|
||||
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;
|
||||
break;
|
||||
|
||||
case 2: /* SF10/a (25m 32Hz) */
|
||||
_min_distance = 0.01f;
|
||||
_max_distance = 25.0f;
|
||||
_conversion_interval = 31250;
|
||||
break;
|
||||
|
||||
case 3: /* SF10/b (50m 32Hz) */
|
||||
_min_distance = 0.01f;
|
||||
_max_distance = 50.0f;
|
||||
_conversion_interval = 31250;
|
||||
break;
|
||||
|
||||
case 4: /* SF10/c (100m 16Hz) */
|
||||
_min_distance = 0.01f;
|
||||
_max_distance = 100.0f;
|
||||
_conversion_interval = 62500;
|
||||
break;
|
||||
|
||||
case 5:
|
||||
/* SF11/c (120m 20Hz) */
|
||||
_min_distance = 0.01f;
|
||||
_max_distance = 120.0f;
|
||||
_conversion_interval = 50000;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("invalid HW model %d.", hw_model);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* status */
|
||||
int ret = 0;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void
|
||||
SF0X::set_minimum_distance(float min)
|
||||
{
|
||||
_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.
|
||||
*/
|
||||
char cmd = SF0X_TAKE_RANGE_REG;
|
||||
ret = ::write(_fd, &cmd, 1);
|
||||
|
||||
if (ret != sizeof(cmd)) {
|
||||
perf_count(_comms_errors);
|
||||
PX4_DEBUG("write fail %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
SF0X::collect()
|
||||
{
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
/* clear buffer if last read was too long ago */
|
||||
int64_t read_elapsed = hrt_elapsed_time(&_last_read);
|
||||
|
||||
/* the buffer for read chars is buflen minus null termination */
|
||||
char readbuf[sizeof(_linebuf)];
|
||||
unsigned readlen = sizeof(readbuf) - 1;
|
||||
|
||||
/* read from the sensor (uart buffer) */
|
||||
int ret = ::read(_fd, &readbuf[0], readlen);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_DEBUG("read err: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
|
||||
/* only throw an error if we time out */
|
||||
if (read_elapsed > (_conversion_interval * 2)) {
|
||||
return ret;
|
||||
|
||||
} else {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
} else if (ret == 0) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
_last_read = hrt_absolute_time();
|
||||
|
||||
float distance_m = -1.0f;
|
||||
bool valid = false;
|
||||
|
||||
for (int i = 0; i < ret; i++) {
|
||||
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!valid) {
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
void
|
||||
SF0X::Run()
|
||||
{
|
||||
/* fds initialized? */
|
||||
if (_fd < 0) {
|
||||
/* open fd */
|
||||
_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
if (_fd < 0) {
|
||||
PX4_ERR("open failed (%i)", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_fd, &uart_config);
|
||||
|
||||
/* clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
/* no parity, one stop bit */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB);
|
||||
|
||||
/* if distance sensor model is SF11/C, then set baudrate 115200, else 9600 */
|
||||
int hw_model;
|
||||
|
||||
param_get(param_find("SENS_EN_SF0X"), &hw_model);
|
||||
|
||||
unsigned speed;
|
||||
|
||||
if (hw_model == 5) {
|
||||
speed = B115200;
|
||||
|
||||
} else {
|
||||
speed = B9600;
|
||||
}
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("CFG: %d ISPD", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("CFG: %d OSPD", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("baud %d ATTR", termios_state);
|
||||
}
|
||||
}
|
||||
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
int collect_ret = collect();
|
||||
|
||||
if (collect_ret == -EAGAIN) {
|
||||
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
|
||||
ScheduleDelayed(1042 * 8);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (OK != collect_ret) {
|
||||
|
||||
/* we know the sensor needs about four seconds to initialize */
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
PX4_ERR("collection error #%u", _consecutive_fail_count);
|
||||
}
|
||||
|
||||
_consecutive_fail_count++;
|
||||
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
|
||||
} else {
|
||||
/* apparently success */
|
||||
_consecutive_fail_count = 0;
|
||||
}
|
||||
|
||||
/* 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 */
|
||||
if (OK != measure()) {
|
||||
PX4_DEBUG("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(_conversion_interval);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
Reference in New Issue
Block a user