Custom UART device driver crashes when starting

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;
}

Do you have access to the printf logs?
https://dev.px4.io/master/en/debug/system_console.html

And you should also check out this:
https://dev.px4.io/master/en/debug/gdb_debugging.html

Thanks for the information.

Tried accessing to the printf logs and got this result. Could you give me advice?

INFO  [mavlink] Starting mavlink shell
up_hardfault: Hard Fault:
up_hardfault:   IRQ: 3 regs: 2007d58c
up_hardfault:   BASEPRI: 000000f0 PRIMASK: 00000000 IPSR: 00000003 CONTROL: 00000000
up_hardfault:   CFAULTS: 00008200 HFAULTS: 40000000 DFAULTS: 00000000 BFAULTADDR: 00000005 AFAULTS: 00000000
up_hardfault: PANIC!!! Hard fault: 40000000
up_assert: Assertion failed at file:armv7-m/up_hardfault.c line: 148 task: hpwork
up_registerdump: R0: 00000000 00000000 20029474 00000000 20037560 00000000 000000f0 00000000
up_registerdump: R8: 0367bd90 00000000 0367dd88 00000000 00000009 2007d660 08029bef 08029bee
up_registerdump: xPSR: 21000000 BASEPRI: 000000f0 CONTROL: 00000000
up_registerdump: EXC_RETURN: ffffffe9
up_dumpstate: sp:     20021488
up_dumpstate: IRQ stack:
up_dumpstate:   base: 200214f0
up_dumpstate:   size: 000002e8
up_dumpstate:   used: 000001cc
up_stackdump: 20021480: 000006f4 08009a85 000000f0 00000000 00000000 00000009 2007d660 08029bef
up_stackdump: 200214a0: 08029bee 200214b0 08009519 00000003 00000000 08009521 40000000 00000000
up_stackdump: 200214c0: 00000005 00000000 200214f0 0800b2a3 000000f0 080083a5 000000f0 2007d58c
up_stackdump: 200214e0: 00000000 000000f0 00000000 08008237 2007d58c 2c495724 3d505657 2e303030
up_dumpstate: sp:     2007d660
up_dumpstate: User stack:
up_dumpstate:   base: 2007d730
up_dumpstate:   size: 000006f4
up_dumpstate:   used: 00000320
up_stackdump: 2007d660: 2002f3d0 00000000 00000000 08135225 00000000 00000014 0367dd81 00000000
up_stackdump: 2007d680: 0000000a 0812767d 00000014 2002f270 0808b445 2002f170 00000001 0808b3fb
up_stackdump: 2007d6a0: 00000014 00000000 00000000 20037560 2003759c 000000f0 00000000 200257c4
up_stackdump: 2007d6c0: 00000080 0000002b 20025aa0 08029df9 20037560 08029e5d 20026968 2003759c
up_stackdump: 2007d6e0: 00000000 0800cf55 351cf33f 0000df5a 20000010 00000000 00000000 20025aa0
up_stackdump: 2007d700: 00000000 00000000 00000000 00000000 00000000 00000000 00000000 0800c3b5
up_stackdump: 2007d720: 0800c3a9 0800c24d 00000000 00000000 deadbeef 2007d73c 00000000 6f777068
up_taskdump: Idle Task: PID=0 Stack Used=0 of 0
up_taskdump: hpwork: PID=1 Stack Used=800 of 1780
up_taskdump: lpwork: PID=2 Stack Used=832 of 1780
up_taskdump: init: PID=3 Stack Used=2032 of 2604
up_taskdump: mavlink_shell: PID=517 Stack Used=880 of 2020
up_taskdump: px4io: PID=294 Stack Used=1048 of 1484
up_taskdump: mavlink_if0: PID=167 Stack Used=1760 of 2532
up_taskdump: mavlink_rcv_if0: PID=168 Stack Used=1544 of 2836
up_taskdump: ekf2: PID=457 Stack Used=4576 of 6572
up_taskdump: mavlink_if1: PID=267 Stack Used=1680 of 2484
up_taskdump: mavlink_rcv_if1: PID=268 Stack Used=1120 of 2836
up_taskdump: fw_att_controol: PID=461 Stack Used=840 of 1460
up_taskdump: gps: PID=238 Stack Used=1072 of 1516
up_taskdump: dataman: PID=16 Stack Used=752 of 1180
up_taskdump: fmu: PID=305 Stack Used=736 of 1324
up_taskdump: fw_pos_control_l1: PID=466 Stack Used=776 of 1764
up_taskdump: mavlink_if2: PID=277 Stack Used=1536 of 2492
up_taskdump: mavlink_rcv_if2: PID=278 Stack Used=1120 of 2836
up_taskdump: navigator: PID=472 Stack Used=896 of 1764
up_taskdump: sensors: PID=154 Stack Used=1232 of 1964
up_taskdump: logger: PID=507 Stack Used=1312 of 3644
up_taskdump: commander: PID=156 Stack Used=1480 of 3212
up_taskdump: commander_low_prio: PID=157 Stack Used=560 of 2996
up_taskdump: log_writer_file: PID=511 Stack Used=376 of 1148
[boot] Rev 0x0 : Ver 0x0 V500

[boot] Fault Log info File No 4 Length 3177 flags:0x01 state:0

[boot] Fault Logged on 2019-10-29-05:08:23 - Valid

[boot] There is a hard fault logged. Hold down the SPACE BAR, while booting to review!

sercon: Registering CDC/ACM serial driver
sercon: Successfully registered the CDC/ACM serial driver
HW arch: PX4_FMU_V5
HW type: V500
HW version: 0x00000000
HW revision: 0x00000000
FW git-hash: 544145b002169f72888ca316564bd14f6c3012b4
FW version: 1.9.2 0 (17367552)
FW git-branch: UFB_1.9.2_FT205EVdev
OS: NuttX
OS version: Release 7.28.0 (119275775)
OS git-hash: 423371c7d4012e725ac4ca51323a18df64e581b3
Build datetime: Oct 29 2019 14:05:52
Build uri: localhost
Toolchain: GNU GCC, 7.2.1 20170904 (release) [ARM/embedded-7-branch revision 255204]
PX4GUID: 0002000000003534363331385118002e0031
MCU: STM32F76xxx, rev. Z
[hardfault_log] Fault Log info File No 4 Length 3177 flags:0x01 state:0

[hardfault_log] Fault Logged on 2019-10-29-05:08:23 - Valid

INFO  [tune_control] Publishing standard tune 2
[hardfault_log] Saving Fault Log file /fs/microsd/fault_2019_10_29_05_08_23.log

[hardfault_log] Fault Log is Armed

[hardfault_log] Done saving Fault Log file

[hardfault_log] No ULog to append to

INFO  [param] selected parameter default file /fs/mtd_params
Board defaults: /etc/init.d/rc.board_defaults
WARN  [rgbled] no RGB led on bus #1
WARN  [rgbled_ncp5623c] no RGB led on bus #1
WARN  [blinkm] I2C init failed
WARN  [blinkm] init failed
Board sensors: /etc/init.d/rc.board_sensors
WARN  [mpu6000] no device on bus #3 (SPI1)
MPU6000 on SPI bus 1 at 0 (1000 KHz)
BMI055_ACCEL on SPI bus 1 at 3 (10000 KHz)
BMI055_GYRO on SPI bus 1 at 2 (10000 KHz)
INFO  [ist8310] no device on bus 1
INFO  [ist8310] no device on bus 2
WARN  [hmc5883] no device on bus 1 (type: 2)
WARN  [hmc5883] no device on bus 2 (type: 2)
WARN  [hmc5883] no device on bus 4 (type: 2)
WARN  [qmc5883] no device on bus 1 (type: 2)
WARN  [qmc5883] no device on bus 2 (type: 2)
WARN  [qmc5883] no device on bus 4 (type: 2)
WARN  [lis3mdl] no device on bus 2
WARN  [lis3mdl] no device on bus 2
WARN  [lis3mdl] no device on bus 2
IST8310 on I2C bus 3 at 0x0e (bus: 100 KHz, max: 400 KHz)
ERROR [pmw3901] driver start failed
MS5611_SPI on SPI bus 4 at 0 (20000 KHz)
WARN  [bst] no devices found
INFO  [mavlink] mode: Config, data rate: 800000 B/s on /dev/ttyACM0 @ 57600B
Starting Main GPS on /dev/ttyS0
Starting MAVLink on /dev/ttyS1
INFO  [mavlink] mode: Normal, data rate: 1200 B/s on /dev/ttyS1 @ 57600B
Starting MAVLink on /dev/ttyS2
INFO  [mavlink] mode: Minimal, data rate: 5760 B/s on /dev/ttyS2 @ 115200B
INFO  [px4io] default PWM output device
INFO  [init] Mixer: /etc/mixers/AETRFG.main.mix on /dev/pwm_output0
INFO  [init] Mixer: /etc/mixers/pass.aux.mix on /dev/pwm_output1
px4flow [487:100]
INFO  [px4flow] scanning I2C buses for device..
INFO  [logger] logger started (mode=all)

NuttShell (NSH)
nsh> e[KINFO  [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)

To figure out what is happening, I modified collect() part as below to make it output bytes_available value.

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);
	PX4_INFO("bytes_available is %d",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);
		PX4_INFO("2");
		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;
}

And the output was

INFO  [mavlink] Starting mavlink shell
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFO  [ft205ev] bytes_available is 0
INFup_hardfault: Hard Fault:
up_hardfault:   IRQ: 3 regs: 2007d58c
up_hardfault:   BASEPRI: 000000f0 PRIMASK: 00000000 IPSR: 00000003 CONTROL: 00000000
up_hardfault:   CFAULTS: 00008200 HFAULTS: 40000000 DFAULTS: 00000000 BFAULTADDR: 00000005 AFAULTS: 00000000
up_hardfault: PANIC!!! Hard fault: 40000000
up_assert: Assertion failed at file:armv7-m/up_hardfault.c line: 148 task: hpwork
up_registerdump: R0: 00000000 00000000 20029474 00000000 20037e20 00000000 032215cc 00000000
up_registerdump: R8: 0321eabf 00000000 0816016e 0813c523 0000000b 2007d660 08029c57 08029c56
up_registerdump: xPSR: 21000000 BASEPRI: 000000f0 CONTROL: 00000000
up_registerdump: EXC_RETURN: ffffffe9
up_dumpstate: sp:     20021488
up_dumpstate: IRQ stack:
up_dumpstate:   base: 200214f0
up_dumpstate:   size: 000002e8
up_dumpstate:   used: 000001cc
up_stackdump: 20021480: 000006f4 08009a85 000000f0 00000000 0813c523 0000000b 2007d660 08029c57
up_stackdump: 200214a0: 08029c56 200214b0 08009519 00000003 0813c523 08009521 40000000 00000000
up_stackdump: 200214c0: 00000005 00000000 200214f0 0800b2a3 000000f0 080083a5 000000f0 2007d58c
up_stackdump: 200214e0: 00000000 032215cc 00000000 08008237 2007d58c 2c495724 3d505657 2e303030
up_dumpstate: sp:     2007d660
up_dumpstate: User stack:
up_dumpstate:   base: 2007d730
up_dumpstate:   size: 000006f4
up_dumpstate:   used: 000003a0
up_stackdump: 2007d660: 2007d6b0 00000000 200257c4 00000080 20025aa0 08044473 2007c860 42500000
up_stackdump: 2007d680: 42c0000a 42c80000 00340060 00000064 03221210 00000000 00000000 00000000
up_stackdump: 2007d6a0: 00060e19 00000000 00000000 20037e20 20037e5c 000000f0 00000000 200257c4
up_stackdump: 2007d6c0: 00000080 00000001 20025aa0 08029e21 20037e20 08029e85 2002f830 20037e5c
up_stackdump: 2007d6e0: 00000000 0800cf55 359f70d1 0000cd7b 80000010 00000000 00000000 20025aa0
up_stackdump: 2007d700: 00000000 00000000 00000000 00000000 00000000 00000000 00000000 0800c3b5
up_stackdump: 2007d720: 0800c3a9 0800c24d 00000000 00000000 deadbeef 2007d73c 00000000 6f777068
up_taskdump: Idle Task: PID=0 Stack Used=0 of 0
up_taskdump: hpwork: PID=1 Stack Used=928 of 1780
up_taskdump: lpwork: PID=2 Stack Used=816 of 1780
up_taskdump: init: PID=3 Stack Used=2024 of 2604
up_taskdump: mavlink_shell: PID=517 Stack Used=896 of 2020
up_taskdump: px4io: PID=294 Stack Used=880 of 1484
up_taskdump: mavlink_if0: PID=167 Stack Used=1760 of 2532
up_taskdump: mavlink_rcv_if0: PID=168 Stack Used=1544 of 2836
up_taskdump: ekf2: PID=457 Stack Used=4576 of 6572
up_taskdump: mavlink_if1: PID=267 Stack Used=1592 of 2484
up_taskdump: mavlink_rcv_if1: PID=268 Stack Used=1120 of 2836
up_taskdump: fw_att_controol: PID=461 Stack Used=720 of 1460
up_taskdump: gps: PID=238 Stack Used=984 of 1516
up_taskdump: dataman: PID=16 Stack Used=752 of 1180
up_taskdump: fmu: PID=305 Stack Used=824 of 1324
up_taskdump: fw_pos_control_l1: PID=466 Stack Used=736 of 1764
up_taskdump: mavlink_if2: PID=277 Stack Used=1544 of 2492
up_taskdump: mavlink_rcv_if2: PID=278 Stack Used=1240 of 2836
up_taskdump: navigator: PID=472 Stack Used=896 of 1764
up_taskdump: sensors: PID=154 Stack Used=1240 of 1964
up_taskdump: logger: PID=507 Stack Used=1312 of 3644
up_taskdump: commander: PID=156 Stack Used=1480 of 3212
up_taskdump: commander_low_prio: PID=157 Stack Used=640 of 2996
up_taskdump: log_writer_file: PID=511 Stack Used=376 of 1148

So no data appeared to be stored on serial buffer of _fd, though the data is confirmed output from the senor.

Changed collect() again.

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[30];

	int ret = 0;
	_airspeed_pub = nullptr;

	/* parse entire buffer */
	while(1) {
		/* read from the sensor (uart buffer) */
		int k = 0;
		ret = ::read(_fd, &readbuf[k], 1);
		//PX4_INFO("2");
		if (ret < 0) {
			PX4_ERR("read err: %d", ret);
			perf_count(_comms_errors);
			perf_end(_sample_perf);
		}

		_last_read = hrt_absolute_time();

		if(readbuf[k] == 0x0A){
//Incoming message format: $WI,<windspeed>,<direction>,<checksum><CR+LF>
			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);
    	PX4_INFO("aspd value is %d", aspdi);

			airspeed_s airspeed;
			airspeed.timestamp = hrt_absolute_time();
			airspeed.indicated_airspeed_m_s = 14.0f;
			airspeed.true_airspeed_m_s = 14.0f;
			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);
			}
			k=0;
			break;
		}
	}
	ret = OK;
	perf_end(_sample_perf);
	return ret;
}

And after some trial I found out this line was the cause of crash.

float windspeed = atof(ws2);

So I tried to print ws2 like this, and this led to crash too.

		if(readbuf[k] == 0x0A){
			//Incoming message format: $WI,<windspeed>,<direction>,<checksum><CR+LF>
			strtok(readbuf,",");
    	char *ws1 = strtok(NULL,",");
    	char *ws2;
    	if(ws1[5] == 0){
    	  ws2 = &ws1[6];
    	}
    	else{
    	  ws2 = &ws1[5];
    	}
    	PX4_INFO("aspd value is %s", ws2);

So accessing address of ws2 could be the cause of crash.
Could you give advice on this issue? Thanks a lot.

If strtok is not successful it will return NULL. This means you might be passing garbage data to atof and atof has undefined behavior if the data can’t be represented by a double.

1 Like

Thank you so much for your advice!
The problem was solved and it worked perfectly.

1 Like

Hi takhashi,
I also need to use the FT205 as an airspeed sensor on PX4, and I read your code which is impressing, so I want to know how you finally solved your problem. Any help would be appreciated. :grinning:

Hi Zack,
Here is my driver for px4 v1.9.2.
https://drive.google.com/file/d/1HMQLlabR5cklOakyPfdZxo6rC02QWYN3/view?usp=sharing

It is not updated for v1.10 uORB system but it should be working. Hope it will help!

Hi takhashi,
Thank you for your help!

1 Like