Checkpoint - moving things out of the NuttX configs/*/src directories

apps/ardrone_interface/ardrone_interface.c

@@ -51,7 +51,7 @@
 #include <time.h>
 #include <systemlib/err.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>

apps/ardrone_interface/ardrone_motor_control.c

@@ -42,7 +42,7 @@
 #include <fcntl.h>
 #include <unistd.h>
 #include <drivers/drv_gpio.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <systemlib/err.h>

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -59,7 +59,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/parameter_update.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
 

apps/commander/commander.c

@@ -62,8 +62,8 @@
 #include <v1.0/common/mavlink.h>
 #include <string.h>
 #include <arch/board/drv_led.h>
-#include <arch/board/up_hrt.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include "state_machine_helper.h"
 #include "systemlib/systemlib.h"

apps/commander/state_machine_helper.c

@@ -45,7 +45,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/systemlib.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"

apps/drivers/bma180/bma180.cpp

@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>

apps/drivers/boards/px4fmu/Makefile

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Board-specific startup code for the PX4FMU
+#
+
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk

apps/drivers/boards/px4fmu/drv_hrt.c

@@ -0,0 +1,845 @@
+/****************************************************************************
+ *
+ *   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 drv_hrt.c
+ *
+ * High-resolution timer callouts and timekeeping.
+ *
+ * This can use any general or advanced STM32 timer.
+ *
+ * Note that really, this could use systick too, but that's
+ * monopolised by NuttX and stealing it would just be awkward.
+ *
+ * We don't use the NuttX STM32 driver per se; rather, we 
+ * claim the timer and then drive it directly.
+ */
+
+#include <nuttx/config.h>
+#include <nuttx/arch.h>
+#include <nuttx/irq.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <assert.h>
+#include <debug.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <string.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "chip.h"
+#include "up_internal.h"
+#include "up_arch.h"
+
+#include "stm32_internal.h"
+#include "stm32_gpio.h"
+#include "stm32_tim.h"
+
+#ifdef CONFIG_HRT_TIMER
+
+/* HRT configuration */
+#if   HRT_TIMER == 1
+# define HRT_TIMER_BASE		STM32_TIM1_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM1EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM1_CLKIN
+# if CONFIG_STM32_TIM1
+#  error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
+# endif
+#elif HRT_TIMER == 2
+# define HRT_TIMER_BASE		STM32_TIM2_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM2EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM2
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM2_CLKIN
+# if CONFIG_STM32_TIM2
+#  error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
+# endif
+#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_VECTOR	STM32_IRQ_TIM3
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
+# if CONFIG_STM32_TIM3
+#  error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
+# endif
+#elif HRT_TIMER == 4
+# define HRT_TIMER_BASE		STM32_TIM4_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM4EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM4
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM4_CLKIN
+# if CONFIG_STM32_TIM4
+#  error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
+# endif
+#elif HRT_TIMER == 5
+# define HRT_TIMER_BASE		STM32_TIM5_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM5EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM5
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM5_CLKIN
+# if CONFIG_STM32_TIM5
+#  error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
+# endif
+#elif HRT_TIMER == 8
+# define HRT_TIMER_BASE		STM32_TIM8_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB2ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM8EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM8CC
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
+# if CONFIG_STM32_TIM8
+#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6
+# endif
+#elif HRT_TIMER == 9
+# define HRT_TIMER_BASE		STM32_TIM9_BASE
+# define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
+# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM9EN
+# define HRT_TIMER_VECTOR	STM32_IRQ_TIM1BRK
+# define HRT_TIMER_CLOCK	STM32_APB1_TIM9_CLKIN
+# if CONFIG_STM32_TIM9
+#  error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
+# endif
+#elif HRT_TIMER == 10
+# define HRT_TIMER_BASE		STM32_TIM10_BASE
+# 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
+# if CONFIG_STM32_TIM10
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
+# endif
+#elif HRT_TIMER == 11
+# define HRT_TIMER_BASE		STM32_TIM11_BASE
+# 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
+# if CONFIG_STM32_TIM11
+#  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
+# endif
+#else
+# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
+#endif
+
+/*
+ * HRT clock must be a multiple of 1MHz greater than 1MHz
+ */
+#if (HRT_TIMER_CLOCK % 1000000) != 0
+# error HRT_TIMER_CLOCK must be a multiple of 1MHz
+#endif
+#if HRT_TIMER_CLOCK <= 1000000
+# error HRT_TIMER_CLOCK must be greater than 1MHz
+#endif
+
+/*
+ * Minimum/maximum deadlines.
+ *
+ * These are suitable for use with a 16-bit timer/counter clocked
+ * at 1MHz.  The high-resolution timer need only guarantee that it
+ * not wrap more than once in the 50ms period for absolute time to
+ * be consistently maintained.
+ *
+ * The minimum deadline must be such that the time taken between
+ * reading a time and writing a deadline to the timer cannot
+ * result in missing the deadline.
+ */
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
+/*
+ * Period of the free-running counter, in microseconds.
+ */
+#define HRT_COUNTER_PERIOD	65536
+
+/*
+ * Scaling factor(s) for the free-running counter; convert an input
+ * in counts to a time in microseconds.
+ */
+#define HRT_COUNTER_SCALE(_c)	(_c)
+
+/*
+ * Timer register accessors
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
+
+#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+
+/*
+ * Specific registers and bits used by HRT sub-functions
+ */
+#if HRT_TIMER_CHANNEL == 1
+# define rCCR_HRT	rCCR1			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC1IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 2
+# define rCCR_HRT	rCCR2			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC2IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC2IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 3
+# define rCCR_HRT	rCCR3			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC3IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC3IF		/* interrupt status for HRT */
+#elif HRT_TIMER_CHANNEL == 4
+# define rCCR_HRT	rCCR4			/* compare register for HRT */
+# define DIER_HRT	GTIM_DIER_CC4IE		/* interrupt enable for HRT */
+# define SR_INT_HRT	GTIM_SR_CC4IF		/* interrupt status for HRT */
+#else
+# error HRT_TIMER_CHANNEL must be a value between 1 and 4
+#endif
+
+/*
+ * Queue of callout entries.
+ */
+static struct sq_queue_s	callout_queue;
+
+/* latency baseline (last compare value applied) */
+static uint16_t			latency_baseline;
+
+/* timer count at interrupt (for latency purposes) */
+static uint16_t			latency_actual;
+
+/* latency histogram */
+#define LATENCY_BUCKET_COUNT	8
+static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+/* timer-specific functions */
+static void		hrt_tim_init(void);
+static int		hrt_tim_isr(int irq, void *context);
+static void		hrt_latency_update(void);
+
+/* callout list manipulation */
+static void		hrt_call_internal(struct hrt_call *entry,
+					  hrt_abstime deadline,
+					  hrt_abstime interval,
+					  hrt_callout callout,
+					  void *arg);
+static void		hrt_call_enter(struct hrt_call *entry);
+static void		hrt_call_reschedule(void);
+static void		hrt_call_invoke(void);
+
+/*
+ * Specific registers and bits used by PPM sub-functions
+ */
+#ifdef CONFIG_HRT_PPM
+# if HRT_PPM_CHANNEL == 1
+#  define rCCR_PPM	rCCR1			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC1IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC1OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	1			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */
+# elif HRT_PPM_CHANNEL == 2
+#  define rCCR_PPM	rCCR2			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC2IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC2IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC2OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	2			/* not on TI1/TI2 */
+#  define CCMR2_PPM	0			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */
+# elif HRT_PPM_CHANNEL == 3
+#  define rCCR_PPM	rCCR3			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC3IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC3IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC3OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	1			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */
+# elif HRT_PPM_CHANNEL == 4
+#  define rCCR_PPM	rCCR4			/* capture register for PPM */
+#  define DIER_PPM	GTIM_DIER_CC4IE		/* capture interrupt (non-DMA mode) */
+#  define SR_INT_PPM	GTIM_SR_CC4IF		/* capture interrupt (non-DMA mode) */
+#  define SR_OVF_PPM	GTIM_SR_CC4OF		/* capture overflow (non-DMA mode) */
+#  define CCMR1_PPM	0			/* not on TI1/TI2 */
+#  define CCMR2_PPM	2			/* on TI3, not on TI4 */
+#  define CCER_PPM	(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
+# else
+#  error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
+# endif
+
+/*
+ * PPM decoder tuning parameters
+ */
+# define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+# define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+# define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+# define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* decoded PPM buffer */
+#define PPM_MAX_CHANNELS	12
+__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
+__EXPORT unsigned ppm_decoded_channels;
+__EXPORT uint64_t ppm_last_valid_decode = 0;
+
+/* PPM edge history */
+__EXPORT uint16_t ppm_edge_history[32];
+unsigned ppm_edge_next;
+
+/* PPM pulse history */
+__EXPORT uint16_t ppm_pulse_history[32];
+unsigned ppm_pulse_next;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+static void	hrt_ppm_decode(uint32_t status);
+
+#else
+/* disable the PPM configuration */
+# define rCCR_PPM	0
+# define DIER_PPM	0
+# define SR_INT_PPM	0
+# define SR_OVF_PPM	0
+# define CCMR1_PPM	0
+# define CCMR2_PPM	0
+# define CCER_PPM	0
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Initialise the timer we are going to use.
+ *
+ * We expect that we'll own one of the reduced-function STM32 general
+ * timers, and that we can use channel 1 in compare mode.
+ */
+static void
+hrt_tim_init(void)
+{
+	/* clock/power on our timer */
+        modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
+
+        /* claim our interrupt vector */
+        irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr);
+
+        /* disable and configure the timer */
+        rCR1 = 0;
+        rCR2 = 0;
+        rSMCR = 0;
+        rDIER = DIER_HRT | DIER_PPM;
+        rCCER = 0;		/* unlock CCMR* registers */
+        rCCMR1 = CCMR1_PPM;		
+        rCCMR2 = CCMR2_PPM;
+        rCCER = CCER_PPM;
+        rDCR = 0;
+
+        /* configure the timer to free-run at 1MHz */
+        rPSC = (HRT_TIMER_CLOCK / 1000000) - 1;	/* this really only works for whole-MHz clocks */
+
+        /* run the full span of the counter */
+        rARR = 0xffff;
+
+        /* set an initial capture a little ways off */
+        rCCR_HRT = 1000;
+
+        /* generate an update event; reloads the counter, all registers */
+        rEGR = GTIM_EGR_UG;
+
+        /* enable the timer */
+        rCR1 = GTIM_CR1_CEN;
+
+        /* enable interrupts */
+        up_enable_irq(HRT_TIMER_VECTOR);
+}
+
+#ifdef CONFIG_HRT_PPM
+/*
+ * Handle the PPM decoder state machine.
+ */
+static void
+hrt_ppm_decode(uint32_t status) 
+{
+	uint16_t count = rCCR_PPM;
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (status & SR_OVF_PPM)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+	ppm.last_edge = count;
+
+	ppm_edge_history[ppm_edge_next++] = width;
+	if (ppm_edge_next >= 32)
+		ppm_edge_next = 0;
+
+	/* 
+	 * if this looks like a start pulse, then push the last set of values
+	 * and reset the state machine
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/* export the last set of samples if we got something sensible */
+		if (ppm.next_channel > 4) {
+			for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++)
+				ppm_buffer[i] = ppm_temp_buffer[i];
+			ppm_decoded_channels = i;
+			ppm_last_valid_decode = hrt_absolute_time();
+
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+	
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+		return;
+
+	case ACTIVE:
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		ppm_pulse_history[ppm_pulse_next++] = interval;
+		if (ppm_pulse_next >= 32)
+			ppm_pulse_next = 0;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;		
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+
+}
+#endif /* CONFIG_HRT_PPM */
+
+/*
+ * Handle the compare interupt by calling the callout dispatcher
+ * and then re-scheduling the next deadline.
+ */
+static int
+hrt_tim_isr(int irq, void *context)
+{
+	uint32_t status;
+
+	/* grab the timer for latency tracking purposes */
+	latency_actual = rCNT;
+
+	/* copy interrupt status */
+	status = rSR;
+
+	/* ack the interrupts we just read */
+	rSR = ~status;
+
+#ifdef CONFIG_HRT_PPM
+	/* was this a PPM edge? */
+	if (status & (SR_INT_PPM | SR_OVF_PPM))
+		hrt_ppm_decode(status);
+#endif
+
+	/* was this a timer tick? */
+	if (status & SR_INT_HRT) {
+
+		/* do latency calculations */
+		hrt_latency_update();
+
+		/* run any callouts that have met their deadline */
+		hrt_call_invoke();
+
+		/* and schedule the next interrupt */
+		hrt_call_reschedule();
+	}
+
+	return OK;
+}
+
+/*
+ * Fetch a never-wrapping absolute time value in microseconds from
+ * some arbitrary epoch shortly after system start.
+ */
+hrt_abstime
+hrt_absolute_time(void)
+{
+	hrt_abstime	abstime;
+	uint32_t	count;
+	uint32_t	flags;
+
+	/*
+	 * Counter state.  Marked volatile as they may change
+	 * inside this routine but outside the irqsave/restore
+	 * pair.  Discourage the compiler from moving loads/stores
+	 * to these outside of the protected range.
+	 */
+	static volatile hrt_abstime base_time;
+	static volatile uint32_t last_count;
+
+	/* prevent re-entry */
+	flags = irqsave();
+
+	/* get the current counter value */
+	count = rCNT;
+
+	/*
+	 * Determine whether the counter has wrapped since the
+	 * last time we're called.
+	 *
+	 * This simple test is sufficient due to the guarantee that
+	 * we are always called at least once per counter period.
+	 */
+	if (count < last_count)
+		base_time += HRT_COUNTER_PERIOD;
+
+	/* save the count for next time */
+	last_count = count;
+
+	/* compute the current time */
+	abstime = HRT_COUNTER_SCALE(base_time + count);
+
+	irqrestore(flags);
+
+	return abstime;
+}
+
+/*
+ * Convert a timespec to absolute time
+ */
+hrt_abstime
+ts_to_abstime(struct timespec *ts)
+{
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
+}
+
+/*
+ * Convert absolute time to a timespec.
+ */
+void
+abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+{
+	ts->tv_sec = abstime / 1000000;
+	abstime -= ts->tv_sec * 1000000;
+	ts->tv_nsec = abstime * 1000;
+}
+
+/*
+ * Initalise the high-resolution timing module.
+ */
+void
+hrt_init(void)
+{
+	sq_init(&callout_queue);
+	hrt_tim_init();
+
+#ifdef CONFIG_HRT_PPM
+	/* configure the PPM input pin */
+	stm32_configgpio(GPIO_PPM_IN);
+#endif
+}
+
+/*
+ * Call callout(arg) after interval has elapsed.
+ */
+void
+hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, 
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at calltime.
+ */
+void
+hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+/*
+ * Call callout(arg) every period.
+ */
+void
+hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	irqstate_t flags = irqsave();
+
+	/* if the entry is currently queued, remove it */
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+
+	irqrestore(flags);
+}
+
+/*
+ * If this returns true, the call has been invoked and removed from the callout list.
+ *
+ * Always returns false for repeating callouts.
+ */
+bool
+hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+/*
+ * Remove the entry from the callout list.
+ */
+void
+hrt_cancel(struct hrt_call *entry)
+{
+	irqstate_t flags = irqsave();
+
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+
+	irqrestore(flags);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+static void
+hrt_call_invoke(void)
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	while (true) {
+		/* get the current time */
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+		if (call == NULL)
+			break;
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline; 
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* invoke the callout (if there is one) */
+		if (call->callout) {
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+		}
+
+		/* if the callout has a non-zero period, it has to be re-entered */
+		if (call->period != 0) {
+			call->deadline = deadline + call->period;
+			hrt_call_enter(call);
+		}
+	}
+}
+
+/*
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in 
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+	//lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
+
+	/* set the new compare value and remember it for latency tracking */
+	rCCR_HRT = latency_baseline = deadline & 0xffff;
+}
+
+static void
+hrt_latency_update(void)
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	/* bounded buckets */
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+	/* catch-all at the end */
+	latency_counters[index]++;
+}
+
+
+#endif /* CONFIG_HRT_TIMER */

apps/drivers/boards/px4fmu/px4fmu_init.c

@@ -0,0 +1,290 @@
+/****************************************************************************
+ *
+ *   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_init.c
+ *
+ * PX4FMU-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/arch.h>
+
+#include "stm32_internal.h"
+#include "px4fmu_internal.h"
+#include "stm32_uart.h"
+
+#include <arch/board/up_cpuload.h>
+#include <arch/board/up_adc.h>
+#include <arch/board/board.h>
+#include <arch/board/drv_led.h>
+#include <arch/board/drv_eeprom.h>
+
+#include <drivers/drv_hrt.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lib_lowprintf(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lib_lowprintf
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT int nsh_archinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi3;
+static struct i2c_dev_s *i2c1;
+static struct i2c_dev_s *i2c2;
+static struct i2c_dev_s *i2c3;
+
+#include <math.h>
+
+#ifdef __cplusplus
+int matherr(struct __exception *e) {
+  return 1;
+}
+#else
+int matherr(struct exception *e) {
+  return 1;
+}
+#endif
+
+int nsh_archinitialize(void)
+{
+  int result;
+
+  /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
+
+  /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
+
+  /* configure the high-resolution time/callout interface */
+#ifdef CONFIG_HRT_TIMER
+  hrt_init();
+#endif
+
+  /* configure CPU load estimation */
+  #ifdef CONFIG_SCHED_INSTRUMENTATION
+  cpuload_initialize_once();
+  #endif
+
+  /* set up the serial DMA polling */
+#ifdef SERIAL_HAVE_DMA
+  {
+    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);
+  }
+#endif
+
+  message("\r\n");
+
+  up_ledoff(LED_BLUE);
+  up_ledoff(LED_AMBER);
+
+  up_ledon(LED_BLUE);
+
+  /* Configure user-space led driver */
+  px4fmu_led_init();
+
+  /* Configure SPI-based devices */
+
+  spi1 = up_spiinitialize(1);
+  if (!spi1)
+  {
+	  message("[boot] FAILED to initialize SPI port 1\r\n");
+	  up_ledon(LED_AMBER);
+	  return -ENODEV;
+  }
+
+  // Default SPI1 to 1MHz and de-assert the known chip selects.
+  SPI_SETFREQUENCY(spi1, 10000000);
+  SPI_SETBITS(spi1, 8);
+  SPI_SETMODE(spi1, SPIDEV_MODE3);
+  SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+  SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
+  SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+  up_udelay(20);
+
+  message("[boot] Successfully initialized SPI port 1\r\n");
+
+  /* initialize I2C2 bus */
+
+  i2c2 = up_i2cinitialize(2);
+  if (!i2c2) {
+	  message("[boot] FAILED to initialize I2C bus 2\n");
+	  up_ledon(LED_AMBER);
+	  return -ENODEV;
+  }
+
+  /* set I2C2 speed */
+  I2C_SETFREQUENCY(i2c2, 400000);
+
+
+  i2c3 = up_i2cinitialize(3);
+  if (!i2c3) {
+	  message("[boot] FAILED to initialize I2C bus 3\n");
+	  up_ledon(LED_AMBER);
+	  return -ENODEV;
+  }
+
+  /* set I2C3 speed */
+  I2C_SETFREQUENCY(i2c3, 400000);
+
+  /* try to attach, don't fail if device is not responding */
+  (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
+		  FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
+		  FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
+		  FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
+
+#if defined(CONFIG_STM32_SPI3)
+  /* Get the SPI port */
+
+  message("[boot] Initializing SPI port 3\n");
+  spi3 = up_spiinitialize(3);
+  if (!spi3)
+    {
+      message("[boot] FAILED to initialize SPI port 3\n");
+      up_ledon(LED_AMBER);
+      return -ENODEV;
+    }
+  message("[boot] Successfully initialized SPI port 3\n");
+
+  /* Now bind the SPI interface to the MMCSD driver */
+  result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
+  if (result != OK)
+  {
+	  message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
+	  up_ledon(LED_AMBER);
+	  return -ENODEV;
+  }
+  message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
+#endif /* SPI3 */
+
+  /* initialize I2C1 bus */
+
+  i2c1 = up_i2cinitialize(1);
+  if (!i2c1) {
+    message("[boot] FAILED to initialize I2C bus 1\n");
+    up_ledon(LED_AMBER);
+    return -ENODEV;
+  }
+
+  /* set I2C1 speed */
+  I2C_SETFREQUENCY(i2c1, 400000);
+
+  /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
+
+  /* Get board information if available */
+
+  /* Initialize the user GPIOs */
+  px4fmu_gpio_init();
+
+#ifdef CONFIG_ADC
+  int adc_state = adc_devinit();
+  if (adc_state != OK)
+  {
+    /* Try again */
+    adc_state = adc_devinit();
+    if (adc_state != OK)
+    {
+      /* Give up */
+      message("[boot] FAILED adc_devinit: %d\n", adc_state);
+      return -ENODEV;
+    }
+  }
+#endif
+
+  return OK;
+}

apps/drivers/boards/px4fmu/px4fmu_internal.h

@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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_internal.h
+ *
+ * PX4FMU internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+
+#ifdef CONFIG_STM32_SPI2
+#  error "SPI2 is not supported on this board"
+#endif
+
+#if defined(CONFIG_STM32_CAN1)
+#  warning "CAN1 is not supported on this board"
+#endif
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+
+/* External interrupts */
+#define GPIO_EXTI_COMPASS	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1)
+// XXX MPU6000 DRDY?
+
+/* SPI chip selects */
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
+#define GPIO_SPI_CS_ACCEL	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+#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)
+
+/* User GPIOs
+ *
+ * GPIO0-1 are the buffered high-power GPIOs.
+ * GPIO2-5 are the USART2 pins.
+ * GPIO6-7 are the CAN2 pins.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0)
+#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_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN2)
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#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_PORTB|GPIO_PIN13)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_GPIO_DIR		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing (also connected to the green LED)
+ */
+#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* PWM
+ *
+ * The PX4FMU has five PWM outputs, of which only the output on
+ * pin PC8 is fixed assigned to this function. The other four possible
+ * pwm sources are the TX, RX, RTS and CTS pins of USART2
+ *
+ * Alternate function mapping:
+ * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2
+ */
+
+#ifdef CONFIG_PWM
+#  if defined(CONFIG_STM32_TIM3_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#	 define BUZZER_PWMTIMER 3
+#  elif defined(CONFIG_STM32_TIM8_PWM)
+#    define BUZZER_PWMCHANNEL 3
+#    define BUZZER_PWMTIMER 8
+#  endif
+#endif
+
+/****************************************************************************************************
+ * 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);
+
+/****************************************************************************************************
+ * Name: px4fmu_gpio_init
+ *
+ * Description:
+ *   Called to configure the PX4FMU user GPIOs
+ *
+ ****************************************************************************************************/
+
+extern void px4fmu_gpio_init(void);
+
+
+// XXX additional SPI chipselect functions required?
+
+#endif /* __ASSEMBLY__ */

apps/drivers/drv_hrt.h

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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 drv_hrt.h
+ *
+ * High-resolution timer with callouts and timekeeping.
+ */
+
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+
+#include <time.h>
+#include <queue.h>
+
+__BEGIN_DECLS
+
+/*
+ * Absolute time, in microsecond units.
+ *
+ * Absolute time is measured from some arbitrary epoch shortly after
+ * system startup.  It should never wrap or go backwards.
+ */
+typedef uint64_t	hrt_abstime;
+
+/*
+ * Callout function type.
+ *
+ * Note that callouts run in the timer interrupt context, so
+ * they are serialised with respect to each other, and must not
+ * block.
+ */
+typedef void		(* hrt_callout)(void *arg);
+
+/*
+ * Callout record.
+ */
+typedef struct hrt_call {
+	struct sq_entry_s	link;
+
+	hrt_abstime		deadline;
+	hrt_abstime		period;
+	hrt_callout		callout;
+	void			*arg;
+} *hrt_call_t;
+
+/*
+ * Get absolute time.
+ */
+__EXPORT extern hrt_abstime hrt_absolute_time(void);
+
+/*
+ * Convert a timespec to absolute time.
+ */
+__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+
+/*
+ * Convert absolute time to a timespec.
+ */
+__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+__EXPORT extern void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+__EXPORT extern void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg);
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+__EXPORT extern void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg);
+
+/*
+ * If this returns true, the entry has been invoked and removed from the callout list, 
+ * or it has never been entered.
+ *
+ * Always returns false for repeating callouts.
+ */
+__EXPORT extern bool	hrt_called(struct hrt_call *entry);
+
+/*
+ * Remove the entry from the callout list.
+ */
+__EXPORT extern void	hrt_cancel(struct hrt_call *entry);
+
+/*
+ * Initialise the HRT.
+ */
+__EXPORT extern void	hrt_init(void);
+
+__END_DECLS

apps/drivers/hmc5883/hmc5883.cpp

@@ -58,7 +58,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -115,6 +115,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 class HMC5883 : public device::I2C
 {
 public:

apps/drivers/l3gd20/l3gd20.cpp

@@ -57,7 +57,7 @@
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 
 #include <drivers/device/spi.h>

apps/drivers/mpu6000/mpu6000.cpp

@@ -61,7 +61,7 @@
 #include <nuttx/clock.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>

apps/drivers/ms5611/ms5611.cpp

@@ -57,7 +57,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -70,6 +70,10 @@
 #endif
 static const int ERROR = -1;
 
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
 /**
  * Calibration PROM as reported by the device.
  */

apps/drivers/stm32/tone_alarm/tone_alarm.cpp

@@ -71,7 +71,7 @@
 #include <unistd.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <arch/stm32/chip.h>
 #include <up_internal.h>

apps/fixedwing_control/fixedwing_control.c

@@ -48,7 +48,7 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>

apps/gps/mtk.c

@@ -43,7 +43,7 @@
 #include <pthread.h>
 #include <poll.h>
 #include <fcntl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>

apps/gps/nmea_helper.c

@@ -44,7 +44,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <mavlink/mavlink_log.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
 #define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2

apps/gps/ubx.c

@@ -40,7 +40,7 @@
 #include "gps.h"
 #include <sys/prctl.h>
 #include <poll.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <string.h>
 #include <stdbool.h>

apps/mavlink/mavlink.c

@@ -50,7 +50,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/mavlink/mavlink_receiver.c

@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/mavlink/missionlib.c

@@ -49,7 +49,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/mavlink/orb_listener.c

@@ -46,7 +46,7 @@
 #include <string.h>
 #include "mavlink_bridge_header.h"
 #include <v1.0/common/mavlink.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -54,7 +54,7 @@
 #include <math.h>
 #include <poll.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
 #include <uORB/topics/vehicle_status.h>

apps/multirotor_att_control/multirotor_attitude_control.c

@@ -52,7 +52,7 @@
 #include <math.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/param/param.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);

apps/multirotor_att_control/multirotor_rate_control.c

@@ -53,7 +53,7 @@
 #include <systemlib/pid/pid.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);

apps/multirotor_pos_control/multirotor_pos_control.c

@@ -48,7 +48,7 @@
 #include <termios.h>
 #include <time.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>

apps/px4/attitude_estimator_bm/attitude_estimator_bm.c

@@ -46,7 +46,7 @@
 #include <sys/time.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <string.h>
 #include <poll.h>
 #include <uORB/uORB.h>

apps/px4/tests/test_hrt.c

@@ -50,7 +50,7 @@
 #include <unistd.h>
 
 #include <arch/board/board.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 
 #include <nuttx/spi.h>

apps/px4/tests/test_time.c

@@ -53,7 +53,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************

apps/px4/tests/test_uart_console.c

@@ -56,7 +56,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************

apps/px4/tests/test_uart_send.c

@@ -56,7 +56,7 @@
 
 #include <math.h>
 #include <float.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 
 /****************************************************************************

apps/px4/tests/tests_file.c

@@ -44,7 +44,7 @@
 #include <systemlib/perf_counter.h>
 #include <string.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "tests.h"
 

apps/px4io/comms.c

@@ -48,7 +48,7 @@
 
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 

apps/px4io/mixer.c

@@ -47,7 +47,7 @@
 
 #include <arch/board/drv_ppm_input.h>
 #include <arch/board/drv_pwm_servo.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "px4io.h"
 

apps/px4io/px4io.c

@@ -50,7 +50,7 @@
 #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 <drivers/drv_hrt.h>
 
 #include "px4io.h"
 

apps/px4io/safety.c

@@ -49,7 +49,7 @@
 #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 <drivers/drv_hrt.h>
 
 #include "px4io.h"
 

apps/sdlog/sdlog.c

@@ -51,7 +51,7 @@
 #include <string.h>
 #include <systemlib/err.h>
 #include <unistd.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>

apps/sensors/sensors.cpp

@@ -52,7 +52,7 @@
 #include <errno.h>
 #include <math.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>

apps/systemcmds/led/led.c

@@ -50,7 +50,7 @@
 #include <termios.h>
 #include <time.h>
 #include <sys/prctl.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 #include <arch/board/drv_led.h>
 
 #include <systemlib/err.h>

apps/systemcmds/top/top.c

@@ -47,7 +47,7 @@
 #include <poll.h>
 
 #include <arch/board/up_cpuload.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 /**
  * Start the top application.

apps/systemlib/Makefile

@@ -41,8 +41,13 @@ CSRCS		 = err.c \
 		   param/param.c \
 		   bson/tinybson.c \
 		   conversions.c \
+		   cpuload.c \
 		   getopt_long.c
 
+#		   ppm_decode.c \
+
+
+
 #
 # XXX this really should be a CONFIG_* test
 #

apps/systemlib/cpuload.c

@@ -0,0 +1,180 @@
+/****************************************************************************
+ * configs/px4fmu/src/up_leds.c
+ * arch/arm/src/board/up_leds.c
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <sys/time.h>
+#include <sched.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+
+#include "cpuload.h"
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+__EXPORT void sched_note_start(FAR _TCB *tcb);
+__EXPORT void sched_note_stop(FAR _TCB *tcb);
+__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
+
+/****************************************************************************
+ * Name:
+ ****************************************************************************/
+
+__EXPORT struct system_load_s system_load;
+
+extern FAR _TCB *sched_gettcb(pid_t pid);
+
+void cpuload_initialize_once()
+{
+//	if (!system_load.initialized)
+//	{
+		system_load.start_time = hrt_absolute_time();
+		int i;
+		for (i = 0; i < CONFIG_MAX_TASKS; i++)
+		{
+			system_load.tasks[i].valid = false;
+		}
+		system_load.total_count = 0;
+
+		uint64_t now = hrt_absolute_time();
+
+		/* initialize idle thread statically */
+		system_load.tasks[0].start_time = now;
+		system_load.tasks[0].total_runtime = 0;
+		system_load.tasks[0].curr_start_time = 0;
+		system_load.tasks[0].tcb = sched_gettcb(0);
+		system_load.tasks[0].valid = true;
+		system_load.total_count++;
+
+		/* initialize init thread statically */
+		system_load.tasks[1].start_time = now;
+		system_load.tasks[1].total_runtime = 0;
+		system_load.tasks[1].curr_start_time = 0;
+		system_load.tasks[1].tcb = sched_gettcb(1);
+		system_load.tasks[1].valid = true;
+		/* count init thread */
+		system_load.total_count++;
+		//	}
+}
+
+void sched_note_start(FAR _TCB *tcb )
+{
+	/* search first free slot */
+	int i;
+	for (i = 1; i < CONFIG_MAX_TASKS; i++)
+	{
+		if (!system_load.tasks[i].valid)
+		{
+			/* slot is available */
+			system_load.tasks[i].start_time = hrt_absolute_time();
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = tcb;
+			system_load.tasks[i].valid = true;
+			system_load.total_count++;
+			break;
+		}
+	}
+}
+
+void sched_note_stop(FAR _TCB *tcb )
+{
+	int i;
+	for (i = 1; i < CONFIG_MAX_TASKS; i++)
+	{
+		if (system_load.tasks[i].tcb->pid == tcb->pid)
+		{
+			/* mark slot as fee */
+			system_load.tasks[i].valid = false;
+			system_load.tasks[i].total_runtime = 0;
+			system_load.tasks[i].curr_start_time = 0;
+			system_load.tasks[i].tcb = NULL;
+			system_load.total_count--;
+			break;
+		}
+	}
+}
+
+void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+{
+	uint64_t new_time = hrt_absolute_time();
+
+	/* Kind of inefficient: find both tasks and update times */
+	uint8_t both_found = 0;
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++)
+	{
+		/* Task ending its current scheduling run */
+		if (system_load.tasks[i].tcb->pid == pFromTcb->pid)
+		{
+			//if (system_load.tasks[i].curr_start_time != 0)
+			{
+				system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time;
+			}
+			both_found++;
+		}
+		else if (system_load.tasks[i].tcb->pid == pToTcb->pid)
+		{
+			system_load.tasks[i].curr_start_time = new_time;
+			both_found++;
+		}
+
+		/* Do only iterate as long as needed */
+		if (both_found == 2)
+		{
+			break;
+		}
+	}
+}
+
+#endif /* CONFIG_SCHED_INSTRUMENTATION */

apps/systemlib/cpuload.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+
+__BEGIN_DECLS
+
+#include <nuttx/sched.h>
+
+struct system_load_taskinfo_s {
+	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
+	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
+	uint64_t start_time;         ///< FIRST start time of task
+	FAR struct _TCB *tcb;               ///<
+	bool valid;                  ///< Task is currently active / valid
+};
+
+struct system_load_s {
+	uint64_t start_time;         ///< Global start time of measurements
+	struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
+	uint8_t initialized;
+	int total_count;
+	int running_count;
+	int sleeping_count;
+};
+
+__EXPORT extern struct system_load_s system_load;
+
+__EXPORT void cpuload_initialize_once(void);
+
+#endif

apps/systemlib/param/param.c

@@ -50,7 +50,7 @@
 
 #include <sys/stat.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "systemlib/param/param.h"
 #include "systemlib/uthash/utarray.h"

apps/systemlib/perf_counter.c

@@ -40,7 +40,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <sys/queue.h>
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include "perf_counter.h"
 

apps/systemlib/ppm_decode.c

@@ -0,0 +1,242 @@
+/****************************************************************************
+ *
+ *   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 ppm_decode.c
+ *
+ * PPM input decoder.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "ppm_decode.h"
+
+/*
+ * PPM decoder tuning parameters.
+ *
+ * The PPM decoder works as follows.
+ *
+ * Initially, the decoder waits in the UNSYNCH state for two edges 
+ * separated by PPM_MIN_START.  Once the second edge is detected,
+ * the decoder moves to the ARM state.
+ *
+ * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the
+ * timing mark for the first channel.  If this is detected, it moves to
+ * the INACTIVE state.
+ *
+ * The INACTIVE phase waits for and discards the next edge, as it is not
+ * significant.  Once the edge is detected, it moves to the ACTIVE stae.
+ *
+ * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
+ * received calculates the time from the previous mark and records
+ * this time as the value for the next channel. 
+ *
+ * If at any time waiting for an edge, the delay from the previous edge 
+ * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
+ * values are advertised to clients.
+ */
+#define PPM_MAX_PULSE_WIDTH	500		/* maximum width of a pulse */
+#define PPM_MIN_CHANNEL_VALUE	800		/* shortest valid channel signal */
+#define PPM_MAX_CHANNEL_VALUE	2200		/* longest valid channel signal */
+#define PPM_MIN_START		2500		/* shortest valid start gap */
+
+/* Input timeout - after this interval we assume signal is lost */
+#define PPM_INPUT_TIMEOUT	100 * 1000	/* 100ms */
+
+/* Number of same-sized frames required to 'lock' */
+#define PPM_CHANNEL_LOCK	3		/* should be less than the input timeout */
+
+/* decoded PPM buffer */
+#define PPM_MIN_CHANNELS	4
+#define PPM_MAX_CHANNELS	12
+
+/*
+ * Public decoder state
+ */
+uint16_t	ppm_buffer[PPM_MAX_CHANNELS];
+unsigned	ppm_decoded_channels;
+hrt_abstime	ppm_last_valid_decode;
+
+static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
+
+/* PPM decoder state machine */
+static struct {
+	uint16_t	last_edge;	/* last capture time */
+	uint16_t	last_mark;	/* last significant edge */
+	unsigned	next_channel;
+	unsigned	count_max;
+	enum {
+		UNSYNCH = 0,
+		ARM,
+		ACTIVE,
+		INACTIVE
+	} phase;
+} ppm;
+
+
+void
+ppm_input_init(unsigned count_max)
+{
+	ppm_decoded_channels = 0;
+	ppm_last_valid_decode = 0;
+
+	memset(&ppm, 0, sizeof(ppm));
+	ppm.count_max = count_max;
+}
+
+void
+ppm_input_decode(bool reset, unsigned count)
+{
+	uint16_t width;
+	uint16_t interval;
+	unsigned i;
+
+	/* if we missed an edge, we have to give up */
+	if (reset)
+		goto error;
+
+	/* how long since the last edge? */
+	width = count - ppm.last_edge;
+	if (count < ppm.last_edge)
+		width += ppm.count_max;	/* handle wrapped count */
+	ppm.last_edge = count;
+
+	/* 
+	 * If this looks like a start pulse, then push the last set of values
+	 * and reset the state machine.
+	 *
+	 * Note that this is not a "high performance" design; it implies a whole
+	 * frame of latency between the pulses being received and their being 
+	 * considered valid.
+	 */
+	if (width >= PPM_MIN_START) {
+
+		/* 
+		 * If the number of channels changes unexpectedly, we don't want
+		 * to just immediately jump on the new count as it may be a result
+		 * of noise or dropped edges.  Instead, take a few frames to settle.
+		 */
+		if (ppm.next_channel != ppm_decoded_channels) {
+			static unsigned new_channel_count;
+			static unsigned new_channel_holdoff;
+
+			if (new_channel_count != ppm.next_channel) {
+				/* start the lock counter for the new channel count */
+				new_channel_count = ppm.next_channel;
+				new_channel_holdoff = PPM_CHANNEL_LOCK;
+
+			} else if (new_channel_holdoff > 0) {
+				/* this frame matched the last one, decrement the lock counter */
+				new_channel_holdoff--;
+
+			} else {
+				/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
+				ppm_decoded_channels = new_channel_count;
+				new_channel_count = 0;
+			}
+		} else {
+			/* frame channel count matches expected, let's use it */
+			if (ppm.next_channel > PPM_MIN_CHANNELS) {
+				for (i = 0; i < ppm.next_channel; i++)
+					ppm_buffer[i] = ppm_temp_buffer[i];
+				ppm_last_valid_decode = hrt_absolute_time();
+			}
+		}
+
+		/* reset for the next frame */
+		ppm.next_channel = 0;
+
+		/* next edge is the reference for the first channel */
+		ppm.phase = ARM;
+
+		return;
+	}
+
+	switch (ppm.phase) {
+	case UNSYNCH:
+		/* we are waiting for a start pulse - nothing useful to do here */
+		return;
+
+	case ARM:
+		/* we expect a pulse giving us the first mark */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+	
+		/* record the mark timing, expect an inactive edge */
+		ppm.last_mark = count;
+		ppm.phase = INACTIVE;
+		return;
+
+	case INACTIVE:
+		/* this edge is not interesting, but now we are ready for the next mark */
+		ppm.phase = ACTIVE;
+
+		/* note that we don't bother looking at the timing of this edge */
+
+		return;
+
+	case ACTIVE:
+		/* we expect a well-formed pulse */
+		if (width > PPM_MAX_PULSE_WIDTH)
+			goto error;		/* pulse was too long */
+
+		/* determine the interval from the last mark */
+		interval = count - ppm.last_mark;
+		ppm.last_mark = count;
+
+		/* if the mark-mark timing is out of bounds, abandon the frame */
+		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
+			goto error;
+
+		/* if we have room to store the value, do so */
+		if (ppm.next_channel < PPM_MAX_CHANNELS)
+			ppm_temp_buffer[ppm.next_channel++] = interval;
+
+		ppm.phase = INACTIVE;
+		return;		
+
+	}
+
+	/* the state machine is corrupted; reset it */
+
+error:
+	/* we don't like the state of the decoder, reset it and try again */
+	ppm.phase = UNSYNCH;
+	ppm_decoded_channels = 0;
+}
+

apps/systemlib/ppm_decode.h

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 ppm_decode.h
+ *
+ * PPM input decoder.
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+/**
+ * Maximum number of channels that we will decode.
+ */
+#define PPM_MAX_CHANNELS	12
+
+__BEGIN_DECLS
+
+/*
+ * PPM decoder state
+ */
+__EXPORT extern uint16_t	ppm_buffer[];		/**< decoded PPM channel values */
+__EXPORT extern unsigned	ppm_decoded_channels;	/**< count of decoded channels */
+__EXPORT extern hrt_abstime	ppm_last_valid_decode;	/**< timestamp of the last valid decode */
+
+/**
+ * Initialise the PPM input decoder.
+ *
+ * @param count_max		The maximum value of the counter passed to
+ *				ppm_input_decode, used to determine how to
+ *				handle counter wrap.
+ */
+__EXPORT void		ppm_input_init(unsigned count_max);
+
+/**
+ * Inform the decoder of an edge on the PPM input.
+ *
+ * This function can be registered with the HRT as the PPM edge handler.
+ *
+ * @param reset			If set, the edge detector has missed one or
+ *				more edges and the decoder needs to be reset.
+ * @param count			A microsecond timestamp corresponding to the
+ *				edge, in the range 0-count_max.  This value
+ *				is expected to wrap.
+ */
+__EXPORT void		ppm_input_decode(bool reset, unsigned count);
+
+__END_DECLS

apps/uORB/uORB.cpp

@@ -56,7 +56,7 @@
 #include <nuttx/wqueue.h>
 #include <nuttx/clock.h>
 
-#include <arch/board/up_hrt.h>
+#include <drivers/drv_hrt.h>
 
 #include <drivers/drv_orb_dev.h>