diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.sensors | 13 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_default.mk | 1 | ||||
-rw-r--r-- | src/drivers/drv_range_finder.h | 1 | ||||
-rw-r--r-- | src/drivers/mb1230serial/mb1230serial.c | 228 | ||||
-rw-r--r-- | src/drivers/mb1230serial/module.mk | 4 |
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
|