Hi all,
I am writing a custom driver for a new kind of airspeed sensor, which sends data constantly in UART, ASCII at 10Hz.
I don’t know much about writing c++/posix, so I started from writing based on tfmini driver.
I somehow wrote the code below, but when the sensor is connected, it crashes as soon as it is started, and then pixhawk is rebooted and the error tune sounds.
When no sensors are connected to the port, it does at least start successfully.
What seems to be the problem? Thank you.
#include <float.h>
#include <px4_config.h>
#include <px4_workqueue.h>
#include <px4_getopt.h>
#include <px4_module.h>
#include <sys/types.h>
#include <sys/ioctl.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>
#ifdef __PX4_CYGWIN
#include <asm/socket.h>
#endif
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_airspeed.h>
#include <drivers/airspeed/airspeed.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <board_config.h>
#include <airspeed/airspeed.h>
#define FT205EV_PATH "/dev/ft205ev"
/* Configuration Constants */
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class FT205EV : public cdev::CDev
{
public:
FT205EV(const char *port);
virtual ~FT205EV();
virtual int init();
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
private:
char _port[20];
int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
int _measure_ticks;
bool _collect_phase;
int _fd;
char _linebuf[30];
unsigned _linebuf_index;
orb_advert_t _airspeed_pub;
hrt_abstime _last_read;
int _class_instance;
int _orb_class_instance;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ft205ev_main(int argc, char *argv[]);
FT205EV::FT205EV(const char *port) :
CDev(FT205EV_PATH),
_conversion_interval(9000),
_reports(nullptr),
_measure_ticks(0),
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_last_read(0),
_class_instance(-1),
_orb_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ft205ev_read")),
_comms_errors(perf_alloc(PC_COUNT, "ft205ev_com_err"))
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
}
FT205EV::~FT205EV()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
FT205EV::init()
{
/* status */
int ret = 0;
do { /* create a scope to handle exit conditions using break */
/* open fd */
_fd = ::open(_port, O_RDONLY | O_NOCTTY);
if (_fd < 0) {
PX4_ERR("Error opening fd");
return -1;
}
/*baudrate 38400, 8 bits, no parity, 1 stop bit */
unsigned speed = B38400;
struct termios uart_config;
int termios_state;
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1;
break;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d OSPD\n", termios_state);
ret = -1;
break;
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state);
ret = -1;
break;
}
uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
uart_config.c_cflag &= ~CSIZE;
uart_config.c_cflag |= CS8; /* 8-bit characters */
uart_config.c_cflag &= ~PARENB; /* no parity bit */
uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
uart_config.c_oflag &= ~OPOST;
/* fetch bytes as they become available */
uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1;
if (_fd < 0) {
PX4_ERR("FAIL: laser fd");
ret = -1;
break;
}
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(airspeed_s));
if (_reports == nullptr) {
PX4_ERR("mem err");
ret = -1;
break;
}
_class_instance = register_class_devname(FT205EV_PATH);
/* get a publish handle on the range finder topic */
_airspeed_pub = nullptr;
airspeed_s airspeed;
if (_airspeed_pub == nullptr) {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
}
} while (0);
/* close the fd */
::close(_fd);
_fd = -1;
return ret;
}
int
FT205EV::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_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(_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_ticks == 0);
/* convert hz to tick interval via microseconds */
int ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* 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
FT205EV::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct airspeed_s);
struct airspeed_s *rbuf = reinterpret_cast<struct airspeed_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 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();
/* 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
FT205EV::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)];
int ret = 0;
/* Check the number of bytes available in the buffer*/
int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) {
return -EAGAIN;
}
/* parse entire buffer */
do {
/* read from the sensor (uart buffer) */
int k = 0;
ret = ::read(_fd, &readbuf[k], 1);
if (ret < 0) {
PX4_ERR("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)) {
/* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH);
return ret;
} else {
return -EAGAIN;
}
}
_last_read = hrt_absolute_time();
/* bytes left to parse */
bytes_available -= ret;
if(readbuf[k] == 0x0A){
k = 0;
strtok(readbuf,",");
char *ws1 = strtok(NULL,",");
char *ws2;
if(ws1[5] == 0){
ws2 = &ws1[6];
}
else{
ws2 = &ws1[5];
}
char *dir = strtok(NULL,",");
//Serial.println(ws2);
//Serial.println(dir);
float windspeed = atof(ws2);
float direction = (atof(dir) * 71) / 4068;
float aspd = windspeed * cosf(direction);
_airspeed_pub = nullptr;
airspeed_s airspeed;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = aspd;
airspeed.true_airspeed_m_s = aspd;
airspeed.air_temperature_celsius = -1000.0;
airspeed.confidence = 1.0f;
if (_airspeed_pub == nullptr) {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
}
break;
}
} while (bytes_available > 0);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
FT205EV::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&FT205EV::cycle_trampoline, this, 1);
}
void
FT205EV::stop()
{
work_cancel(HPWORK, &_work);
}
void
FT205EV::cycle_trampoline(void *arg)
{
FT205EV *dev = (FT205EV *)arg;
dev->cycle();
}
void
FT205EV::cycle()
{
/* fds initialized? */
if (_fd < 0) {
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY);
}
/* collection phase? */
if (_collect_phase) {
/* perform collection */
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */
work_queue(HPWORK,
&_work,
(worker_t)&FT205EV::cycle_trampoline,
this,
USEC2TICK(87 * 9));
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(_conversion_interval)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&FT205EV::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(_conversion_interval));
return;
}
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&FT205EV::cycle_trampoline,
this,
USEC2TICK(_conversion_interval));
}
void
FT205EV::print_info()
{
printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %d ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace ft205ev
{
FT205EV *g_dev;
int start(const char *port);
int stop();
int test();
int info();
void usage();
/**
* Start the driver.
*/
int
start(const char *port)
{
int fd;
if (g_dev != nullptr) {
PX4_ERR("already started");
return 1;
}
/* create the driver */
g_dev = new FT205EV(port);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = px4_open(FT205EV_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("Opening device '%s' failed", port);
goto fail;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
return 0;
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
PX4_ERR("driver start failed");
return 1;
}
/**
* Stop the driver
*/
int stop()
{
if (g_dev != nullptr) {
PX4_INFO("stopping driver");
delete g_dev;
g_dev = nullptr;
PX4_INFO("driver stopped");
} else {
PX4_ERR("driver not running");
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 airspeed_s report;
ssize_t sz;
int fd = px4_open(FT205EV_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'ft205ev start' if the driver is not running", FT205EV_PATH);
return 1;
}
/* do a simple demand read */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
close(fd);
return 1;
}
print_message(report);
/* start the sensor polling at 2 Hz rate */
if (OK != px4_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++) {
px4_pollfd_struct_t fds{};
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
break;
}
/* now go get it */
sz = px4_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 != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("failed to set default poll rate");
return 1;
}
PX4_INFO("PASS");
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;
}
/**
* Print a little info on how to use the driver.
*/
void
usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Serial bus driver for the Benewake TFmini LiDAR.
Most boards are configured to enable/start the driver on a specified UART using the SENS_FT205EV_CFG parameter.
Setup/usage information: https://docs.px4.io/en/sensor/ft205ev.html
### Examples
Attempt to start driver on a specified serial device.
$ ft205ev start -d /dev/ttyS1
Stop driver
$ ft205ev stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ft205ev", "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");
PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
}
} // namespace
int
ft205ev_main(int argc, char *argv[])
{
int ch;
const char *device_path = "";
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
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")) {
if (strcmp(device_path, "") != 0) {
return ft205ev::start(device_path);
} else {
PX4_WARN("Please specify device path!");
ft205ev::usage();
return -1;
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return ft205ev::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
return ft205ev::test();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
ft205ev::info();
return 0;
}
out_error:
PX4_ERR("unrecognized command");
ft205ev::usage();
return -1;
}