Fresh import of the PX4 firmware sources.

7 files changed, 917 insertions, 0 deletions

diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
new file mode 100644
index 000000000..06be90a0c
--- /dev/null
+++ b/apps/px4io/Makefile
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build the px4io application.
+#
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
new file mode 100644
index 000000000..19802bf4f
--- /dev/null
+++ b/apps/px4io/comms.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FMU communication for the PX4IO module.
+ */
+
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <debug.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+
+#include <nuttx/clock.h>
+
+#include <arch/board/up_hrt.h>
+#include <systemlib/hx_stream.h>
+#include <systemlib/perf_counter.h>
+
+#include "px4io.h"
+
+#define FMU_MIN_REPORT_INTERVAL	  5000	/*  5ms */
+#define FMU_MAX_REPORT_INTERVAL	100000	/* 100ms */
+
+static int			fmu_fd;
+static hx_stream_t		stream;
+
+static struct px4io_report	report;
+
+static void			comms_handle_frame(void *arg, const void *buffer, size_t length);
+
+void
+comms_init(void)
+{
+	fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK);
+	if (fmu_fd < 0)
+		lib_lowprintf("COMMS: fmu open failed %d\n", errno);
+
+	stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
+
+	report.i2f_magic = I2F_MAGIC;
+}
+
+void
+comms_check(void)
+{
+	static hrt_abstime last_report_time;
+	hrt_abstime now, delta;
+	uint8_t c;
+
+	/* should we send a report to the FMU? */
+	now = hrt_absolute_time();
+	delta = now - last_report_time;
+	if ((delta > FMU_MIN_REPORT_INTERVAL) && 
+		(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
+
+		system_state.fmu_report_due = false;
+		last_report_time = now;
+
+		/* 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;
+
+		/* and send it */
+		hx_stream_send(stream, &report, sizeof(report));
+	}
+
+	/* feed any received bytes to the HDLC receive engine */
+	while (read(fmu_fd, &c, 1) == 1)
+		hx_stream_rx(stream, c);
+}
+
+static void
+comms_handle_frame(void *arg, const void *buffer, size_t length)
+{
+	struct px4io_command *cmd;
+
+	/* make sure it's what we are expecting */
+	if (length != sizeof(struct px4io_command))
+		return;
+
+	cmd = (struct px4io_command *)buffer;
+
+	/* fetch new PWM output values */
+	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+
+	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;
+
+	/* XXX do relay changes here */	
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
+		system_state.relays[i] = cmd->relay_state[i];
+}
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
new file mode 100644
index 000000000..9dc1fdcba
--- /dev/null
+++ b/apps/px4io/mixer.c
@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Control channel input/output mixer and failsafe.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <errno.h>
+#include <fcntl.h>
+
+#include <arch/board/drv_ppm_input.h>
+#include <arch/board/drv_pwm_servo.h>
+#include <arch/board/up_hrt.h>
+
+#include "px4io.h"
+
+#ifdef CONFIG_DISABLE_MQUEUE
+# error Mixer requires message queues - set CONFIG_DISABLE_MQUEUE=n and try again
+#endif
+
+static mqd_t	input_queue;
+
+/*
+ * Count of periodic calls in which we have no data.
+ */
+static unsigned mixer_input_drops;
+#define MIXER_INPUT_DROP_LIMIT	10
+
+/*
+ * Count of periodic calls in which we have no FMU input.
+ */
+static unsigned fmu_input_drops;
+#define FMU_INPUT_DROP_LIMIT	10
+
+/*
+ * HRT periodic call used to check for control input data.
+ */
+static struct hrt_call mixer_input_call;
+
+/*
+ * Mixer periodic tick.
+ */
+static void	mixer_tick(void *arg);
+
+/*
+ * Collect RC input data from the controller source(s).
+ */
+static void	mixer_get_rc_input(void);
+
+/*
+ * Update a mixer based on the current control signals.
+ */
+static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
+
+/* servo driver handle */
+int mixer_servo_fd;
+
+/* current servo arm/disarm state */
+bool mixer_servos_armed;
+
+/*
+ * 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];
+
+int
+mixer_init(const char *mq_name)
+{
+	/* open the control input queue; this should always exist */
+	input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK);
+	ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL);
+
+	/* open the servo driver */
+	mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY);
+	ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL);
+
+	/* look for control data at 50Hz */
+	hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL);
+
+	return 0;
+}
+
+static void
+mixer_tick(void *arg)
+{
+	uint16_t *control_values;
+	int control_count;
+	int i;
+	bool should_arm;
+
+	/*
+	 * Start by looking for R/C control inputs.
+	 * This updates system_state with any control inputs received.
+	 */
+	mixer_get_rc_input();
+
+	/*
+	 * Decide which set of inputs we're using.
+	 */
+	if (system_state.mixer_use_fmu) {
+		/* we have recent control data from the FMU */
+		control_count = PX4IO_OUTPUT_CHANNELS;
+		control_values = &system_state.fmu_channel_data[0];
+
+		/* check that we are receiving fresh data from the FMU */
+		if (!system_state.fmu_data_received) {
+			fmu_input_drops++;
+
+			/* too many frames without FMU input, time to go to failsafe */
+			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;
+		}
+
+	} else if (system_state.rc_channels > 0) {
+		/* we have control data from an R/C input */
+		control_count = system_state.rc_channels;
+		control_values = &system_state.rc_channel_data[0];
+
+	} else {
+		/* 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++) {
+			mixer_update(i, control_values, control_count);
+
+			/*
+			 * If we are armed, update the servo output.
+			 */
+			if (system_state.armed)
+				ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value);
+		}
+
+	}
+
+	/*
+	 * Decide whether the servos should be armed right now.
+	 */
+	should_arm = system_state.armed && (control_count > 0);
+	if (should_arm && !mixer_servos_armed) {
+		/* need to arm, but not armed */
+		ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0);
+		mixer_servos_armed = true;
+
+	} else if (!should_arm && mixer_servos_armed) {
+		/* armed but need to disarm*/
+		ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0);		
+		mixer_servos_armed = false;
+	}
+}
+
+static void
+mixer_get_rc_input(void)
+{
+	ssize_t len;
+
+	/*
+	 * Pull channel data from the message queue into the system state structure.
+	 *
+	 */
+	len = mq_receive(input_queue, &system_state.rc_channel_data, sizeof(system_state.rc_channel_data), NULL);
+
+	/*
+	 * If we have data, update the count and status.
+	 */
+	if (len > 0) {
+		system_state.rc_channels = len / sizeof(system_state.rc_channel_data[0]);
+		mixer_input_drops = 0;
+
+		system_state.fmu_report_due = true;
+	} else {
+		/*
+		 * No data; count the 'frame drops' and once we hit the limit
+		 * assume that we have lost input.
+		 */
+		if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) {
+			mixer_input_drops++;
+
+			/* if we hit the limit, stop pretending we have input and let the FMU know */
+			if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) {
+				system_state.rc_channels = 0;
+				system_state.fmu_report_due = true;
+			}
+		}
+	}
+}
+
+static void
+mixer_update(int mixer, uint16_t *inputs, int input_count)
+{
+	/* simple passthrough for now */
+	if (mixer < input_count) {
+		mixers[mixer].current_value = inputs[mixer];
+	} else {
+		mixers[mixer].current_value = 0;
+	}
+}
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
new file mode 100644
index 000000000..92558a61d
--- /dev/null
+++ b/apps/px4io/protocol.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file PX4FMU <-> PX4IO messaging protocol.
+ *
+ * This initial version of the protocol is very simple; each side transmits a
+ * complete update with each frame.  This avoids the sending of many small
+ * messages and the corresponding complexity involved.
+ */
+
+/*
+ * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL
+ * TREES ARE MERGED.
+ */
+
+#define PX4IO_OUTPUT_CHANNELS	8
+#define PX4IO_INPUT_CHANNELS	12
+#define PX4IO_RELAY_CHANNELS	2
+
+/* command from FMU to IO */
+struct px4io_command {
+	uint16_t	f2i_magic;
+#define F2I_MAGIC	0x636d
+
+	uint16_t	servo_command[PX4IO_OUTPUT_CHANNELS];
+	bool		relay_state[PX4IO_RELAY_CHANNELS];
+	bool		arm_ok;
+} __attribute__((packed));
+
+/* report from IO to FMU */
+struct px4io_report {
+	uint16_t	i2f_magic;
+#define I2F_MAGIC		0x7570
+
+	uint16_t	rc_channel[PX4IO_INPUT_CHANNELS];
+	bool		armed;
+	uint8_t		channel_count;
+} __attribute__((packed));
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
new file mode 100644
index 000000000..44b62f3f9
--- /dev/null
+++ b/apps/px4io/px4io.c
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Top-level logic for the PX4IO module.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <debug.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include <nuttx/clock.h>
+
+#include <arch/board/up_boardinitialize.h>
+#include <arch/board/drv_gpio.h>
+#include <arch/board/drv_ppm_input.h>
+#include <arch/board/up_hrt.h>
+
+#include "px4io.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int user_start(int argc, char *argv[]);
+
+/****************************************************************************
+ * user_start
+ ****************************************************************************/
+
+struct sys_state_s 	system_state;
+int			gpio_fd;
+
+static const char cursor[] = {'|', '/', '-', '\\'};
+
+static const char *rc_input_mq_name = "rc_input";
+
+static struct hrt_call timer_tick_call;
+volatile int timers[TIMER_NUM_TIMERS];
+static void timer_tick(void *arg);
+
+int user_start(int argc, char *argv[])
+{
+	int     cycle = 0;
+	bool	heartbeat = false;
+	bool	failsafe = false;
+
+	/* Do board init */
+	(void)up_boardinitialize();
+
+	/* print some startup info */
+	lib_lowprintf("\nPX4IO: starting\n");
+	struct mallinfo minfo = mallinfo();
+	lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
+
+	/* start the software timer service */
+	hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL);
+
+	/* Open the GPIO driver so we can do LEDs and the like. */
+	gpio_fd = open("/dev/gpio", 0);
+	ASSERTCODE((gpio_fd >= 0), A_GPIO_OPEN_FAIL);
+
+	/* default all the LEDs to off while we start */
+	LED_AMBER(heartbeat);
+	LED_BLUE(failsafe);
+	LED_SAFETY(false);
+
+	/* turn on servo power */
+	POWER_SERVO(true);
+
+	/* configure the PPM input driver */
+	ppm_input_init(rc_input_mq_name);
+
+	/* start the mixer */
+	mixer_init(rc_input_mq_name);
+
+	/* start the safety switch handler */
+	safety_init();
+
+	/* init the FMU link */
+	comms_init();
+
+	/* set up some timers for the main loop */
+	timers[TIMER_BLINK_AMBER] = 250;	/* heartbeat blink @ 2Hz */
+	timers[TIMER_STATUS_PRINT] = 1000;	/* print status message @ 1Hz */
+
+	/*
+	 * Main loop servicing communication with FMU
+	 */
+	while(true) {
+
+		/* check for communication from FMU, send updates */
+		comms_check();
+
+		/* blink the heartbeat LED */
+		if (timers[TIMER_BLINK_AMBER] == 0) {
+			timers[TIMER_BLINK_AMBER] = 250;
+			LED_AMBER((heartbeat = !heartbeat));
+		}
+
+		/* blink the failsafe LED if we don't have FMU input */
+		if (!system_state.mixer_use_fmu) {
+			if (timers[TIMER_BLINK_BLUE] == 0) {
+				timers[TIMER_BLINK_BLUE] = 125;
+				LED_BLUE((failsafe = !failsafe));
+			}
+		} else {
+			LED_BLUE((failsafe = false));
+		}
+		
+		/* print some simple status */
+		if (timers[TIMER_STATUS_PRINT] == 0) {
+			timers[TIMER_STATUS_PRINT] = 1000;
+			lib_lowprintf("%c %s | %s | %s | C=%d    \r",
+				cursor[cycle++ & 3],
+				(system_state.armed         ? "ARMED"  : "SAFE"),
+				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
+				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
+				system_state.rc_channels
+			);
+		}
+
+	}
+
+	/* Should never reach here */
+	return ERROR;
+}
+
+static void
+timer_tick(void *arg)
+{
+	for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++)
+		if (timers[i] > 0)
+			timers[i]--;
+}
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
new file mode 100644
index 000000000..d90f7e36b
--- /dev/null
+++ b/apps/px4io/px4io.h
@@ -0,0 +1,142 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file General defines and structures for the PX4IO module firmware.
+  */
+
+#include <arch/board/drv_gpio.h>
+#include "protocol.h"
+
+/*
+ * Constants and limits.
+ */
+#define MAX_CONTROL_CHANNELS	12
+#define IO_SERVO_COUNT		8
+
+/*
+ * System state structure.
+ */
+struct sys_state_s 
+{
+
+	bool		armed;		/* actually armed */
+	bool		arm_ok;		/* FMU says OK to arm */
+
+	/*
+	 * Data from the remote control input(s)
+	 */
+	int		rc_channels;
+	uint16_t	rc_channel_data[PX4IO_INPUT_CHANNELS];
+
+	/*
+	 * Control signals from FMU.
+	 */
+	uint16_t	fmu_channel_data[PX4IO_OUTPUT_CHANNELS];
+
+	/*
+	 * Relay controls
+	 */
+	bool		relays[PX4IO_RELAY_CHANNELS];
+
+	/*
+	 * If true, we are using the FMU controls.
+	 */
+	bool		mixer_use_fmu;
+
+	/*
+	 * If true, state that should be reported to FMU has been updated.
+	 */
+	bool		fmu_report_due;
+
+	/*
+	 * If true, new control data from the FMU has been received.
+	 */
+	bool		fmu_data_received;
+};
+
+extern struct sys_state_s system_state;
+
+/*
+ * Software countdown timers.
+ *
+ * Each timer counts down to zero at one tick per ms.
+ */
+#define TIMER_BLINK_AMBER	0
+#define TIMER_BLINK_BLUE	1
+#define TIMER_STATUS_PRINT	2
+#define TIMER_SANITY		7
+#define TIMER_NUM_TIMERS	8
+extern volatile int	timers[TIMER_NUM_TIMERS];
+
+/*
+ * GPIO handling.
+ */
+extern int gpio_fd;
+
+#define POWER_SERVO(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_POWER), (_s))
+#define POWER_ACC1(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC1), (_s))
+#define POWER_ACC2(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC2), (_s))
+#define POWER_RELAY1(_s)	ioctl(gpio_fd, GPIO_SET(GPIO_RELAY1, (_s))
+#define POWER_RELAY2(_s)	ioctl(gpio_fd, GPIO_SET(GPIO_RELAY2, (_s))
+
+#define LED_AMBER(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_LED_AMBER), !(_s))
+#define LED_BLUE(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_LED_BLUE), !(_s))
+#define LED_SAFETY(_s)		ioctl(gpio_fd, GPIO_SET(GPIO_LED_SAFETY), !(_s))
+
+#define OVERCURRENT_ACC		ioctl(gpio_fd, GPIO_GET(GPIO_ACC_OVERCURRENT), 0)
+#define OVERCURRENT_SERVO	ioctl(gpio_fd, GPIO_GET(GPIO_SERVO_OVERCURRENT), 0)
+#define BUTTON_SAFETY		ioctl(gpio_fd, GPIO_GET(GPIO_SAFETY_BUTTON), 0)
+
+/*
+ * Mixer
+ */
+extern int	mixer_init(const char *mq_name);
+
+/*
+ * Safety switch/LED.
+ */
+extern void	safety_init(void);
+
+/*
+ * FMU communications
+ */
+extern void	comms_init(void);
+extern void	comms_check(void);
+
+/*
+ * Assertion codes
+ */
+#define A_GPIO_OPEN_FAIL		100
+#define A_SERVO_OPEN_FAIL		101
+#define A_INPUTQ_OPEN_FAIL		102
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
new file mode 100644
index 000000000..895c33806
--- /dev/null
+++ b/apps/px4io/safety.c
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+ /**
+  * @file Safety button logic.
+  */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <debug.h>
+#include <stdlib.h>
+#include <errno.h>
+
+#include <nuttx/clock.h>
+
+#include <arch/board/up_boardinitialize.h>
+#include <arch/board/drv_gpio.h>
+#include <arch/board/drv_ppm_input.h>
+#include <arch/board/up_hrt.h>
+
+#include "px4io.h"
+
+static struct hrt_call arming_call;
+
+/*
+ * Count the number of times in a row that we see the arming button
+ * held down.
+ */
+static unsigned arm_counter;
+#define ARM_COUNTER_THRESHOLD	10
+
+static bool safety_led_state;
+
+static void safety_check_button(void *arg);
+
+void
+safety_init(void)
+{
+	/* arrange for the button handler to be called at 10Hz */
+	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+}
+
+static void
+safety_check_button(void *arg)
+{
+	/* 
+	 * Debounce the safety button, change state if it has been held for long enough.
+	 *
+	 * Ignore the button if FMU has not said it's OK to arm yet.
+	 */
+	if (BUTTON_SAFETY && system_state.arm_ok) {
+		if (arm_counter < ARM_COUNTER_THRESHOLD) {
+			arm_counter++;
+		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
+			/* change our armed state and notify the FMU */
+			system_state.armed = !system_state.armed;
+			arm_counter++;
+			system_state.fmu_report_due = true;
+		}
+	} else {
+		arm_counter = 0;
+	}
+
+	/* when armed, toggle the LED; when safe, leave it on */
+	if (system_state.armed) {
+		safety_led_state = !safety_led_state;
+	} else {
+		safety_led_state = true;
+	}
+	LED_SAFETY(safety_led_state);
+}