Merge branch 'master' into navigator_rewrite

Conflicts: src/drivers/gps/gps.cpp src/drivers/gps/mtk.cpp src/modules/commander/commander.cpp src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp src/modules/navigator/mission.cpp src/modules/navigator/mission.h src/modules/navigator/navigator_main.cpp src/modules/navigator/navigator_state.h src/modules/position_estimator_inav/position_estimator_inav_main.c
2026-10-16 20:23:33 +08:00 · 2014-05-26 20:19:11 +02:00
parent 68352cb923 36495cdb62
commit 063caba36b
173 changed files with 5312 additions and 1890 deletions
@@ -119,7 +119,7 @@ int ardrone_interface_main(int argc, char *argv[])
 		ardrone_interface_task = task_spawn_cmd("ardrone_interface",
 						    SCHED_DEFAULT,
 						    SCHED_PRIORITY_MAX - 15,
-						    2048,
+						    1100,
 						    ardrone_interface_thread_main,
 						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -38,3 +38,4 @@
 MODULE_COMMAND	 = ardrone_interface
 SRCS		 = ardrone_interface.c \
 			   ardrone_motor_control.c
+MODULE_STACKSIZE = 1200
@@ -194,6 +194,26 @@ private:

 	bool systemstate_run;

+	int vehicle_status_sub_fd;
+	int vehicle_control_mode_sub_fd;
+	int vehicle_gps_position_sub_fd;
+	int actuator_armed_sub_fd;
+	int safety_sub_fd;
+
+	int num_of_cells;
+	int detected_cells_runcount;
+
+	int t_led_color[8];
+	int t_led_blink;
+	int led_thread_runcount;
+	int led_interval;
+
+	bool topic_initialized;
+	bool detected_cells_blinked;
+	bool led_thread_ready;
+
+	int num_of_used_sats;
+
 	void 			setLEDColor(int ledcolor);
 	static void		led_trampoline(void *arg);
 	void			led();
@@ -265,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) :
 	led_color_7(LED_OFF),
 	led_color_8(LED_OFF),
 	led_blink(LED_NOBLINK),
-	systemstate_run(false)
+	systemstate_run(false),
+	vehicle_status_sub_fd(-1),
+	vehicle_control_mode_sub_fd(-1),
+	vehicle_gps_position_sub_fd(-1),
+	actuator_armed_sub_fd(-1),
+	safety_sub_fd(-1),
+	num_of_cells(0),
+	detected_cells_runcount(0),
+	t_led_color({0}),
+	t_led_blink(0),
+	led_thread_runcount(0),
+	led_interval(1000),
+	topic_initialized(false),
+	detected_cells_blinked(false),
+	led_thread_ready(true),
+	num_of_used_sats(0)
 {
 	memset(&_work, 0, sizeof(_work));
 }
@@ -382,31 +417,6 @@ void
 BlinkM::led()
 {

-	static int vehicle_status_sub_fd;
-	static int vehicle_control_mode_sub_fd;
-	static int vehicle_gps_position_sub_fd;
-	static int actuator_armed_sub_fd;
-	static int safety_sub_fd;
-
-	static int num_of_cells = 0;
-	static int detected_cells_runcount = 0;
-
-	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
-	static int t_led_blink = 0;
-	static int led_thread_runcount=0;
-	static int led_interval = 1000;
-
-	static int no_data_vehicle_status = 0;
-	static int no_data_vehicle_control_mode = 0;
-	static int no_data_actuator_armed = 0;
-	static int no_data_vehicle_gps_position = 0;
-
-	static bool topic_initialized = false;
-	static bool detected_cells_blinked = false;
-	static bool led_thread_ready = true;
-
-	int num_of_used_sats = 0;
-
 	if(!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 250);
@@ -494,6 +504,11 @@ BlinkM::led()

 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);

+		int no_data_vehicle_status = 0;
+		int no_data_vehicle_control_mode = 0;
+		int no_data_actuator_armed = 0;
+		int no_data_vehicle_gps_position = 0;
+
 		if (new_data_vehicle_status) {
 			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
 			no_data_vehicle_status = 0;
@@ -638,11 +653,11 @@ BlinkM::led()

 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
+							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   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 aerocore_init.c
+ *
+ * AeroCore-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi3;
+static struct spi_dev_s *spi4;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* J1 breakout */
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure Sensors on SPI bus #3 */
+	spi3 = up_spiinitialize(3);
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: 1MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi3, 10000000);
+	SPI_SETBITS(spi3, 8);
+	SPI_SETMODE(spi3, SPIDEV_MODE3);
+	SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+	message("[boot] Initialized SPI port 3 (SENSORS)\n");
+
+	/* Configure FRAM on SPI bus #4 */
+	spi4 = up_spiinitialize(4);
+	if (!spi4) {
+		message("[boot] FAILED to initialize SPI port 4\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: ~10MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
+	SPI_SETBITS(spi4, 8);
+	SPI_SETMODE(spi4, SPIDEV_MODE0);
+	SPI_SELECT(spi4, SPIDEV_FLASH, false);
+	message("[boot] Initialized SPI port 4 (FRAM)\n");
+
+	return OK;
+}
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 aerocore_led.c
+ *
+ * AeroCore LED backend.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+__EXPORT void led_init()
+{
+	stm32_configgpio(GPIO_LED0);
+	stm32_configgpio(GPIO_LED1);
+}
+
+__EXPORT void led_on(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, true);
+		break;
+
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, true);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
+
+__EXPORT void led_off(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, false);
+		break;
+
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, false);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	switch (led) {
+	case 0:
+		if (stm32_gpioread(GPIO_LED0))
+			stm32_gpiowrite(GPIO_LED0, false);
+		else
+			stm32_gpiowrite(GPIO_LED0, true);
+		break;
+
+	case 1:
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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 aerocore_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1500,
+	}
+};
@@ -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 aerocore_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI1_NSS);
+	stm32_gpiowrite(GPIO_SPI1_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI2_NSS);
+	stm32_gpiowrite(GPIO_SPI2_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+#endif
+
+#ifdef CONFIG_STM32_SPI4
+	stm32_configgpio(GPIO_SPI4_NSS);
+	stm32_gpiowrite(GPIO_SPI4_NSS, 1);
+#endif
+}
+
+#ifdef CONFIG_STM32_SPI1
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+
+#ifdef CONFIG_STM32_SPI4
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 board_config.h
+ *
+ * AeroCore internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+/* LEDs */
+#define GPIO_LED0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_LED1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10)
+
+/* Gyro */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0)
+#define SENSOR_BOARD_ROTATION_DEFAULT	3 /* SENSOR_BOARD_ROTATION_270_DEG */
+
+/* Accel & Mag */
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2)
+
+/* GPS */
+#define GPIO_GPS_NRESET		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPS_TIMEPULSE	(GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
+#define GPS_DEFAULT_UART_PORT	"/dev/ttyS0"
+
+/* SPI3--Sensors */
+#define PX4_SPI_BUS_SENSORS	3
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+
+/* Nominal chip selects for devices on SPI bus #3 */
+#define PX4_SPIDEV_ACCEL_MAG	0
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_BARO		2
+
+/* User GPIOs broken out on J11 */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+
+/* PWM
+ *
+ * Eight PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PA8  : TIM1_CH1
+ * CH2 : PA9  : TIM1_CH2
+ * CH3 : PA10 : TIM1_CH3
+ * CH4 : PA11 : TIM1_CH4
+ * CH5 : PC6  : TIM3_CH1
+ * CH6 : PC7  : TIM3_CH2
+ * CH7 : PC8  : TIM3_CH3
+ * CH8 : PC9  : TIM3_CH4
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_1
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_1
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_1
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_1
+#define GPIO_TIM3_CH1OUT	GPIO_TIM3_CH1OUT_3
+#define GPIO_TIM3_CH2OUT	GPIO_TIM3_CH2OUT_3
+#define GPIO_TIM3_CH3OUT	GPIO_TIM3_CH3OUT_2
+#define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_2
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer 8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/* Tone Alarm (no onboard speaker )*/
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
+
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
@@ -0,0 +1,8 @@
+#
+# Board-specific startup code for the AeroCore
+#
+
+SRCS		 = aerocore_init.c \
+		   aerocore_pwm_servo.c \
+		   aerocore_spi.c \
+		   aerocore_led.c
@@ -85,6 +85,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)

+#define PX4_SPI_BUS_SENSORS	1
+
 /*
 * Use these in place of the spi_dev_e enumeration to
 * select a specific SPI device on SPI1
@@ -107,6 +107,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)

+#define PX4_SPI_BUS_SENSORS	1
+
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO		1
 #define PX4_SPIDEV_ACCEL_MAG	2
@@ -94,6 +94,14 @@

 #endif

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+/*
+ * AeroCore GPIO numbers and configuration.
+ *
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /* no GPIO driver on the PX4IOv1 board */
 #endif
@@ -146,4 +154,4 @@

 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)

-#endif /* _DRV_GPIO_H */
+#endif /* _DRV_GPIO_H */
@@ -43,10 +43,14 @@
 #include <stdint.h>
 #include <sys/ioctl.h>

+#include "board_config.h"
+
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"

+#ifndef GPS_DEFAULT_UART_PORT
 #define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+#endif

 #define GPS_DEVICE_PATH	"/dev/gps"

@@ -154,8 +154,9 @@ ETSAirspeed::collect()
 		return ret;
 	}

-	uint16_t diff_pres_pa = val[1] << 8 | val[0];
-        if (diff_pres_pa == 0) {
+	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
+	uint16_t diff_pres_pa;
+        if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
 		// caller could end up using this value as part of an
@@ -165,10 +166,10 @@ ETSAirspeed::collect()
 		return -1;
        }

-	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
+	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 	} else {
-		diff_pres_pa -= _diff_pres_offset;
+		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
 	}

 	// Track maximum differential pressure measured (so we can work out top speed).
@@ -183,6 +184,7 @@ ETSAirspeed::collect()

 	// XXX we may want to smooth out the readings to remove noise.
 	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -53,6 +53,8 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>

+#include <drivers/drv_hrt.h>
+
 /* FrSky sensor hub data IDs */
 #define FRSKY_ID_GPS_ALT_BP     0x01
 #define FRSKY_ID_TEMP1          0x02
@@ -192,17 +194,12 @@ void frsky_send_frame1(int uart)
 }

 /**
- * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ * Formats the decimal latitude/longitude to the required degrees/minutes.
 */
 static float frsky_format_gps(float dec)
 {
-	float dms_deg = (int) dec;
-	float dec_deg = dec - dms_deg;
-	float dms_min = (int) (dec_deg * 60);
-	float dec_min = (dec_deg * 60) - dms_min;
-	float dms_sec = dec_min * 60;
-
-	return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+	float dm_deg = (int) dec;
+	return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
 }

 /**
@@ -230,9 +227,9 @@ void frsky_send_frame2(int uart)
 		struct tm *tm_gps = gmtime(&time_gps);

 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
-		lat    = frsky_format_gps(abs(global_pos.lat));
+		lat    = frsky_format_gps(fabsf(global_pos.lat));
 		lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
-		lon    = frsky_format_gps(abs(global_pos.lon));
+		lon    = frsky_format_gps(fabsf(global_pos.lon));
 		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
 		speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
 				* 25.0f / 46.0f;
@@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[])
 		frsky_task = task_spawn_cmd("frsky_telemetry",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
-					  2048,
+					  2000,
 					  frsky_telemetry_thread_main,
 					  (const char **)argv);

@@ -39,3 +39,5 @@ MODULE_COMMAND	= frsky_telemetry

 SRCS		= frsky_data.c \
              frsky_telemetry.c
+
+MODULE_STACKSIZE = 1200
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -56,6 +56,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/i2c.h>
+#include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
@@ -63,6 +64,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>

+#include <board_config.h>
+
 #include "ubx.h"
 #include "mtk.h"

@@ -76,12 +79,6 @@
 #endif
 static const int ERROR = -1;

-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-
-
 class GPS : public device::CDev
 {
 public:
@@ -209,7 +206,8 @@ GPS::init()
 		goto out;

 	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
+				SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);

 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
@@ -421,7 +419,14 @@ GPS::task_main()
 void
 GPS::cmd_reset()
 {
-	//XXX add reset?
+#ifdef GPIO_GPS_NRESET
+	warnx("Toggling GPS reset pin");
+	stm32_configgpio(GPIO_GPS_NRESET);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+	usleep(100);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+	warnx("Toggled GPS reset pin");
+#endif
 }

 void
@@ -443,10 +448,10 @@ GPS::print_info()
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");

 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
+				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", _report.eph, _report.epv);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
 	return _rate_vel;
 }

-float
+void
 GPS_Helper::reset_update_rates()
 {
 	_rate_count_vel = 0;
@@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
 	_interval_rate_start = hrt_absolute_time();
 }

-float
+void
 GPS_Helper::store_update_rates()
 {
 	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
@@ -46,13 +46,17 @@
 class GPS_Helper
 {
 public:
+
+	GPS_Helper() {};
+	virtual ~GPS_Helper() {};
+
 	virtual int			configure(unsigned &baud) = 0;
 	virtual int 			receive(unsigned timeout) = 0;
 	int 				set_baudrate(const int &fd, unsigned baud);
 	float				get_position_update_rate();
 	float				get_velocity_update_rate();
-	float				reset_update_rates();
-	float				store_update_rates();
+	void				reset_update_rates();
+	void				store_update_rates();

 protected:
 	uint8_t _rate_count_lat_lon;
@@ -41,3 +41,5 @@ SRCS		= gps.cpp \
 		  gps_helper.cpp \
 		  mtk.cpp \
 		  ubx.cpp
+
+MODULE_STACKSIZE = 1200
@@ -249,12 +249,18 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
+
+		// Indicate this data is not usable and bail out
+		_gps_position->eph_m = 1000.0f;
+		_gps_position->epv_m = 1000.0f;
+		_gps_position->fix_type = 0;
+		return;
 	}

 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
-	_gps_position->epv = 0.0; //unknown in mtk custom mode
+	_gps_position->eph = packet.hdop / 100.0f; // from cm to m
+	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
@@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
 	send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: RATE");
+		warnx("CFG FAIL: RATE");
 		return 1;
 	}

@@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
 	send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: NAV5");
+		warnx("CFG FAIL: NAV5");
 		return 1;
 	}

@@ -194,35 +194,42 @@ UBX::configure(unsigned &baudrate)
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH");
+		warnx("MSG CFG FAIL: NAV POSLLH");
 		return 1;
 	}

 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
+		warnx("MSG CFG FAIL: NAV TIMEUTC");
 		return 1;
 	}

 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL");
+		warnx("MSG CFG FAIL: NAV SOL");
 		return 1;
 	}

 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED");
+		warnx("MSG CFG FAIL: NAV VELNED");
 		return 1;
 	}

 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);

 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO");
+		warnx("MSG CFG FAIL: NAV SVINFO");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("MSG CFG FAIL: MON HW");
 		return 1;
 	}

@@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)

 		if (ret < 0) {
 			/* something went wrong when polling */
-			warnx("ubx: poll error");
+			warnx("poll error");
 			return -1;

 		} else if (ret == 0) {
@@ -310,7 +317,7 @@ UBX::receive(unsigned timeout)

 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
-			warnx("ubx: timeout - no useful messages");
+			warnx("timeout - no useful messages");
 			return -1;
 		}
 	}
@@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
 				return 1;	// message received successfully

 			} else {
-				warnx("ubx: checksum wrong");
+				warnx("checksum wrong");
 				decode_init();
 				return -1;
 			}
@@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
 			_rx_count++;

 		} else {
-			warnx("ubx: buffer full");
+			warnx("buffer full");
 			decode_init();
 			return -1;
 		}
@@ -566,6 +573,24 @@ UBX::handle_message()
 				break;
 			}

+		case UBX_CLASS_MON: {
+			switch (_message_id) {
+			case UBX_MESSAGE_MON_HW: {
+
+					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+
+					_gps_position->noise_per_ms = p->noisePerMS;
+					_gps_position->jamming_indicator = p->jamInd;
+
+					ret = 1;
+					break;
+				}
+
+			default:
+				break;
+			}
+		}
+
 		default:
 			break;
 		}
@@ -56,6 +56,7 @@
 //#define UBX_CLASS_RXM 0x02
 #define UBX_CLASS_ACK 0x05
 #define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_MON 0x0A

 /* MessageIDs (the ones that are used) */
 #define UBX_MESSAGE_NAV_POSLLH 0x02
@@ -72,6 +73,8 @@
 #define UBX_MESSAGE_CFG_RATE 0x08
 #define UBX_MESSAGE_CFG_NAV5 0x24

+#define UBX_MESSAGE_MON_HW	0x09
+
 #define UBX_CFG_PRT_LENGTH 20
 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
 #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
@@ -210,6 +213,27 @@ typedef struct {
 	uint8_t ck_b;
 } gps_bin_nav_velned_packet_t;

+struct gps_bin_mon_hw_packet {
+	uint32_t pinSel;
+	uint32_t pinBank;
+	uint32_t pinDir;
+	uint32_t pinVal;
+	uint16_t noisePerMS;
+	uint16_t agcCnt;
+	uint8_t aStatus;
+	uint8_t aPower;
+	uint8_t flags;
+	uint8_t __reserved1;
+	uint32_t usedMask;
+	uint8_t VP[25];
+	uint8_t jamInd;
+	uint16_t __reserved3;
+	uint32_t pinIrq;
+	uint32_t pulLH;
+	uint32_t pullL;
+};
+
+
 //typedef struct {
 //	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
 //	int16_t week; 					/**< Measurement GPS week number */
@@ -319,7 +343,7 @@ typedef enum {
 //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
 #pragma pack(pop)

-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer

 class UBX : public GPS_Helper
 {
@@ -383,7 +407,7 @@ private:
 	uint8_t			_message_class;
 	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_last;
+	hrt_abstime		_disable_cmd_last;
 };

 #endif /* UBX_H_ */
@@ -122,7 +122,7 @@ private:
 	actuator_controls_s _controls;

 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();

 	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
@@ -158,6 +158,7 @@ private:
 	int			_class_instance;

 	orb_advert_t		_mag_topic;
+	orb_advert_t		_subsystem_pub;

 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -324,7 +325,9 @@ HMC5883::HMC5883(int bus) :
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
+	_collect_phase(false),
 	_mag_topic(-1),
+	_subsystem_pub(-1),
 	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
@@ -1137,13 +1140,12 @@ int HMC5883::check_calibration()
 			true,
 			_calibrated,
 			SUBSYSTEM_TYPE_MAG};
-		static orb_advert_t pub = -1;

 		if (!(_pub_blocked)) {
-			if (pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), pub, &info);
+			if (_subsystem_pub > 0) {
+				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
 			} else {
-				pub = orb_advertise(ORB_ID(subsystem_info), &info);
+				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
 			}
 		}
 	}
@@ -51,6 +51,8 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>

+#include <drivers/drv_hrt.h>
+
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5

@@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
 static int _esc_sub = -1;

 static orb_advert_t _esc_pub;
-struct esc_status_s _esc;

 static bool _home_position_set = false;
 static double _home_lat = 0.0d;
@@ -82,8 +83,6 @@ init_sub_messages(void)
 void 
 init_pub_messages(void)
 {
-	memset(&_esc, 0, sizeof(_esc));
-	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
 }

 void
@@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
 	size_t size = sizeof(msg);
 	memset(&msg, 0, size);
 	memcpy(&msg, buffer, size);
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+
+	// Publish it.
+	esc.timestamp = hrt_absolute_time();
+	esc.esc_count = 1;
+	esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
+
+	esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
+	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	esc.esc[0].esc_temperature = msg.temperature1 - 20; 
+	esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
+	esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));

 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
-		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+		orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
 	} else {
-		_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
 	}
-
-	// Publish it.
-	_esc.esc_count = 1;
-	_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
-
-	_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
-	_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
-	_esc.esc[0].esc_temperature = msg.temperature1 - 20; 
-	_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
-	_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 }

 void 
@@ -34,6 +34,9 @@
 /**
 * @file l3gd20.cpp
 * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ *
+ * Note: With the exception of the self-test feature, the ST L3G4200D is
+ *       also supported by this driver.
 */

 #include <nuttx/config.h>
@@ -89,9 +92,11 @@ static const int ERROR = -1;
 #define ADDR_WHO_AM_I			0x0F
 #define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
+#define WHO_I_AM_L3G4200D		0xD3	/* for L3G4200D */

 #define ADDR_CTRL_REG1			0x20
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
+
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
@@ -166,9 +171,14 @@ static const int ERROR = -1;
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)

 #define L3GD20_DEFAULT_RATE			760
+#define L3G4200D_DEFAULT_RATE			800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30

+#ifndef SENSOR_BOARD_ROTATION_DEFAULT
+#define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
+#endif
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }

 class L3GD20 : public device::SPI
@@ -216,6 +226,9 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;

+	/* true if an L3G4200D is detected */
+	bool	_is_l3g4200d;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_topic(-1),
 	_class_instance(-1),
 	_current_rate(0),
-	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
 	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_is_l3g4200d(false)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -413,14 +427,7 @@ L3GD20::probe()
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {

-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#else
-			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
-		#endif
-
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
 	}

@@ -430,6 +437,13 @@ L3GD20::probe()
 		success = true;
 	}

+	/* Detect the L3G4200D used on AeroCore */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+		_is_l3g4200d = true;
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
+		success = true;
+	}
+
 	if (success)
 		return OK;

@@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
+				if (_is_l3g4200d) {
+					return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
+				}
 				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);

 				/* adjust to a legal polling interval in Hz */
@@ -683,23 +700,26 @@ L3GD20::set_samplerate(unsigned frequency)
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;

 	if (frequency == 0)
-		frequency = 760;
+		frequency = _is_l3g4200d ? 800 : 760;

-	/* use limits good for H or non-H models */
+	/*
+	 * Use limits good for H or non-H models. Rates are slightly different
+	 * for L3G4200D part but register settings are the same.
+	 */
 	if (frequency <= 100) {
-		_current_rate = 95;
+		_current_rate = _is_l3g4200d ? 100 : 95;
 		bits |= RATE_95HZ_LP_25HZ;

 	} else if (frequency <= 200) {
-		_current_rate = 190;
+		_current_rate = _is_l3g4200d ? 200 : 190;
 		bits |= RATE_190HZ_LP_50HZ;

 	} else if (frequency <= 400) {
-		_current_rate = 380;
+		_current_rate = _is_l3g4200d ? 400 : 380;
 		bits |= RATE_380HZ_LP_50HZ;

 	} else if (frequency <= 800) {
-		_current_rate = 760;
+		_current_rate = _is_l3g4200d ? 800 : 760;
 		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
@@ -772,7 +792,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);

-	set_samplerate(0); // 760Hz
+	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);

@@ -971,7 +991,7 @@ start()
 		errx(0, "already started");

 	/* create the driver */
-	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);

 	if (g_dev == nullptr)
 		goto fail;
@@ -1793,7 +1793,7 @@ start()
 		errx(0, "already started");

 	/* create the driver */
-	g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+	g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);

 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
@@ -92,8 +92,20 @@
 #define MOTOR_SPINUP_COUNTER			30
 #define ESC_UORB_PUBLISH_DELAY		500000

-
-
+struct MotorData_t {
+	unsigned int Version;                        // the version of the BL (0 = old)
+	unsigned int SetPoint;                       // written by attitude controller
+	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	float SetPoint_PX4; 			     // Values from PX4
+	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+	unsigned int ReadMode;                       // select data to read
+	unsigned short RawPwmValue;							// length of PWM pulse
+	// the following bytes must be exactly in that order!
+	unsigned int Current;                        // in 0.1 A steps, read back from BL
+	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int RoundCount;
+};

 class MK : public device::I2C
 {
@@ -154,8 +166,10 @@ private:

 	actuator_controls_s _controls;

+	MotorData_t Motor[MAX_MOTORS];
+
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();

 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
@@ -195,24 +209,6 @@ const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};

 int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};

-struct MotorData_t {
-	unsigned int Version;                        // the version of the BL (0 = old)
-	unsigned int SetPoint;                       // written by attitude controller
-	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
-	float SetPoint_PX4; 			     // Values from PX4
-	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
-	unsigned int ReadMode;                       // select data to read
-	unsigned short RawPwmValue;							// length of PWM pulse
-	// the following bytes must be exactly in that order!
-	unsigned int Current;                        // in 0.1 A steps, read back from BL
-	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
-	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
-	unsigned int RoundCount;
-};
-
-MotorData_t Motor[MAX_MOTORS];
-
-
 namespace
 {

@@ -1380,7 +1380,6 @@ MPU6000_gyro::init()

 	_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);

-out:
 	return ret;
 }

@@ -753,8 +753,8 @@ MS5611::print_info()
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);
-	printf("P:              %.3f\n", _P);
-	printf("T:              %.3f\n", _T);
+	printf("P:              %.3f\n", (double)_P);
+	printf("T:              %.3f\n", (double)_T);
 	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));

 	printf("factory_setup             %u\n", _prom.factory_setup);
@@ -117,7 +117,7 @@ private:
 device::Device *
 MS5611_spi_interface(ms5611::prom_u &prom_buf)
 {
-	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
 }

 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
@@ -92,6 +92,7 @@ public:
 		MODE_2PWM,
 		MODE_4PWM,
 		MODE_6PWM,
+		MODE_8PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -113,6 +114,9 @@ private:
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 	static const unsigned _max_actuators = 6;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	static const unsigned _max_actuators = 8;
+#endif

 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -149,7 +153,7 @@ private:
 	unsigned	_num_disarmed_set;

 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();

 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
@@ -203,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
 	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	/* AeroCore breaks out User GPIOs on J11 */
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+	{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0},
+	{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0},
+	{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0},
+	{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0},
+	{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0},
+	{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0},
+#endif
 };

 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -382,6 +400,20 @@ PX4FMU::set_mode(Mode mode)

 		break;

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
+		debug("MODE_8PWM");
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xff);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+		break;
+#endif
+
 	case MODE_NONE:
 		debug("MODE_NONE");

@@ -559,13 +591,12 @@ PX4FMU::task_main()
 		}

 		/* sleep waiting for data, stopping to check for PPM
-		 * input at 100Hz */
+		 * input at 50Hz */
 		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);

 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;

 		} else if (ret == 0) {
@@ -603,6 +634,9 @@ PX4FMU::task_main()
 					num_outputs = 6;
 					break;

+				case MODE_8PWM:
+					num_outputs = 8;
+					break;
 				default:
 					num_outputs = 0;
 					break;
@@ -614,11 +648,9 @@ PX4FMU::task_main()

 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i >= outputs.noutputs ||
-						!isfinite(outputs.output[i]) ||
-						outputs.output[i] < -1.0f ||
-						outputs.output[i] > 1.0f) {
+					/* last resort: catch NaN and INF */
+					if ((i >= outputs.noutputs) ||
+						!isfinite(outputs.output[i])) {
 						/*
 						 * Value is NaN, INF or out of band - set to the minimum value.
 						 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -630,6 +662,7 @@ PX4FMU::task_main()

 				uint16_t pwm_limited[num_outputs];

+				/* the PWM limit call takes care of out of band errors and constrains */
 				pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);

 				/* output to the servos */
@@ -758,6 +791,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	case MODE_2PWM:
 	case MODE_4PWM:
 	case MODE_6PWM:
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM:
+#endif
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;

@@ -987,6 +1023,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_SET(7):
+	case PWM_SERVO_SET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
 		if (_mode < MODE_6PWM) {
@@ -1014,6 +1059,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)

 		break;

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET(7):
+	case PWM_SERVO_GET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_GET(5):
 	case PWM_SERVO_GET(4):
 		if (_mode < MODE_6PWM) {
@@ -1041,12 +1095,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(3):
 	case PWM_SERVO_GET_RATEGROUP(4):
 	case PWM_SERVO_GET_RATEGROUP(5):
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET_RATEGROUP(6):
+	case PWM_SERVO_GET_RATEGROUP(7):
+#endif
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;

 	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		case MODE_8PWM:
+			*(unsigned *)arg = 8;
+			break;
+#endif
+
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
@@ -1092,6 +1156,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			set_mode(MODE_6PWM);
 			break;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		case 8:
+			set_mode(MODE_8PWM);
+			break;
+#endif

 		default:
 			ret = -EINVAL;
@@ -1182,10 +1251,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	unsigned count = len / 2;
 	uint16_t values[6];

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	if (count > 8) {
+		// we have at most 8 outputs
+		count = 8;
+	}
+#else
 	if (count > 6) {
 		// we have at most 6 outputs
 		count = 6;
 	}
+#endif

 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
@@ -1459,6 +1535,9 @@ fmu_new_mode(PortMode new_mode)
 #endif
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 		servo_mode = PX4FMU::MODE_6PWM;
+#endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		servo_mode = PX4FMU::MODE_8PWM;
 #endif
 		break;

@@ -1777,7 +1856,7 @@ fmu_main(int argc, char *argv[])
 	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
 	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
 #endif
 	exit(1);
@@ -4,3 +4,5 @@

 MODULE_COMMAND	 = fmu
 SRCS		 = fmu.cpp
+
+MODULE_STACKSIZE = 1200
@@ -44,3 +44,5 @@ SRCS			= px4io.cpp \

 # XXX prune to just get UART registers
 INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MODULE_STACKSIZE = 1200
@@ -529,6 +529,11 @@ PX4IO::~PX4IO()
 	if (_interface != nullptr)
 		delete _interface;

+	/* deallocate perfs */
+	perf_free(_perf_update);
+	perf_free(_perf_write);
+	perf_free(_perf_chan_count);
+
 	g_dev = nullptr;
 }

@@ -576,8 +581,10 @@ PX4IO::init()
 	ASSERT(_task == -1);

 	sys_restart_param = param_find("SYS_RESTART_TYPE");
-	/* Indicate restart type is unknown */
-	param_set(sys_restart_param, &sys_restart_val);
+	if (sys_restart_param != PARAM_INVALID) {
+		/* Indicate restart type is unknown */
+		param_set(sys_restart_param, &sys_restart_val);
+	}

 	/* do regular cdev init */
 	ret = CDev::init();
@@ -789,7 +796,12 @@ PX4IO::init()
 	}

 	/* start the IO interface task */
-	_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("px4io",
+					SCHED_DEFAULT,
+					SCHED_PRIORITY_ACTUATOR_OUTPUTS,
+					2000,
+					(main_t)&PX4IO::task_main_trampoline,
+					nullptr);

 	if (_task < 0) {
 		debug("task start failed: %d", errno);
@@ -984,13 +996,17 @@ PX4IO::task_main()
 				int32_t failsafe_param_val;
 				param_t failsafe_param = param_find("RC_FAILS_THR");

-				if (failsafe_param > 0) {
+				if (failsafe_param != PARAM_INVALID) {

 					param_get(failsafe_param, &failsafe_param_val);
-					uint16_t failsafe_thr = failsafe_param_val;
-					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
-					if (pret != OK) {
-						log("failsafe upload failed");
+
+					if (failsafe_param_val > 0) {
+
+						uint16_t failsafe_thr = failsafe_param_val;
+						pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+						if (pret != OK) {
+							log("failsafe upload failed, FS: %d us", (int)failsafe_thr);
+						}
 					}
 				}

@@ -1448,7 +1464,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;

-	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
 	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];

 	/*
@@ -1456,8 +1472,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 *
 	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	input_rc.timestamp_publication = hrt_absolute_time();
-
 	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);

 	if (ret != OK)
@@ -1469,23 +1483,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[PX4IO_P_RAW_RC_COUNT];

-	if (channel_count != _rc_chan_count)
+	/* limit the channel count */
+	if (channel_count > RC_INPUT_MAX_CHANNELS) {
+		channel_count = RC_INPUT_MAX_CHANNELS;
+	}
+
+	/* count channel count changes to identify signal integrity issues */
+	if (channel_count != _rc_chan_count) {
 		perf_count(_perf_chan_count);
+	}

 	_rc_chan_count = channel_count;

+	input_rc.timestamp_publication = hrt_absolute_time();
+
 	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
 	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
 	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
 	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
 	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+	input_rc.channel_count = channel_count;

 	/* rc_lost has to be set before the call to this function */
-	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
 		_rc_last_valid = input_rc.timestamp_publication;
+	}

 	input_rc.timestamp_last_signal = _rc_last_valid;

+	/* FIELDS NOT SET HERE */
+	/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);

@@ -1493,8 +1522,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 			return ret;
 	}

-	input_rc.channel_count = channel_count;
-	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+	/* last thing set are the actual channel values as 16 bit values */
+	for (unsigned i = 0; i < channel_count; i++) {
+		input_rc.values[i] = regs[prolog + i];
+	}

 	return ret;
 }
@@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
 		if (_rx_dma_status == _dma_status_waiting) {

 			/* verify that the received packet is complete */
-			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
+			int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);

@@ -254,9 +254,6 @@ SF0X::~SF0X()
 int
 SF0X::init()
 {
-	int ret = ERROR;
-	unsigned i = 0;
-
 	/* do regular cdev init */
 	if (CDev::init() != OK) {
 		goto out;
@@ -594,7 +591,7 @@ SF0X::collect()
 		valid = false;

 		/* wipe out partially read content from last cycle(s), check for dot */
-		for (int i = 0; i < (lend - 2); i++) {
+		for (unsigned i = 0; i < (lend - 2); i++) {
 			if (_linebuf[i] == '\n') {
 				char buf[sizeof(_linebuf)];
 				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
@@ -795,7 +792,7 @@ const int ERROR = -1;

 SF0X	*g_dev;

-void	start();
+void	start(const char *port);
 void	stop();
 void	test();
 void	reset();
@@ -145,7 +145,7 @@ private:

 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
 	_channel_count(0),
 	_samples(nullptr),
 	_to_system_power(0)
@@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
 		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
 			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
 #endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif

 		if (g_adc == nullptr)
 			errx(1, "couldn't allocate the ADC driver");
@@ -94,7 +94,7 @@
 #elif HRT_TIMER == 3
 # define HRT_TIMER_BASE		STM32_TIM3_BASE
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
-# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
 # define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
 # if CONFIG_STM32_TIM3
@@ -141,7 +141,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
 # if CONFIG_STM32_TIM10
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
 # endif
@@ -150,7 +150,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
 # if CONFIG_STM32_TIM11
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
 # endif
@@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0;
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;

+#define PPM_DEBUG 0
+
+#if PPM_DEBUG
 /* PPM edge history */
 __EXPORT uint16_t ppm_edge_history[32];
 unsigned ppm_edge_next;
@@ -361,6 +364,7 @@ unsigned ppm_edge_next;
 /* PPM pulse history */
 __EXPORT uint16_t ppm_pulse_history[32];
 unsigned ppm_pulse_next;
+#endif

 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];

@@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status)
 	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;

+#if PPM_DEBUG
 	ppm_edge_history[ppm_edge_next++] = width;

 	if (ppm_edge_next >= 32)
 		ppm_edge_next = 0;
+#endif

 	/*
 	 * if this looks like a start pulse, then push the last set of values
@@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status)
 		interval = count - ppm.last_mark;
 		ppm.last_mark = count;

+#if PPM_DEBUG
 		ppm_pulse_history[ppm_pulse_next++] = interval;

 		if (ppm_pulse_next >= 32)
 			ppm_pulse_next = 0;
+#endif

 		/* if the mark-mark timing is out of bounds, abandon the frame */
 		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))