tfmini : fix scheduling and modernize output

This reduces the scheduling interval to 9ms, such that the driver is always ready to read new data. Running it at exactly 100Hz is not correct since the driver and sensor measurement intervals are not "in sync", causing the driver to miss data. This causes a fill-up of the UART buffer.

src/drivers/distance_sensor/tfmini/tfmini.cpp

@@ -118,16 +118,11 @@ private:
 
 	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();
 
@@ -174,7 +169,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
 	_rotation(rotation),
 	_min_distance(0.30f),
 	_max_distance(12.0f),
-	_conversion_interval(10000),
+	_conversion_interval(9000),
 	_reports(nullptr),
 	_measure_ticks(0),
 	_collect_phase(false),
@@ -185,7 +180,6 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
 	_class_instance(-1),
 	_orb_class_instance(-1),
 	_distance_sensor_topic(nullptr),
-	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err"))
 {
@@ -233,7 +227,7 @@ TFMINI::init()
 	case 1: /* TFMINI (12m, 100 Hz)*/
 		_min_distance = 0.3f;
 		_max_distance = 12.0f;
-		_conversion_interval =	10000;
+		_conversion_interval = 9000;
 		break;
 
 	default:
@@ -247,10 +241,10 @@ TFMINI::init()
 	do { /* create a scope to handle exit conditions using break */
 
 		/* open fd */
-		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);
 
 		if (_fd < 0) {
-			warnx("Error opening fd");
+			PX4_ERR("Error opening fd");
 			return -1;
 		}
 
@@ -268,19 +262,19 @@ TFMINI::init()
 
 		/* set baud rate */
 		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-			warnx("ERR CFG: %d ISPD", termios_state);
+			PX4_ERR("CFG: %d ISPD", termios_state);
 			ret = -1;
 			break;
 		}
 
 		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-			warnx("ERR CFG: %d OSPD\n", termios_state);
+			PX4_ERR("CFG: %d OSPD\n", termios_state);
 			ret = -1;
 			break;
 		}
 
 		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
-			warnx("ERR baud %d ATTR", termios_state);
+			PX4_ERR("baud %d ATTR", termios_state);
 			ret = -1;
 			break;
 		}
@@ -302,7 +296,7 @@ TFMINI::init()
 		uart_config.c_cc[VTIME] = 1;
 
 		if (_fd < 0) {
-			warnx("FAIL: laser fd");
+			PX4_ERR("FAIL: laser fd");
 			ret = -1;
 			break;
 		}
@@ -316,7 +310,7 @@ TFMINI::init()
 		_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
 
 		if (_reports == nullptr) {
-			warnx("mem err");
+			PX4_ERR("mem err");
 			ret = -1;
 			break;
 		}
@@ -538,7 +532,7 @@ TFMINI::collect()
 	ret = ::read(_fd, &readbuf[0], readlen);
 
 	if (ret < 0) {
-		DEVICE_DEBUG("read err: %d", ret);
+		PX4_ERR("read err: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 
@@ -629,7 +623,7 @@ TFMINI::cycle()
 	/* fds initialized? */
 	if (_fd < 0) {
 		/* open fd */
-		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
+		_fd = ::open(_port, O_RDWR | O_NOCTTY);
 	}
 
 	/* collection phase? */
@@ -639,31 +633,13 @@ TFMINI::cycle()
 		int collect_ret = collect();
 
 		if (collect_ret == -EAGAIN) {
-			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
+			/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */
 			work_queue(HPWORK,
 				   &_work,
 				   (worker_t)&TFMINI::cycle_trampoline,
 				   this,
-				   USEC2TICK(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) {
-				DEVICE_LOG("collection error #%u", _consecutive_fail_count);
-			}
-
-			_consecutive_fail_count++;
-
-			/* restart the measurement state machine */
-			start();
+				   USEC2TICK(87 * 9));
 			return;
-
-		} else {
-			/* apparently success */
-			_consecutive_fail_count = 0;
 		}
 
 		/* next phase is measurement */
@@ -717,7 +693,6 @@ TFMINI	*g_dev;
 void start(const char *port, uint8_t rotation);
 void stop();
 void test();
-void reset();
 void info();
 void usage();
 
@@ -730,7 +705,8 @@ start(const char *port, uint8_t rotation)
 	int fd;
 
 	if (g_dev != nullptr) {
-		errx(1, "already started");
+		PX4_ERR("already started");
+		exit(1);
 	}
 
 	/* create the driver */
@@ -748,7 +724,7 @@ start(const char *port, uint8_t rotation)
 	fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		warnx("Opening device '%s' failed");
+		PX4_ERR("Opening device '%s' failed");
 		goto fail;
 	}
 
@@ -765,7 +741,8 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	PX4_ERR("driver start failed");
+	exit(1);
 }
 
 /**
@@ -774,13 +751,14 @@ fail:
 void stop()
 {
 	if (g_dev != nullptr) {
-		warnx("stopping driver");
+		PX4_INFO("stopping driver");
 		delete g_dev;
 		g_dev = nullptr;
-		warnx("driver stopped");
+		PX4_INFO("driver stopped");
 
 	} else {
-		errx(1, "driver not running");
+		PX4_ERR("driver not running");
+		exit(1);
 	}
 
 	exit(0);
@@ -814,7 +792,8 @@ test()
 
 	/* start the sensor polling at 2 Hz rate */
 	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		errx(1, "failed to set 2Hz poll rate");
+		PX4_ERR("failed to set 2Hz poll rate");
+		exit(1);
 	}
 
 	/* read the sensor 5x and report each value */
@@ -827,7 +806,7 @@ test()
 		int ret = px4_poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warnx("timed out");
+			PX4_ERR("timed out");
 			break;
 		}
 
@@ -835,7 +814,7 @@ test()
 		sz = px4_read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
+			PX4_ERR("read failed: got %d vs exp. %d", sz, sizeof(report));
 			break;
 		}
 
@@ -844,33 +823,11 @@ test()
 
 	/* reset the sensor polling to the default rate */
 	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		errx(1, "ERR: DEF RATE");
-	}
-
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset()
-{
-	int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
-
-	if (fd < 0) {
-		err(1, "failed ");
-	}
-
-	if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		err(1, "driver reset failed");
+		PX4_ERR("failed to set default poll rate");
+		exit(1);
 	}
 
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		err(1, "driver poll restart failed");
-	}
-
-	exit(0);
+	PX4_INFO("PASS");
 }
 
 /**
@@ -880,7 +837,8 @@ void
 info()
 {
 	if (g_dev == nullptr) {
-		errx(1, "driver not running");
+		PX4_ERR("driver not running");
+		exit(1);
 	}
 
 	printf("state @ %p\n", g_dev);
@@ -958,13 +916,6 @@ tfmini_main(int argc, char *argv[])
 		tfmini::test();
 	}
 
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[myoptind], "reset")) {
-		tfmini::reset();
-	}
-
 	/*
 	 * Print driver information.
 	 */
@@ -973,6 +924,6 @@ tfmini_main(int argc, char *argv[])
 	}
 
 out_error:
-	PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
 	return -1;
 }