MaxBotix MB1230 sonar support

5 files changed, 245 insertions, 2 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 739df7ac0..10da72927 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -6,6 +6,11 @@
 ms5611 start
 adc start
 
+if [ _$MB_SONAR_TTY = _ ]
+then
+    set MB_SONAR_TTY /dev/ttyS2
+fi
+
 if mpu6000 -X start
 then
 fi
@@ -68,11 +73,15 @@ else
 	fi
 fi
 
-# Check for flow sensor
-if px4flow start
+# Start MaxBotix sonar on serial
+if mb1230serial start $MB_SONAR_TTY
 then
+ echo [init] Using MaxBotix sonar at $MB_SONAR_TTY
+else
+ echo error starting MaxBotix sonar at $MB_SONAR_TTY
 fi
 
+
 #
 # Start the sensor collection task.
 # IMPORTANT: this also loads param offsets
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index fdb7e2396..90dd73e0a 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -43,6 +43,7 @@ MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/pca8574
 #MODULES		+= drivers/px4flow
 
+MODULES		+= drivers/mb1230serial
 
 # Needs to be burned to the ground and re-written; for now,
 # just don't build it.
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 0f8362f58..7c3e522f5 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -48,6 +48,7 @@
 
 enum RANGE_FINDER_TYPE {
 	RANGE_FINDER_TYPE_LASER = 0,
+	RANGE_FINDER_TYPE_ULTRASONIC = 1,
 };
 
 /**
diff --git a/src/drivers/mb1230serial/mb1230serial.c b/src/drivers/mb1230serial/mb1230serial.c
new file mode 100644
index 000000000..91282fc18
--- /dev/null
+++ b/src/drivers/mb1230serial/mb1230serial.c
@@ -0,0 +1,228 @@
+/**

+ * The deamon app only briefly exists to start

+ * the background job. The stack size assigned in the

+ * Makefile does only apply to this management task.

+ * 

+ * The actual stack size should be set in the call

+ * to task_create().

+ * http://nuttx.org/Documentation/NuttxUserGuide.html#taskcreate

+ */

+#include <nuttx/config.h>

+#include <unistd.h>

+#include <stdlib.h>

+#include <stdio.h>

+#include <string.h>

+#include <stdbool.h>

+#include <fcntl.h>

+#include <float.h>

+#include <nuttx/sched.h>

+#include <sys/prctl.h>

+#include <termios.h>

+#include <errno.h>

+#include <limits.h>

+#include <math.h>

+#include <uORB/uORB.h>

+#include <drivers/drv_hrt.h>

+#include <drivers/drv_range_finder.h>

+#include <systemlib/perf_counter.h>

+#include <systemlib/systemlib.h>

+#include <systemlib/err.h>

+#include <poll.h>

+

+/**

+ * Minimal and maximal distances are get from

+ * http://www.maxbotix.com/documents/XL-MaxSonar-EZ_Datasheet.pdf

+ */

+#define MINIMAL_DISTANCE 20

+#define MAXIMAL_DISTANCE 700

+

+/**

+ * Experimentally found scale value

+ */

+#define DISTANCE_SCALE 0.01022f

+

+

+__EXPORT int mb1230serial_main(int argc, char *argv[]);

+int mb1230serial_thread_main(int argc, char *argv[]);

+

+static bool thread_should_run = true;	 /**< Daemon running flag */

+static bool thread_running = false;	 /**< Daemon status flag */

+static int daemon_task;			 /**< Handle of daemon task / thread */

+

+/**

+ * Print the correct usage.

+ */

+void

+usage(const char *reason)

+{

+	if (reason)

+		fprintf(stderr, "%s\n", reason);

+	fprintf(stderr, "usage: mb1230serial {start device_path | stop}\n\n");

+	exit(1);

+}

+

+int mb1230serial_main(int argc, char *argv[])

+{

+	if (argc < 1)

+		usage("missing command");

+

+	if (!strcmp(argv[1], "start"))

+	{

+		if (thread_running)

+		{

+			warnx("already running\n");

+			/* this is not an error */

+			exit(0);

+		}

+

+		if (argc < 3)

+		{

+			usage("missing device path");

+		}

+

+		thread_should_run = true;

+		daemon_task = task_spawn_cmd("mb1230serial",

+					 SCHED_DEFAULT,

+					 SCHED_PRIORITY_MAX - 5,

+					 2000,

+					 mb1230serial_thread_main,

+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);

+		exit(0);

+	}

+

+	if (!strcmp(argv[1], "stop"))

+	{

+		thread_should_run = false;

+		exit(0);

+	}

+

+	usage("unrecognized command");

+	exit(1);

+}

+

+void cfsetraw(struct termios *termios_s)

+{

+	termios_s->c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP

+							  | INLCR | IGNCR | ICRNL | IXON);

+	termios_s->c_oflag &= ~OPOST;

+	termios_s->c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);

+	termios_s->c_cflag &= ~(CSIZE | PARENB | CSTOPB);

+	termios_s->c_cflag |= CS8 | CLOCAL;

+}

+

+void setup_serial(int fd)

+{

+	struct termios options;

+	int termios_state;

+

+	if ((termios_state = tcgetattr(fd, &options)) < 0)

+	{

+		close(fd);

+		err(-1, "ERR GET CONF: %d\n", termios_state);

+	}

+

+	cfsetraw(&options);

+	cfsetspeed(&options, B9600);

+

+	if ((termios_state = tcsetattr(fd, TCSANOW, &options)) < 0)

+	{

+		close(fd);

+		err(-1, "ERR SET CONF: %d\n", termios_state);

+	}

+}

+

+int mb1230serial_thread_main(int argc, char *argv[])

+{

+	warnx("opening port %s", argv[1]);

+

+	int serial_fd = open(argv[1], O_RDONLY | O_NOCTTY);

+

+	if (serial_fd < 0)

+	{

+		err(1, "failed to open port: %s", argv[1]);

+	}

+

+	setup_serial(serial_fd);

+

+	char R = 0;

+	char raw_data[3];

+	int nread = 0;

+	int distance;

+

+	struct range_finder_report data = {

+		.error_count = 0,

+		.type = RANGE_FINDER_TYPE_ULTRASONIC,

+		.minimum_distance = MINIMAL_DISTANCE,

+		.maximum_distance = MAXIMAL_DISTANCE,

+		.valid = 1

+	};

+

+	orb_advert_t range_finder_pub = -1;

+

+	thread_running = true;

+

+	while (thread_should_run)

+	{

+

+		/*This runs at the rate of the sensors */

+		struct pollfd fds[] = {

+				{ .fd = serial_fd, .events = POLLIN }

+		};

+

+		/* wait for a sensor update, check for exit condition  */

+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

+

+		if (ret < 0)

+		{

+			/* poll error, ignore */

+		}

+		else if (ret == 0)

+		{

+			/* no return value, ignore */

+		}

+		else

+		{

+			if (fds[0].revents & POLLIN)

+			{

+				if (R != 'R')

+				{

+					read(serial_fd, &R, sizeof(R));

+				}

+				else

+				{

+					nread += read(serial_fd, &raw_data[nread], sizeof(raw_data) - nread);

+					if (nread == 3)

+					{

+						nread = 0;

+						R = 0;

+

+						distance =

+								(raw_data[0] - '0') * 100 +

+								(raw_data[1] - '0') * 10 +

+								(raw_data[2] - '0');

+

+						if (distance >= MINIMAL_DISTANCE && distance <= MAXIMAL_DISTANCE) {

+							data.timestamp = hrt_absolute_time();

+							data.distance = (float)distance * DISTANCE_SCALE;

+

+							//printf("%llu: %.3f\n", data.timestamp, data.distance);

+

+							if (range_finder_pub > 0) {

+								orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data);

+							} else {

+								range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data);

+							}

+						}

+					}

+				}

+			}

+		}

+	}

+

+	warnx("exiting");

+	thread_running = false;

+

+	fflush(stdout);

+	return 0;

+}

+

diff --git a/src/drivers/mb1230serial/module.mk b/src/drivers/mb1230serial/module.mk
new file mode 100644
index 000000000..2261b8c4e
--- /dev/null
+++ b/src/drivers/mb1230serial/module.mk
@@ -0,0 +1,4 @@
+MODULE_COMMAND		= mb1230serial

+MODULE_STACKSIZE	= 12000

+

+SRCS			= mb1230serial.c