Merge pull request #132 from PX4/#111-px4io-integrated-mixing

#111 px4io integrated mixing

10 files changed, 259 insertions, 107 deletions

diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
index 801695f84..d97f963ab 100644
--- a/apps/px4io/Makefile
+++ b/apps/px4io/Makefile
@@ -41,7 +41,9 @@
 #
 CSRCS		= $(wildcard *.c) \
 		  ../systemlib/hx_stream.c \
-		  ../systemlib/perf_counter.c
+		  ../systemlib/perf_counter.c \
+		  ../systemlib/up_cxxinitialize.c
+CXXSRCS		= $(wildcard *.cpp)
 
 INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
 
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index d7a03b007..183634742 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -70,6 +70,8 @@ static struct px4io_report	report;
 
 static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
 
+perf_counter_t			comms_rx_errors;
+
 static void
 comms_init(void)
 {
@@ -77,6 +79,9 @@ comms_init(void)
 	fmu_fd = open("/dev/ttyS1", O_RDWR);
 	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
 
+	comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
+	hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
+
 	/* default state in the report to FMU */
 	report.i2f_magic = I2F_MAGIC;
 
@@ -110,6 +115,7 @@ comms_main(void)
 		if (fds.revents & POLLIN) {
 			char buf[32];
 			ssize_t count = read(fmu_fd, buf, sizeof(buf));
+
 			for (int i = 0; i < count; i++)
 				hx_stream_rx(stream, buf[i]);
 		}
@@ -123,7 +129,8 @@ comms_main(void)
 		/* should we send a report to the FMU? */
 		now = hrt_absolute_time();
 		delta = now - last_report_time;
-		if ((delta > FMU_MIN_REPORT_INTERVAL) && 
+
+		if ((delta > FMU_MIN_REPORT_INTERVAL) &&
 		    (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
 
 			system_state.fmu_report_due = false;
@@ -132,6 +139,7 @@ comms_main(void)
 			/* populate the report */
 			for (unsigned i = 0; i < system_state.rc_channels; i++)
 				report.rc_channel[i] = system_state.rc_channel_data[i];
+
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
@@ -168,19 +176,17 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
-		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
+		system_state.fmu_channel_data[i] = cmd->output_control[i];
 
 	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
-	if (system_state.arm_ok && !cmd->arm_ok) {
+	if (system_state.arm_ok && !cmd->arm_ok)
 		system_state.armed = false;
-	}
 
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.mixer_use_fmu = true;
 	system_state.fmu_data_received = true;
 
-
 	/* handle changes signalled by FMU */
 //	if (!system_state.arm_ok && system_state.armed)
 //		system_state.armed = false;
@@ -203,13 +209,12 @@ comms_handle_command(const void *buffer, size_t length)
 				break;
 			}
 		}
-		system_state.relays[i] != cmd->relay_state[i]
+		system_state.relays[i] = cmd->relay_state[i];
 	}
 
 	irqrestore(flags);
 }
 
-
 static void
 comms_handle_frame(void *arg, const void *buffer, size_t length)
 {
@@ -222,11 +227,17 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 		case F2I_MAGIC:
 			comms_handle_command(buffer, length);
 			break;
+
 		case F2I_CONFIG_MAGIC:
 			comms_handle_config(buffer, length);
 			break;
+
+		case F2I_MIXER_MAGIC:
+			mixer_handle_text(buffer, length);
+			break;
+
 		default:
-		    	frame_bad++;
+			frame_bad++;
 			break;
 		}
 	}
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 3b3782918..43e811ab0 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -90,6 +90,7 @@ controls_main(void)
 
 		if (fds[0].revents & POLLIN)
 			locked |= dsm_input();
+
 		if (fds[1].revents & POLLIN)
 			locked |= sbus_input();
 
@@ -139,6 +140,7 @@ ppm_input(void)
 
 		/* PPM data exists, copy it */
 		system_state.rc_channels = ppm_decoded_channels;
+
 		for (unsigned i = 0; i < ppm_decoded_channels; i++)
 			system_state.rc_channel_data[i] = ppm_buffer[i];
 
@@ -150,5 +152,5 @@ ppm_input(void)
 
 		/* trigger an immediate report to the FMU */
 		system_state.fmu_report_due = true;
-	}	
+	}
 }
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 2611f3a03..560ef47d9 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -47,7 +47,7 @@
 #include <termios.h>
 
 #include <systemlib/ppm_decode.h>
- 
+
 #include <drivers/drv_hrt.h>
 
 #define DEBUG
@@ -97,6 +97,7 @@ dsm_init(const char *device)
 		dsm_guess_format(true);
 
 		debug("DSM: ready");
+
 	} else {
 		debug("DSM: open failed");
 	}
@@ -118,7 +119,7 @@ dsm_input(void)
 	 * frame transmission time is ~1.4ms.
 	 *
 	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls, 
+	 * and if an interval of more than 5ms passes between calls,
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -126,6 +127,7 @@ dsm_input(void)
 	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
+
 	if ((now - last_rx_time) > 5000) {
 		if (partial_frame_count > 0) {
 			dsm_frame_drops++;
@@ -142,6 +144,7 @@ dsm_input(void)
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
 		goto out;
+
 	last_rx_time = now;
 
 	/*
@@ -153,7 +156,7 @@ dsm_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < DSM_FRAME_SIZE)
-	 	goto out;
+		goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -164,7 +167,7 @@ dsm_input(void)
 
 out:
 	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
 	 */
 	return (now - last_frame_time) < 200000;
 }
@@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
 		/* if the channel decodes, remember the assigned number */
 		if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
 			cs10 |= (1 << channel);
+
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
@@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
 	if (samples++ < 5)
 		return;
 
-	/* 
+	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
 	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
 	 *
@@ -233,7 +237,7 @@ dsm_guess_format(bool reset)
 	 *     See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
 	 *     of this issue.
 	 */
-	static uint32_t masks[] = { 
+	static uint32_t masks[] = {
 		0x3f,	/* 6 channels (DX6) */
 		0x7f,	/* 7 channels (DX7) */
 		0xff,	/* 8 channels (DX8) */
@@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
 
 		if (cs10 == masks[i])
 			votes10++;
+
 		if (cs11 == masks[i])
 			votes11++;
 	}
+
 	if ((votes11 == 1) && (votes10 == 0)) {
 		channel_shift = 11;
 		debug("DSM: detected 11-bit format");
 		return;
 	}
+
 	if ((votes10 == 1) && (votes11 == 0)) {
 		channel_shift = 10;
 		debug("DSM: detected 10-bit format");
@@ -270,13 +277,13 @@ static void
 dsm_decode(hrt_abstime frame_time)
 {
 
-/*
-	debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-	      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-	      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
-*/
 	/*
-	 * If we have lost signal for at least a second, reset the 
+		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	*/
+	/*
+	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
 	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
@@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time)
 	}
 
 	/*
-	 * The encoding of the first two bytes is uncertain, so we're 
+	 * The encoding of the first two bytes is uncertain, so we're
 	 * going to ignore them for now.
 	 *
 	 * Each channel is a 16-bit unsigned value containing either a 10-
@@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time)
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		if (channel_shift == 11)
 			value /= 2;
+
 		value += 998;
 
-		/* 
+		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
 		 * account the different ideas about channel assignement that we have.
 		 *
@@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
 		case 0:
 			channel = 2;
 			break;
+
 		case 1:
 			channel = 0;
 			break;
+
 		case 2:
 			channel = 1;
+
 		default:
 			break;
 		}
+
 		system_state.rc_channel_data[channel] = value;
 	}
 
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.cpp
index f02e98ae4..d21b3a898 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mixer.c
+ * @file mixer.cpp
  *
  * Control channel input/output mixer and failsafe.
  */
@@ -49,8 +49,12 @@
 #include <fcntl.h>
 
 #include <drivers/drv_pwm_output.h>
+#include <systemlib/mixer/mixer.h>
 
+extern "C" {
+//#define DEBUG
 #include "px4io.h"
+}
 
 /*
  * Count of periodic calls in which we have no FMU input.
@@ -58,28 +62,23 @@
 static unsigned fmu_input_drops;
 #define FMU_INPUT_DROP_LIMIT	20
 
-/*
- * Update a mixer based on the current control signals.
- */
-static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
-
 /* current servo arm/disarm state */
 bool mixer_servos_armed = false;
 
-/*
- * Each mixer consumes a set of inputs and produces a single output.
- */
-struct mixer {
-	uint16_t current_value;
-	/* XXX more config here */
-} mixers[IO_SERVO_COUNT];
+/* selected control values and count for mixing */
+static uint16_t *control_values;
+static int control_count;
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+static MixerGroup mixer_group(mixer_callback, 0);
 
 void
 mixer_tick(void)
 {
-	uint16_t *control_values;
-	int control_count;
-	int i;
 	bool should_arm;
 
 	/*
@@ -87,7 +86,7 @@ mixer_tick(void)
 	 */
 	if (system_state.mixer_use_fmu) {
 		/* we have recent control data from the FMU */
-		control_count = PX4IO_OUTPUT_CHANNELS;
+		control_count = PX4IO_CONTROL_CHANNELS;
 		control_values = &system_state.fmu_channel_data[0];
 
 		/* check that we are receiving fresh data from the FMU */
@@ -98,6 +97,7 @@ mixer_tick(void)
 			if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
 				system_state.mixer_use_fmu = false;
 			}
+
 		} else {
 			fmu_input_drops = 0;
 			system_state.fmu_data_received = false;
@@ -115,17 +115,31 @@ mixer_tick(void)
 	}
 
 	/*
-	 * Tickle each mixer, if we have control data.
+	 * Run the mixers if we have any control data at all.
 	 */
 	if (control_count > 0) {
-		for (i = 0; i < IO_SERVO_COUNT; i++) {
-			mixer_update(i, control_values, control_count);
+		float	outputs[IO_SERVO_COUNT];
+		unsigned mixed;
+
+		/* mix */
+		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+		/* scale to PWM and update the servo outputs as required */
+		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+			if (i < mixed) {
+				/* scale to servo output */
+				system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
+
+			} else {
+				/* set to zero to inhibit PWM pulse output */
+				system_state.servos[i] = 0;
+			}
 
 			/*
 			 * If we are armed, update the servo output.
 			 */
 			if (system_state.armed && system_state.arm_ok)
-				up_pwm_servo_set(i, mixers[i].current_value);
+				up_pwm_servo_set(i, system_state.servos[i]);
 		}
 	}
 
@@ -133,6 +147,7 @@ mixer_tick(void)
 	 * Decide whether the servos should be armed right now.
 	 */
 	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
@@ -145,13 +160,73 @@ mixer_tick(void)
 	}
 }
 
-static void
-mixer_update(int mixer, uint16_t *inputs, int input_count)
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
 {
-	/* simple passthrough for now */
-	if (mixer < input_count) {
-		mixers[mixer].current_value = inputs[mixer];
-	} else {
-		mixers[mixer].current_value = 0;
-	}
+	/* if the control index refers to an input that's not valid, we can't return it */
+	if (control_index >= control_count)
+		return -1;
+
+	/* scale from current PWM units (1000-2000) to mixer input values */
+	control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+
+	return 0;
 }
+
+static char mixer_text[256];
+static unsigned mixer_text_length = 0;
+
+void
+mixer_handle_text(const void *buffer, size_t length)
+{
+
+	px4io_mixdata	*msg = (px4io_mixdata *)buffer;
+
+	debug("mixer text %u", length);
+
+	if (length < sizeof(px4io_mixdata))
+		return;
+
+	unsigned	text_length = length - sizeof(px4io_mixdata);
+
+	switch (msg->action) {
+	case F2I_MIXER_ACTION_RESET:
+		debug("reset");
+		mixer_group.reset();
+		mixer_text_length = 0;
+
+		/* FALLTHROUGH */
+	case F2I_MIXER_ACTION_APPEND:
+		debug("append %d", length);
+
+		/* check for overflow - this is really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
+			return;
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		debug("buflen %u", mixer_text_length);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+			debug("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		break;
+	}
+}
\ No newline at end of file
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index c704b1201..afbf09cd3 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -41,31 +41,27 @@
 
 #pragma once
 
-#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_CONTROL_CHANNELS	8
 #define PX4IO_INPUT_CHANNELS	12
 #define PX4IO_RELAY_CHANNELS	4
 
 #pragma pack(push, 1)
 
-/* command from FMU to IO */
+/**
+ * Periodic command from FMU to IO.
+ */
 struct px4io_command {
 	uint16_t	f2i_magic;
-#define F2I_MAGIC	0x636d
+#define F2I_MAGIC		0x636d
 
-	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	output_control[PX4IO_CONTROL_CHANNELS];
 	bool		relay_state[PX4IO_RELAY_CHANNELS];
 	bool		arm_ok;
 };
 
-/* config message from FMU to IO */
-struct px4io_config {
-	uint16_t f2i_config_magic;
-#define F2I_CONFIG_MAGIC 0x6366
-
-	/* XXX currently nothing here */
-};
-
-/* report from IO to FMU */
+/**
+ * Periodic report from IO to FMU
+ */
 struct px4io_report {
 	uint16_t	i2f_magic;
 #define I2F_MAGIC		0x7570
@@ -75,4 +71,34 @@ struct px4io_report {
 	uint8_t		channel_count;
 };
 
+/**
+ * As-needed config message from FMU to IO
+ */
+struct px4io_config {
+	uint16_t	f2i_config_magic;
+#define F2I_CONFIG_MAGIC	 0x6366
+
+	/* XXX currently nothing here */
+};
+
+/**
+ * As-needed mixer data upload.
+ *
+ * This message adds text to the mixer text buffer; the text
+ * buffer is drained as the definitions are consumed.
+ */
+struct px4io_mixdata {
+	uint16_t	f2i_mixer_magic;
+#define F2I_MIXER_MAGIC		0x6d74
+
+	uint8_t		action;
+#define F2I_MIXER_ACTION_RESET		0
+#define F2I_MIXER_ACTION_APPEND		1
+
+	char		text[0];	/* actual text size may vary */
+};
+
+/* maximum size is limited by the HX frame size */
+#define F2I_MIXER_MAX_TEXT	(HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
+
 #pragma pack(pop)
\ No newline at end of file
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index a3ac9e3e7..74362617d 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -55,10 +55,15 @@
 
 __EXPORT int user_start(int argc, char *argv[]);
 
+extern void up_cxxinitialize(void);
+
 struct sys_state_s 	system_state;
 
 int user_start(int argc, char *argv[])
 {
+	/* run C++ ctors before we go any further */
+	up_cxxinitialize();
+
 	/* reset all to zero */
 	memset(&system_state, 0, sizeof(system_state));
 
@@ -83,7 +88,7 @@ int user_start(int argc, char *argv[])
 	up_pwm_servo_init(0xff);
 
 	/* start the flight control signal handler */
-	task_create("FCon", 
+	task_create("FCon",
 		    SCHED_PRIORITY_DEFAULT,
 		    1024,
 		    (main_t)controls_main,
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 45b7cf847..bd91eba0e 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -31,11 +31,11 @@
  *
  ****************************************************************************/
 
- /**
-  * @file px4io.h
-  *
-  * General defines and structures for the PX4IO module firmware.
-  */
+/**
+ * @file px4io.h
+ *
+ * General defines and structures for the PX4IO module firmware.
+ */
 
 #include <nuttx/config.h>
 
@@ -66,8 +66,7 @@
 /*
  * System state structure.
  */
-struct sys_state_s 
-{
+struct sys_state_s {
 
 	bool		armed;			/* IO armed */
 	bool		arm_ok;			/* FMU says OK to arm */
@@ -82,7 +81,12 @@ struct sys_state_s
 	/*
 	 * Control signals from FMU.
 	 */
-	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
+	uint16_t	fmu_channel_data[PX4IO_CONTROL_CHANNELS];
+
+	/*
+	 * Mixed servo outputs
+	 */
+	uint16_t	servos[IO_SERVO_COUNT];
 
 	/*
 	 * Relay controls
@@ -136,8 +140,8 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
 #define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
 
 #define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s))
-#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s))
+#define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+#define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
 #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
 
@@ -149,6 +153,7 @@ extern volatile int	timers[TIMER_NUM_TIMERS];
  * Mixer
  */
 extern void	mixer_tick(void);
+extern void	mixer_handle_text(const void *buffer, size_t length);
 
 /*
  * Safety switch/LED.
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 60d20905a..93596ca75 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -31,9 +31,9 @@
  *
  ****************************************************************************/
 
- /**
-  * @file Safety button logic.
-  */
+/**
+ * @file Safety button logic.
+ */
 
 #include <nuttx/config.h>
 #include <stdio.h>
@@ -95,13 +95,13 @@ safety_init(void)
 static void
 safety_check_button(void *arg)
 {
-	/* 
+	/*
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
 	 */
 	safety_button_pressed = BUTTON_SAFETY;
 
-	if(safety_button_pressed) {
+	if (safety_button_pressed) {
 		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
 	}
 
@@ -109,34 +109,42 @@ safety_check_button(void *arg)
 	if (safety_button_pressed && !system_state.armed) {
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
+
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to armed state and notify the FMU */
 			system_state.armed = true;
 			counter++;
 			system_state.fmu_report_due = true;
 		}
-	/* Disarm quickly */
+
+		/* Disarm quickly */
+
 	} else if (safety_button_pressed && system_state.armed) {
 		if (counter < DISARM_COUNTER_THRESHOLD) {
 			counter++;
+
 		} else if (counter == DISARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
 			system_state.armed = false;
 			counter++;
 			system_state.fmu_report_due = true;
 		}
+
 	} else {
 		counter = 0;
 	}
 
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_SAFE;
+
 	if (system_state.armed) {
 		if (system_state.arm_ok) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
+
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
+
 	} else if (system_state.arm_ok) {
 		pattern = LED_PATTERN_FMU_ARMED;
 	}
@@ -167,8 +175,10 @@ failsafe_blink(void *arg)
 	/* blink the failsafe LED if we don't have FMU input */
 	if (!system_state.mixer_use_fmu) {
 		failsafe = !failsafe;
+
 	} else {
 		failsafe = false;
 	}
+
 	LED_AMBER(failsafe);
 }
\ No newline at end of file
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index a8f628a84..9679f657b 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -88,6 +88,7 @@ sbus_init(const char *device)
 		last_rx_time = hrt_absolute_time();
 
 		debug("Sbus: ready");
+
 	} else {
 		debug("Sbus: open failed");
 	}
@@ -109,7 +110,7 @@ sbus_input(void)
 	 * frame transmission time is ~2ms.
 	 *
 	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 3ms passes between calls, 
+	 * and if an interval of more than 3ms passes between calls,
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -117,6 +118,7 @@ sbus_input(void)
 	 * if we didn't drop bytes...
 	 */
 	now = hrt_absolute_time();
+
 	if ((now - last_rx_time) > 3000) {
 		if (partial_frame_count > 0) {
 			sbus_frame_drops++;
@@ -133,6 +135,7 @@ sbus_input(void)
 	/* if the read failed for any reason, just give up here */
 	if (ret < 1)
 		goto out;
+
 	last_rx_time = now;
 
 	/*
@@ -144,7 +147,7 @@ sbus_input(void)
 	 * If we don't have a full frame, return
 	 */
 	if (partial_frame_count < SBUS_FRAME_SIZE)
-	 	goto out;
+		goto out;
 
 	/*
 	 * Great, it looks like we might have a frame.  Go ahead and
@@ -155,7 +158,7 @@ sbus_input(void)
 
 out:
 	/*
-	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked' 
+	 * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
 	 */
 	return (now - last_frame_time) < 200000;
 }
@@ -177,23 +180,23 @@ struct sbus_bit_pick {
 	uint8_t mask;
 	uint8_t lshift;
 };
-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} },
-/*  3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
-/*  4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
-/*  5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03,  9} },
-/*  6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
-/*  7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00,  0} },
-/*  8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00,  0} },
-/*  9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00,  0} },
-/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
-/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00,  0} },
-/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00,  0} },
-/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03,  9} },
-/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00,  0} },
-/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00,  0} }
+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} },
+	/*  3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/*  4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/*  5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03,  9} },
+	/*  6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/*  7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00,  0} },
+	/*  8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00,  0} },
+	/*  9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00,  0} },
+	/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
+	/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00,  0} },
+	/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00,  0} },
+	/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03,  9} },
+	/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00,  0} },
+	/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00,  0} }
 };
 
 static void
@@ -213,8 +216,8 @@ sbus_decode(hrt_abstime frame_time)
 	/* we have received something we think is a frame */
 	last_frame_time = frame_time;
 
-	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? 
-		SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+	unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
+			     SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
 
 	/* use the decoder matrix to extract channel data */
 	for (unsigned channel = 0; channel < chancount; channel++) {
@@ -232,6 +235,7 @@ sbus_decode(hrt_abstime frame_time)
 				value |= piece;
 			}
 		}
+
 		/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
 		system_state.rc_channel_data[channel] = (value / 2) + 998;
 	}
@@ -245,7 +249,7 @@ sbus_decode(hrt_abstime frame_time)
 	system_state.rc_channels = chancount;
 
 	/* and note that we have received data from the R/C controller */
-	system_state.rc_channels_timestamp = frame_time;	
+	system_state.rc_channels_timestamp = frame_time;
 
 	/* trigger an immediate report to the FMU */
 	system_state.fmu_report_due = true;