merged master

3 files changed, 197 insertions, 40 deletions

diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 7693df295..5adb8cf69 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -762,6 +762,11 @@ test()
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
 	errx(0, "PASS");
 }
 
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 5dd1f59de..70cd1ab1e 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -53,6 +53,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <termios.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -83,8 +84,8 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-#define SF0X_CONVERSION_INTERVAL	84000
-#define SF0X_TAKE_RANGE_REG		'D'
+#define SF0X_CONVERSION_INTERVAL	83334
+#define SF0X_TAKE_RANGE_REG		'd'
 #define SF02F_MIN_DISTANCE		0.0f
 #define SF02F_MAX_DISTANCE		40.0f
 #define SF0X_DEFAULT_PORT		"/dev/ttyS2"
@@ -92,7 +93,7 @@ static const int ERROR = -1;
 class SF0X : public device::CDev
 {
 public:
-	SF0X(const char* port=SF0X_DEFAULT_PORT);
+	SF0X(const char *port = SF0X_DEFAULT_PORT);
 	virtual ~SF0X();
 
 	virtual int 			init();
@@ -117,6 +118,9 @@ private:
 	int				_measure_ticks;
 	bool				_collect_phase;
 	int				_fd;
+	char				_linebuf[10];
+	unsigned			_linebuf_index;
+	hrt_abstime			_last_read;
 
 	orb_advert_t			_range_finder_topic;
 
@@ -178,6 +182,9 @@ SF0X::SF0X(const char *port) :
 	_sensor_ok(false),
 	_measure_ticks(0),
 	_collect_phase(false),
+	_fd(-1),
+	_linebuf_index(0),
+	_last_read(0),
 	_range_finder_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
@@ -186,8 +193,45 @@ SF0X::SF0X(const char *port) :
 	/* open fd */
 	_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
-	// enable debug() calls
-	_debug_enabled = true;
+	if (_fd < 0) {
+		warnx("FAIL: laser fd");
+	}
+
+	/* tell it to stop auto-triggering */
+	char stop_auto = ' ';
+	(void)::write(_fd, &stop_auto, 1);
+	usleep(100);
+	(void)::write(_fd, &stop_auto, 1);
+
+	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);
+
+	unsigned speed = B9600;
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d ISPD", termios_state);
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d OSPD\n", termios_state);
+	}
+
+	if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR baud %d ATTR", termios_state);
+	}
+
+	// disable debug() calls
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -208,15 +252,18 @@ int
 SF0X::init()
 {
 	int ret = ERROR;
+	unsigned i = 0;
 
 	/* do regular cdev init */
-	if (CDev::init() != OK)
+	if (CDev::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
 	if (_reports == nullptr) {
+		warnx("mem err");
 		goto out;
 	}
 
@@ -226,12 +273,35 @@ SF0X::init()
 	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
 
 	if (_range_finder_topic < 0) {
-		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+		warnx("advert err");
 	}
 
-	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
+	/* attempt to get a measurement 5 times */
+	while (ret != OK && i < 5) {
+
+		if (measure()) {
+			ret = ERROR;
+			_sensor_ok = false;
+		}
+
+		usleep(100000);
+
+		if (collect()) {
+			ret = ERROR;
+			_sensor_ok = false;
+
+		} else {
+			ret = OK;
+			/* sensor is ok, but we don't really know if it is within range */
+			_sensor_ok = true;
+		}
+
+		i++;
+	}
+
+	/* close the fd */
+	::close(_fd);
+	_fd = -1;
 out:
 	return ret;
 }
@@ -453,7 +523,7 @@ SF0X::measure()
 
 	if (ret != sizeof(cmd)) {
 		perf_count(_comms_errors);
-		log("serial transfer returned %d", ret);
+		log("write fail %d", ret);
 		return ret;
 	}
 
@@ -469,21 +539,64 @@ SF0X::collect()
 
 	perf_begin(_sample_perf);
 
-	char buf[16];
+	/* clear buffer if last read was too long ago */
+	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
+
+	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+		_linebuf_index = 0;
+	}
+
 	/* read from the sensor (uart buffer) */
-	ret = ::read(_fd, buf, sizeof(buf));
+	ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
 
-	if (ret < 1) {
-		log("error reading from sensor: %d", ret);
+	if (ret < 0) {
+		_linebuf[sizeof(_linebuf) - 1] = '\0';
+		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
-		return ret;
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+			return ret;
+
+		} else {
+			return -EAGAIN;
+		}
+	}
+
+	_linebuf_index += ret;
+
+	if (_linebuf_index >= sizeof(_linebuf)) {
+		_linebuf_index = 0;
+	}
+
+	_last_read = hrt_absolute_time();
+
+	if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
+		/* incomplete read, reschedule ourselves */
+		return -EAGAIN;
+	}
+
+	char *end;
+	float si_units;
+	bool valid;
+
+	/* enforce line ending */
+	_linebuf[sizeof(_linebuf) - 1] = '\0';
+
+	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
+		si_units = -1.0f;
+		valid = false;
+
+	} else {
+		si_units = strtod(_linebuf, &end);
+		valid = true;
 	}
 
-	printf("ret: %d, val (str): %s\n", ret, buf);
-	char* end;
-	float si_units = strtod(buf,&end);
-	printf("val (float): %8.4f\n", si_units);
+	debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
+
+	/* done with this chunk, resetting */
+	_linebuf_index = 0;
 
 	struct range_finder_report report;
 
@@ -491,7 +604,7 @@ SF0X::collect()
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
-	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
+	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
 
 	/* publish it */
 	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
@@ -519,21 +632,21 @@ SF0X::start()
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
 
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_RANGEFINDER
-	};
-	static orb_advert_t pub = -1;
-
-	if (pub > 0) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
+	// /* notify about state change */
+	// struct subsystem_info_s info = {
+	// 	true,
+	// 	true,
+	// 	true,
+	// 	SUBSYSTEM_TYPE_RANGEFINDER
+	// };
+	// static orb_advert_t pub = -1;
+
+	// if (pub > 0) {
+	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	// } else {
+	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	// }
 }
 
 void
@@ -553,11 +666,29 @@ SF0X::cycle_trampoline(void *arg)
 void
 SF0X::cycle()
 {
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd */
+		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	}
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   USEC2TICK(1100));
+			return;
+		}
+
+		if (OK != collect_ret) {
 			log("collection error");
 			/* restart the measurement state machine */
 			start();
@@ -633,7 +764,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char* port)
+start(const char *port)
 {
 	int fd;
 
@@ -653,9 +784,10 @@ start(const char* port)
 	}
 
 	/* set the poll rate to default, starts automatic data collection */
-	fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+	fd = open(RANGE_FINDER_DEVICE_PATH, 0);
 
 	if (fd < 0) {
+		warnx("device open fail");
 		goto fail;
 	}
 
@@ -749,6 +881,11 @@ test()
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
 	errx(0, "PASS");
 }
 
@@ -802,6 +939,7 @@ sf0x_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 		if (argc > 2) {
 			sf0x::start(argv[2]);
+
 		} else {
 			sf0x::start(SF0X_DEFAULT_PORT);
 		}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ad3a7f12c..2514bafee 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -58,6 +58,8 @@
 #include <drivers/drv_hrt.h>
 #include <math.h>
 
+#include <drivers/drv_range_finder.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
@@ -791,6 +793,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 		struct battery_status_s battery;
 		struct telemetry_status_s telemetry;
+		struct range_finder_report range_finder;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -851,6 +854,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_vel_sp_sub;
 		int battery_sub;
 		int telemetry_sub;
+		int range_finder_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -874,6 +878,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 
 	thread_running = true;
 
@@ -1227,6 +1232,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(TELE);
 		}
 
+		/* --- BOTTOM DISTANCE --- */
+		if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
+			log_msg.msg_type = LOG_DIST_MSG;
+			log_msg.body.log_DIST.bottom = buf.range_finder.distance;
+			log_msg.body.log_DIST.bottom_rate = 0.0f;
+			log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
+			LOGBUFFER_WRITE_AND_COUNT(DIST);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */