6 files changed, 20 insertions, 15 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 66982b707..49ad80943 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -53,6 +53,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <termios.h>
 #include <fcntl.h>
 
 #include <arch/board/board.h>
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index a93ef9cb8..375336730 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -47,6 +47,7 @@
 #include <errno.h>
 #include <string.h>
 #include <poll.h>
+#include <termios.h>
 
 #include <nuttx/clock.h>
 
@@ -69,20 +70,29 @@ static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
-void
+static void
 comms_init(void)
 {
 	/* initialise the FMU interface */
-	fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
+	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
 
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
+
+	struct termios t;
+
+	/* 115200bps, no parity, one stop bit */
+	tcgetattr(fmu_fd, &t);
+	cfsetspeed(&t, 115200);
+	t.c_cflag &= ~(CSTOPB | PARENB);
+	tcsetattr(fmu_fd, TCSANOW, &t);
 }
 
 void
 comms_main(void)
 {
+	comms_init();
 
 	struct pollfd fds;
 	fds.fd = fmu_fd;
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index d8ce06e42..483e9fe4d 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -49,7 +49,6 @@
 #include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
-#include <drivers/drv_hrt.h>
 
 #include <systemlib/ppm_decode.h>
 
@@ -125,12 +124,11 @@ mixer_tick(void)
 		/* we have no control input */
 		control_count = 0;
 	}
-
 	/*
 	 * Tickle each mixer, if we have control data.
 	 */
 	if (control_count > 0) {
-		for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
+		for (i = 0; i < IO_SERVO_COUNT; i++) {
 			mixer_update(i, control_values, control_count);
 
 			/*
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 34b9c8c49..77524797f 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -78,6 +78,9 @@ int user_start(int argc, char *argv[])
 	/* start the safety switch handler */
 	safety_init();
 
+	/* configure the first 8 PWM outputs (i.e. all of them) */
+	up_pwm_servo_init(0xff);
+
 	/* start the flight control signal handler */
 	task_create("FCon", 
 		    SCHED_PRIORITY_DEFAULT,
@@ -86,13 +89,6 @@ int user_start(int argc, char *argv[])
 		    NULL);
 
 
-	/* initialise the FMU communications interface */
-	comms_init();
-
-	/* configure the first 8 PWM outputs (i.e. all of them) */
-	/* note, must do this after comms init to steal back PA0, which is CTS otherwise */
-	up_pwm_servo_init(0xff);
-
 	struct mallinfo minfo = mallinfo();
 	lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
 
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 5a26d355a..483b9bcc8 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -162,7 +162,6 @@ extern void	safety_init(void);
 /*
  * FMU communications
  */
-extern void	comms_init(void);
 extern void	comms_main(void) __attribute__((noreturn));
 
 /*
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index 3d8d7c18b..c3949f2b0 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -53,6 +53,7 @@
 #include "debug.h"
 
 #define SBUS_FRAME_SIZE		25
+#define SBUS_INPUT_CHANNELS	16
 
 static int sbus_fd = -1;
 
@@ -169,7 +170,7 @@ struct sbus_bit_pick {
 	uint8_t mask;
 	uint8_t lshift;
 };
-static struct sbus_bit_pick sbus_decoder[][3] = {	
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {	
 /*  0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
 /*  1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
 /*  2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
@@ -211,7 +212,7 @@ sbus_decode(hrt_abstime frame_time)
 		unsigned value = 0;
 
 		for (unsigned pick = 0; pick < 3; pick++) {
-			struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
+			const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
 
 			if (decode->mask != 0) {
 				unsigned piece = frame[1 + decode->byte];