Merge branch 'next' into ed_mac_halt_bug

HISTORY.txt

@@ -1,5 +1,33 @@
 Short summary of changes.  For a complete list see the git log.
 
+2011-09-12
+Max rate now ONLY applies to attitude and axis lock mode.  Manual rate is the
+only term that limits the rate mode now (and in axis lock when you push stick
+only manual rate applies).  Also integrals are reset when unused.
+
+2011-09-09
+Some large updates to the input system.  Now multiple receivers can be
+connected at once.  A wizard was added for configuring the input channels.  A
+specific collective pitch channel was added.
+
+2011-09-04
+Improvements to the failsafe handling code for inputs.  PWM power off is now
+detected properly.  Powering on transmitter for Spektrum Satellite no longer
+causes a glitch on servos.
+
+2011-08-10
+Added Camera Stabilization and a gui to configure this.  This is a software
+selectable module from the GUI.  However, a restart is required to make it
+active.  The GUI does not currently expose the configuration for using the
+transmitter to change the view angle but this is supported by the hardware.
+
+2011-08-10
+By default a lot of diagnostic objects that were enabled by default are now
+disabled in the build.  This include TaskInfo (and all the FreeRTOS options
+that provide that debugging information).  Also MixerStatus, I2CStatus,
+WatchdogStatus and RateDesired.  These can be reenabled for debugging with
+-DDIAGNOSTICS.
+
 2011-08-04
 Fixed packaging aesthetic issues.  Also avoid runtime issues on OSX Lion by
 disabling the ModelView and Notify plugins for now (sorry).

MILESTONES.txt

@@ -137,6 +137,16 @@ C: Mat Wellington
 D: July 2011
 V: http://www.youtube.com/watch?v=YE4Fd9vdg1I
 
+M: First CopterControl Flybared Heli inverted flight (2:33)
+C: Maxim Izergin (Maximus43)
+D: August 2011
+V: http://www.youtube.com/watch?v=8SrfIS7OkB4
+
+M: First CopterControl Flybared Heli funnel (4:18), loop (5:35)
+C: Sergey Solodennikov (alconaft43)
+D: August 2011
+V: http://www.youtube.com/watch?v=8SrfIS7OkB4
+
 M: First CopterControl Return to Base Fixed Wing
 C: Eric Price (Corvus Corax)
 D: AUgust 2011

Makefile

@@ -183,13 +183,13 @@ qt_sdk_clean:
 ARM_SDK_DIR := $(TOOLS_DIR)/arm-2011.03
 
 .PHONY: arm_sdk_install
-arm_sdk_install: ARM_SDK_URL  := http://www.codesourcery.com/sgpp/lite/arm/portal/package8734/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2
+arm_sdk_install: ARM_SDK_URL  := https://sourcery.mentor.com/sgpp/lite/arm/portal/package8736/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2
 arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
 # order-only prereq on directory existance:
 arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR)
 arm_sdk_install: arm_sdk_clean
         # download the source only if it's newer than what we already have
-	$(V1) wget -N -P "$(DL_DIR)" "$(ARM_SDK_URL)"
+	$(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(ARM_SDK_URL)"
 
         # binary only release so just extract it
 	$(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)"
@@ -202,8 +202,8 @@ arm_sdk_clean:
 OPENOCD_DIR := $(TOOLS_DIR)/openocd
 
 .PHONY: openocd_install
-openocd_install: OPENOCD_URL  := http://sourceforge.net/projects/openocd/files/openocd/0.4.0/openocd-0.4.0.tar.bz2/download
+openocd_install: OPENOCD_URL  := http://sourceforge.net/projects/openocd/files/openocd/0.5.0/openocd-0.5.0.tar.bz2/download
-openocd_install: OPENOCD_FILE := openocd-0.4.0.tar.bz2
+openocd_install: OPENOCD_FILE := openocd-0.5.0.tar.bz2
 # order-only prereq on directory existance:
 openocd_install: | $(DL_DIR) $(TOOLS_DIR)
 openocd_install: openocd_clean
@@ -218,8 +218,8 @@ openocd_install: openocd_clean
         # build and install
 	$(V1) mkdir -p "$(OPENOCD_DIR)"
 	$(V1) ( \
-	  cd $(DL_DIR)/openocd-build/openocd-0.4.0 ; \
+	  cd $(DL_DIR)/openocd-build/openocd-0.5.0 ; \
-	  ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi ; \
+	  ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate; \
 	  $(MAKE) ; \
 	  $(MAKE) install ; \
 	)

flight/AHRS/Makefile

@@ -355,7 +355,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/BootloaderUpdater/Makefile

@@ -258,7 +258,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
 CFLAGS += -mapcs-frame 
 CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
+endif
 
 CFLAGS += -Wall 
 CFLAGS += -Werror

flight/Bootloaders/CopterControl/Makefile

@@ -300,7 +300,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
 CFLAGS += -mapcs-frame 
 CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
+endif
 
 CFLAGS += -Wall 
 CFLAGS += -Werror

flight/Bootloaders/OpenPilot/Makefile

@@ -306,7 +306,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
 CFLAGS += -mapcs-frame 
 CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
+endif
 
 CFLAGS += -Wall 
 CFLAGS += -Werror

flight/Bootloaders/PipXtreme/Makefile

@@ -301,7 +301,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
 CFLAGS += -mapcs-frame 
 CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
+endif
 
 CFLAGS += -Wall 
 CFLAGS += -Werror

flight/CopterControl/Makefile

@@ -37,6 +37,9 @@ OUTDIR := $(TOP)/build/$(TARGET)
 # Set to YES to compile for debugging
 DEBUG ?= NO
 
+# Include objects that are just nice information to show
+DIAGNOSTICS ?= NO
+
 # Set to YES to build a FW version that will erase all flash memory
 ERASE_FLASH ?= NO
 # Set to YES to use the Servo output pins for debugging via scope or logic analyser
@@ -62,7 +65,9 @@ endif
 FLASH_TOOL = OPENOCD
 
 # List of modules to include
-MODULES = Telemetry Attitude Stabilization Actuator ManualControl FirmwareIAP
+MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab
+# Telemetry must be last to grab the optional modules
+MODULES += Telemetry
 
 # Paths
 OPSYSTEM = ./System
@@ -125,7 +130,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
 SRC += $(OPSYSTEM)/pios_usb_hid_desc.c
 else
 ## TESTCODE
@@ -153,21 +157,22 @@ SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
 SRC += $(OPUAVSYNTHDIR)/attituderaw.c
 SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
 SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
-SRC += $(OPUAVSYNTHDIR)/taskinfo.c
 SRC += $(OPUAVSYNTHDIR)/i2cstats.c
 SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
 SRC += $(OPUAVSYNTHDIR)/telemetrysettings.c
-SRC += $(OPUAVSYNTHDIR)/ratedesired.c
 SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
 SRC += $(OPUAVSYNTHDIR)/mixersettings.c
-SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
 SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
 SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
+SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
+SRC += $(OPUAVSYNTHDIR)/cameradesired.c
 SRC += $(OPUAVSYNTHDIR)/hwsettings.c
-#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
+SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
-#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
+SRC += $(OPUAVSYNTHDIR)/receiveractivity.c
-# Cant use until i can automatically generate list of UAVObjects
+SRC += $(OPUAVSYNTHDIR)/taskinfo.c
-#SRC += ${OUTDIR}/InitObjects.c
+SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
+SRC += $(OPUAVSYNTHDIR)/ratedesired.c
+
 endif
 
 ## PIOS Hardware (STM32F10x)
@@ -189,6 +194,7 @@ SRC += $(PIOSSTM32F10X)/pios_gpio.c
 SRC += $(PIOSSTM32F10X)/pios_exti.c
 SRC += $(PIOSSTM32F10X)/pios_rtc.c
 SRC += $(PIOSSTM32F10X)/pios_wdg.c
+SRC += $(PIOSSTM32F10X)/pios_tim.c
 
 
 # PIOS USB related files (seperated to make code maintenance more easy)
@@ -207,6 +213,7 @@ SRC += $(PIOSCOMMON)/pios_i2c_esc.c
 SRC += $(PIOSCOMMON)/pios_iap.c
 SRC += $(PIOSCOMMON)/pios_bl_helper.c
 SRC += $(PIOSCOMMON)/pios_rcvr.c
+SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
 SRC += $(PIOSCOMMON)/printf-stdarg.c
 ## Libraries for flight calculations
 SRC += $(FLIGHTLIB)/fifo_buffer.c
@@ -417,6 +424,10 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -DDEBUG
 endif
 
+ifeq ($(DIAGNOSTICS),YES)
+CFLAGS = -DDIAGNOSTICS
+endif
+
 CFLAGS += -g$(DEBUGF)
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
@@ -425,14 +436,16 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
 
 #CFLAGS += -fno-cprop-registers -fno-defer-pop -fno-guess-branch-probability -fno-section-anchors
 #CFLAGS += -fno-if-conversion -fno-if-conversion2 -fno-ipa-pure-const -fno-ipa-reference -fno-merge-constants
-#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename 
+#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename
 #CFLAGS += -fno-tree-dce -fno-tree-dominator-opts -fno-tree-dse -fno-tree-fre -fno-tree-sink -fno-tree-sra
 #CFLAGS += -fno-tree-ter
 #CFLAGS += -g$(DEBUGF) -DDEBUG
 
 CFLAGS += -mapcs-frame
 CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
 CFLAGS += -fpromote-loop-indices
+endif
 
 CFLAGS += -Wall
 CFLAGS += -Werror
@@ -504,7 +517,7 @@ endif
 endif
 
 # Generate code for PyMite
-#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
+#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
 #	@echo $(MSG_PYMITEINIT) $(call toprel, $@)
 #	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
 #	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
@@ -542,7 +555,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf: $(OUTDIR)/$(TARGET).elf

flight/CopterControl/System/alarms.c

@@ -41,10 +41,12 @@ static xSemaphoreHandle lock;
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
 
 /**
- * Initialize the alarms library
+ * Initialize the alarms library
  */
 int32_t AlarmsInitialize(void)
 {
+    SystemAlarmsInitialize();
+    
 	lock = xSemaphoreCreateRecursiveMutex();
 	//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
 	//AlarmsClearAll();
@@ -56,7 +58,7 @@ int32_t AlarmsInitialize(void)
  * Set an alarm
  * @param alarm The system alarm to be modified
  * @param severity The alarm severity
- * @return 0 if success, -1 if an error
+ * @return 0 if success, -1 if an error
  */
 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
 {
@@ -151,7 +153,7 @@ void AlarmsClearAll()
 
 /**
  * Check if there are any alarms with the given or higher severity
- * @return 0 if no alarms are found, 1 if at least one alarm is found
+ * @return 0 if no alarms are found, 1 if at least one alarm is found
  */
 int32_t AlarmsHasWarnings()
 {
@@ -208,5 +210,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 /**
  * @}
  * @}
- */
+ */
 

flight/CopterControl/System/coptercontrol.c

@@ -1,97 +1,107 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
+ * @addtogroup OpenPilotSystem OpenPilot System
- * @brief These files are the core system files of OpenPilot.
+ * @brief These files are the core system files of OpenPilot.
- * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
+ * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
- * in the main() function of openpilot.c
+ * in the main() function of openpilot.c
- * @{
+ * @{
- * @addtogroup OpenPilotCore OpenPilot Core
+ * @addtogroup OpenPilotCore OpenPilot Core
- * @brief This is where the OP firmware starts. Those files also define the compile-time
+ * @brief This is where the OP firmware starts. Those files also define the compile-time
- * options of the firmware.
+ * options of the firmware.
- * @{
+ * @{
- * @file       openpilot.c 
+ * @file       openpilot.c 
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
+ * @brief      Sets up and runs main OpenPilot tasks.
- * @see        The GNU Public License (GPL) Version 3
+ * @see        The GNU Public License (GPL) Version 3
- * 
+ * 
- *****************************************************************************/
+ *****************************************************************************/
-/* 
+/* 
- * This program is free software; you can redistribute it and/or modify 
+ * This program is free software; you can redistribute it and/or modify 
- * it under the terms of the GNU General Public License as published by 
+ * it under the terms of the GNU General Public License as published by 
- * the Free Software Foundation; either version 3 of the License, or 
+ * the Free Software Foundation; either version 3 of the License, or 
- * (at your option) any later version.
+ * (at your option) any later version.
- * 
+ * 
- * This program is distributed in the hope that it will be useful, but 
+ * This program is distributed in the hope that it will be useful, but 
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
- * for more details.
+ * for more details.
- * 
+ * 
- * You should have received a copy of the GNU General Public License along 
+ * You should have received a copy of the GNU General Public License along 
- * with this program; if not, write to the Free Software Foundation, Inc., 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
+ */
-
+
-
+
-/* OpenPilot Includes */
+/* OpenPilot Includes */
-#include "openpilot.h"
+#include "openpilot.h"
-#include "uavobjectsinit.h"
+#include "uavobjectsinit.h"
-#include "systemmod.h"
+#include "hwsettings.h"
-
+#include "camerastab.h"
-/* Task Priorities */
+#include "systemmod.h"
-#define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
+
-
+/* Task Priorities */
-/* Global Variables */
+#define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
-
+
-/* Prototype of PIOS_Board_Init() function */
+/* Global Variables */
-extern void PIOS_Board_Init(void);
+
-extern void Stack_Change(void);
+/* Prototype of PIOS_Board_Init() function */
-
+extern void PIOS_Board_Init(void);
-/**
+extern void Stack_Change(void);
-* OpenPilot Main function:
+
-*
+/**
-* Initialize PiOS<BR>
+* OpenPilot Main function:
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
+*
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
+* Initialize PiOS<BR>
-* If something goes wrong, blink LED1 and LED2 every 100ms
+* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
-*
+* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
-*/
+* If something goes wrong, blink LED1 and LED2 every 100ms
-int main()
+*
-{
+*/
-	/* NOTE: Do NOT modify the following start-up sequence */
+int main()
-	/* Any new initialization functions should be added in OpenPilotInit() */
+{
-
+	/* NOTE: Do NOT modify the following start-up sequence */
-	/* Brings up System using CMSIS functions, enables the LEDs. */
+	/* Any new initialization functions should be added in OpenPilotInit() */
-	PIOS_SYS_Init();
+
-
+	/* Brings up System using CMSIS functions, enables the LEDs. */
-	/* Architecture dependant Hardware and
+	PIOS_SYS_Init();
-	 * core subsystem initialisation
+
-	 * (see pios_board.c for your arch)
+	/* Architecture dependant Hardware and
-	 * */
+	 * core subsystem initialisation
-	PIOS_Board_Init();
+	 * (see pios_board.c for your arch)
-
+	 * */
-	/* Initialize modules */
+	PIOS_Board_Init();
-	MODULE_INITIALISE_ALL
+
-
+	/* Initialize modules */
-	/* swap the stack to use the IRQ stack */
+	MODULE_INITIALISE_ALL
-	Stack_Change();
+
-
+	/* Optional module initialization.  This code might want to go somewhere else as
-	/* Start the FreeRTOS scheduler which should never returns.*/
+	 * it grows */
-	vTaskStartScheduler();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
-
+	HwSettingsOptionalModulesGet(optionalModules);
-	/* If all is well we will never reach here as the scheduler will now be running. */
+	if(optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTABILIZATION] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
-
+		CameraStabInitialize();
-	/* Do some indication to user that something bad just happened */
+	}
-	PIOS_LED_Off(LED1); \
+
-	for(;;) { \
+	/* swap the stack to use the IRQ stack */
-		PIOS_LED_Toggle(LED1); \
+	Stack_Change();
-		PIOS_DELAY_WaitmS(100); \
+
-	};
+	/* Start the FreeRTOS scheduler which should never returns.*/
-
+	vTaskStartScheduler();
-    return 0;
+
-}
+	/* If all is well we will never reach here as the scheduler will now be running. */
-
+
-/**
+	/* Do some indication to user that something bad just happened */
- * @}
+	PIOS_LED_Off(LED1); \
- * @}
+	for(;;) { \
- */
+		PIOS_LED_Toggle(LED1); \
-
+		PIOS_DELAY_WaitmS(100); \
+	};
+
+    return 0;
+}
+
+/**
+ * @}
+ * @}
+ */
+

flight/CopterControl/System/inc/FreeRTOSConfig.h

@@ -26,6 +26,7 @@
 #define configUSE_PREEMPTION		1
 #define configUSE_IDLE_HOOK		1
 #define configUSE_TICK_HOOK		0
+#define configUSE_MALLOC_FAILED_HOOK	1
 #define configCPU_CLOCK_HZ		( ( unsigned long ) 72000000 )
 #define configTICK_RATE_HZ		( ( portTickType ) 1000 )
 #define configMAX_PRIORITIES		( ( unsigned portBASE_TYPE ) 5 )
@@ -39,7 +40,6 @@
 #define configUSE_RECURSIVE_MUTEXES	1
 #define configUSE_COUNTING_SEMAPHORES	0
 #define configUSE_ALTERNATIVE_API	0
-#define configCHECK_FOR_STACK_OVERFLOW	2
 #define configQUEUE_REGISTRY_SIZE	10
 
 /* Co-routine definitions. */
@@ -76,7 +76,9 @@ NVIC value of 255. */
 #endif
 
 /* Enable run time stats collection */
-//#if defined(DEBUG)
+#if defined(DIAGNOSTICS)
+#define configCHECK_FOR_STACK_OVERFLOW	2
+
 #define configGENERATE_RUN_TIME_STATS 1
 #define INCLUDE_uxTaskGetRunTime 1
 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
@@ -85,7 +87,9 @@ do {\
 (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
 } while(0)
 #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
-//#endif
+#else
+#define configCHECK_FOR_STACK_OVERFLOW	1
+#endif
 
 
 /**

flight/CopterControl/System/inc/pios_config.h

@@ -48,8 +48,9 @@
 /* Supported receiver interfaces */
 #define PIOS_INCLUDE_SPEKTRUM
 #define PIOS_INCLUDE_SBUS
-//#define PIOS_INCLUDE_PPM
+#define PIOS_INCLUDE_PPM
 #define PIOS_INCLUDE_PWM
+#define PIOS_INCLUDE_GCSRCVR
 
 /* Supported USART-based PIOS modules */
 #define PIOS_INCLUDE_TELEMETRY_RF

flight/CopterControl/System/pios_board.c

@@ -32,6 +32,7 @@
 #include <uavobjectsinit.h>
 #include <hwsettings.h>
 #include <manualcontrolsettings.h>
+#include <gcsreceiver.h>
 
 #if defined(PIOS_INCLUDE_SPI)
 
@@ -195,6 +196,220 @@ void PIOS_ADC_handler() {
 	PIOS_ADC_DMA_Handler();
 }
 
+#include "pios_tim_priv.h"
+
+static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = {
+	.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
+	.TIM_CounterMode = TIM_CounterMode_Up,
+	.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
+	.TIM_RepetitionCounter = 0x0000,
+};
+
+static const struct pios_tim_clock_cfg tim_1_cfg = {
+	.timer = TIM1,
+	.time_base_init = &tim_1_2_3_4_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM1_CC_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_2_cfg = {
+	.timer = TIM2,
+	.time_base_init = &tim_1_2_3_4_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM2_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_3_cfg = {
+	.timer = TIM3,
+	.time_base_init = &tim_1_2_3_4_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM3_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_4_cfg = {
+	.timer = TIM4,
+	.time_base_init = &tim_1_2_3_4_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
+	{
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_1,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_6,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},
+	{
+		.timer = TIM3,
+		.timer_chan = TIM_Channel_2,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_5,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+		.remap = GPIO_PartialRemap_TIM3,
+	},
+	{
+		.timer = TIM3,
+		.timer_chan = TIM_Channel_3,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_0,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 
+	{
+		.timer = TIM3,
+		.timer_chan = TIM_Channel_4,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_1,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 
+	{ 
+		.timer = TIM2,
+		.timer_chan = TIM_Channel_1,
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_0,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},  	
+	{
+		.timer = TIM2,
+		.timer_chan = TIM_Channel_2,
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_1,
+				.GPIO_Mode  = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 		
+};
+
+static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
+	{
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_4,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_9,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},
+	{
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_3,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_8,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 
+	{
+		.timer = TIM4,
+		.timer_chan = TIM_Channel_2,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_7,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 
+	{
+		.timer = TIM1,
+		.timer_chan = TIM_Channel_1,
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_8,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	}, 
+	{
+		.timer = TIM3,
+		.timer_chan = TIM_Channel_1,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_4,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+		.remap = GPIO_PartialRemap_TIM3,
+	},  	
+	{
+		.timer = TIM2,
+		.timer_chan = TIM_Channel_3,
+		.pin = {
+			.gpio = GPIOA,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_2,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},
+};
+
 #if defined(PIOS_INCLUDE_USART)
 
 #include "pios_usart_priv.h"
@@ -564,121 +779,40 @@ void PIOS_RTC_IRQ_Handler (void)
  * Servo outputs 
  */
 #include <pios_servo_priv.h>
-static const struct pios_servo_channel pios_servo_channels[] = {
-	{
-		.timer = TIM4,
-		.port = GPIOB,
-		.channel = TIM_Channel_4,
-		.pin = GPIO_Pin_9,
-	}, 
-	{
-		.timer = TIM4,
-		.port = GPIOB,
-		.channel = TIM_Channel_3,
-		.pin = GPIO_Pin_8,
-	}, 
-	{
-		.timer = TIM4,
-		.port = GPIOB,
-		.channel = TIM_Channel_2,
-		.pin = GPIO_Pin_7,
-	}, 
-	{
-		.timer = TIM1,
-		.port = GPIOA,
-		.channel = TIM_Channel_1,
-		.pin = GPIO_Pin_8,
-	}, 
-	{ /* needs to remap to alternative function */
-		.timer = TIM3,
-		.port = GPIOB,
-		.channel = TIM_Channel_1,
-		.pin = GPIO_Pin_4,
-	},  	
-	{
-		.timer = TIM2,
-		.port = GPIOA,
-		.channel = TIM_Channel_3,
-		.pin = GPIO_Pin_2,
-	},	
-};
 
 const struct pios_servo_cfg pios_servo_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_oc_init = {
 		.TIM_OCMode = TIM_OCMode_PWM1,
 		.TIM_OutputState = TIM_OutputState_Enable,
 		.TIM_OutputNState = TIM_OutputNState_Disable,
-		.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,		
+		.TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION,
 		.TIM_OCPolarity = TIM_OCPolarity_High,
 		.TIM_OCNPolarity = TIM_OCPolarity_High,
 		.TIM_OCIdleState = TIM_OCIdleState_Reset,
 		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
 	},
-	.gpio_init = {
+	.channels = pios_tim_servoport_all_pins,
-		.GPIO_Mode = GPIO_Mode_AF_PP,
+	.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
-		.GPIO_Speed = GPIO_Speed_2MHz,
-	},
-	.remap = GPIO_PartialRemap_TIM3,
-	.channels = pios_servo_channels,
-	.num_channels = NELEMENTS(pios_servo_channels),
 };
 
-#if defined(PIOS_INCLUDE_PWM) && defined(PIOS_INCLUDE_PPM)
-#error Cannot define both PIOS_INCLUDE_PWM and PIOS_INCLUDE_PPM at the same time (yet)
-#endif
-
 /*
  * PPM Inputs
  */
 #if defined(PIOS_INCLUDE_PPM)
 #include <pios_ppm_priv.h>
 
-void TIM4_IRQHandler();
-void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler")));
 const struct pios_ppm_cfg pios_ppm_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,	/* For 1 uS accuracy */
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = 0xFFFF, /* shared timer, make sure init correctly in outputs */
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_ic_init = {
-		.TIM_Channel = TIM_Channel_1,
 		.TIM_ICPolarity = TIM_ICPolarity_Rising,
 		.TIM_ICSelection = TIM_ICSelection_DirectTI,
 		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
 		.TIM_ICFilter = 0x0,
 	},
-	.gpio_init = {
+	/* Use only the first channel for ppm */
-		.GPIO_Pin = GPIO_Pin_6,
+	.channels = &pios_tim_rcvrport_all_channels[0],
-		.GPIO_Mode = GPIO_Mode_IPD,
+	.num_channels = 1,
-		.GPIO_Speed = GPIO_Speed_2MHz,
-	},
-	.remap = 0,
-	.irq = {
-		.init    = {
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-	.timer = TIM4,
-	.port = GPIOB,
-	.ccr = TIM_IT_CC1,
 };
 
-void PIOS_TIM4_irq_handler()
-{
-	PIOS_PPM_irq_handler();
-}
 #endif	/* PIOS_INCLUDE_PPM */
 
 /* 
@@ -687,98 +821,16 @@ void PIOS_TIM4_irq_handler()
 #if defined(PIOS_INCLUDE_PWM)
 #include <pios_pwm_priv.h>
 
-static const struct pios_pwm_channel pios_pwm_channels[] = {
-	{
-		.timer = TIM4,
-		.port = GPIOB,
-		.ccr = TIM_IT_CC1,
-		.channel = TIM_Channel_1,
-		.pin = GPIO_Pin_6,
-	}, 
-	{
-		.timer = TIM3,
-		.port = GPIOB,
-		.ccr = TIM_IT_CC2,
-		.channel = TIM_Channel_2,
-		.pin = GPIO_Pin_5,
-	}, 
-	{
-		.timer = TIM3,
-		.port = GPIOB,
-		.ccr = TIM_IT_CC3,
-		.channel = TIM_Channel_3,
-		.pin = GPIO_Pin_0
-	}, 
-	{
-		.timer = TIM3,
-		.port = GPIOB,
-		.ccr = TIM_IT_CC4,
-		.channel = TIM_Channel_4,
-		.pin = GPIO_Pin_1,
-	}, 
-	{ 
-		.timer = TIM2,
-		.port = GPIOA,
-		.ccr = TIM_IT_CC1,
-		.channel = TIM_Channel_1,
-		.pin = GPIO_Pin_0,
-	},  	
-	{
-		.timer = TIM2,
-		.port = GPIOA,
-		.ccr = TIM_IT_CC2,
-		.channel = TIM_Channel_2,
-		.pin = GPIO_Pin_1,
-	}, 		
-};
-
-void TIM2_IRQHandler();
-void TIM3_IRQHandler();
-void TIM4_IRQHandler();
-void TIM2_IRQHandler() __attribute__ ((alias ("PIOS_TIM2_irq_handler")));
-void TIM3_IRQHandler() __attribute__ ((alias ("PIOS_TIM3_irq_handler")));
-void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler")));
 const struct pios_pwm_cfg pios_pwm_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = 0xFFFF,
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_ic_init = {
 		.TIM_ICPolarity = TIM_ICPolarity_Rising,
 		.TIM_ICSelection = TIM_ICSelection_DirectTI,
 		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
-		.TIM_ICFilter = 0x0,		
+		.TIM_ICFilter = 0x0,
 	},
-	.gpio_init = {
+	.channels = pios_tim_rcvrport_all_channels,
-		.GPIO_Mode = GPIO_Mode_IPD,
+	.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
-		.GPIO_Speed = GPIO_Speed_2MHz,
-	},
-	.remap = 0,
-	.irq = {
-		.init    = {
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-	.channels = pios_pwm_channels,
-	.num_channels = NELEMENTS(pios_pwm_channels),
 };
-void PIOS_TIM2_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM2);
-}
-void PIOS_TIM3_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM3);
-}
-void PIOS_TIM4_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM4);
-}
 #endif
 
 #if defined(PIOS_INCLUDE_I2C)
@@ -856,11 +908,19 @@ void PIOS_I2C_main_adapter_er_irq_handler(void)
 
 #endif /* PIOS_INCLUDE_I2C */
 
+#if defined(PIOS_INCLUDE_GCSRCVR)
+#include "pios_gcsrcvr_priv.h"
+#endif	/* PIOS_INCLUDE_GCSRCVR */
+
 #if defined(PIOS_INCLUDE_RCVR)
 #include "pios_rcvr_priv.h"
 
-struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
+/* One slot per selectable receiver group.
-uint32_t pios_rcvr_max_channel;
+ *  eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS
+ * NOTE: No slot in this map for NONE.
+ */
+uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
+
 #endif /* PIOS_INCLUDE_RCVR */
 
 #if defined(PIOS_INCLUDE_USB_HID)
@@ -907,8 +967,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
-	UAVObjectsInitializeAll();
 
+	HwSettingsInitialize();
+	
 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
 	PIOS_RTC_Init(&pios_rtc_main_cfg);
@@ -920,7 +981,15 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Set up pulse timers */
+	PIOS_TIM_InitClock(&tim_1_cfg);
+	PIOS_TIM_InitClock(&tim_2_cfg);
+	PIOS_TIM_InitClock(&tim_3_cfg);
+	PIOS_TIM_InitClock(&tim_4_cfg);
+
 	/* Configure the main IO port */
+	uint8_t hwsettings_DSMxBind;
+	HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
 	uint8_t hwsettings_cc_mainport;
 	HwSettingsCC_MainPortGet(&hwsettings_cc_mainport);
 
@@ -959,6 +1028,13 @@ void PIOS_Board_Init(void) {
 			if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
 				PIOS_Assert(0);
 			}
+
+			uint32_t pios_sbus_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id;
+
 		}
 #endif	/* PIOS_INCLUDE_SBUS */
 		break;
@@ -980,7 +1056,8 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_GPS */
 		break;
-	case HWSETTINGS_CC_MAINPORT_SPEKTRUM:
+	case HWSETTINGS_CC_MAINPORT_SPEKTRUM1:
+	case HWSETTINGS_CC_MAINPORT_SPEKTRUM2:
 #if defined(PIOS_INCLUDE_SPEKTRUM)
 		{
 			uint32_t pios_usart_spektrum_id;
@@ -989,9 +1066,19 @@ void PIOS_Board_Init(void) {
 			}
 
 			uint32_t pios_spektrum_id;
-			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
+			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, 0)) {
 				PIOS_Assert(0);
 			}
+
+			uint32_t pios_spektrum_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) {
+				PIOS_Assert(0);
+			}
+			if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM1) {
+				pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id;
+			} else {
+				pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id;
+			}
 		}
 #endif	/* PIOS_INCLUDE_SPEKTRUM */
 		break;
@@ -1042,7 +1129,8 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_GPS */
 		break;
-	case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM:
+	case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1:
+	case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM2:
 #if defined(PIOS_INCLUDE_SPEKTRUM)
 		{
 			uint32_t pios_usart_spektrum_id;
@@ -1051,9 +1139,19 @@ void PIOS_Board_Init(void) {
 			}
 
 			uint32_t pios_spektrum_id;
-			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
+			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, hwsettings_DSMxBind)) {
 				PIOS_Assert(0);
 			}
+
+			uint32_t pios_spektrum_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) {
+				PIOS_Assert(0);
+			}
+			if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1) {
+				pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id;
+			} else {
+				pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id;
+			}
 		}
 #endif	/* PIOS_INCLUDE_SPEKTRUM */
 		break;
@@ -1070,83 +1168,61 @@ void PIOS_Board_Init(void) {
 		break;
 	}
 
-	/* Configure the selected receiver */
+	/* Configure the rcvr port */
-	uint8_t manualcontrolsettings_inputmode;
+	uint8_t hwsettings_rcvrport;
-	ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode);
+	HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport);
 
-	switch (manualcontrolsettings_inputmode) {
+	switch (hwsettings_rcvrport) {
-	case MANUALCONTROLSETTINGS_INPUTMODE_PWM:
+	case HWSETTINGS_CC_RCVRPORT_DISABLED:
+		break;
+	case HWSETTINGS_CC_RCVRPORT_PWM:
 #if defined(PIOS_INCLUDE_PWM)
-		PIOS_PWM_Init();
+		{
-		uint32_t pios_pwm_rcvr_id;
+			uint32_t pios_pwm_id;
-		if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) {
+			PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
-			PIOS_Assert(0);
+
-		}
+			uint32_t pios_pwm_rcvr_id;
-		for (uint8_t i = 0;
+			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
-		     i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
+				PIOS_Assert(0);
-		     i++) {
+			}
-			pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id;
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
-			pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-			pios_rcvr_max_channel++;
 		}
 #endif	/* PIOS_INCLUDE_PWM */
 		break;
-	case MANUALCONTROLSETTINGS_INPUTMODE_PPM:
+	case HWSETTINGS_CC_RCVRPORT_PPM:
 #if defined(PIOS_INCLUDE_PPM)
-		PIOS_PPM_Init();
+		{
-		uint32_t pios_ppm_rcvr_id;
+			uint32_t pios_ppm_id;
-		if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) {
+			PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
-			PIOS_Assert(0);
+
-		}
+			uint32_t pios_ppm_rcvr_id;
-		for (uint8_t i = 0;
+			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-		     i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
+				PIOS_Assert(0);
-		     i++) {
+			}
-			pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id;
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
-			pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-			pios_rcvr_max_channel++;
 		}
 #endif	/* PIOS_INCLUDE_PPM */
 		break;
-	case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM:
-#if defined(PIOS_INCLUDE_SPEKTRUM)
-		if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM ||
-		    hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) {
-			uint32_t pios_spektrum_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) {
-				PIOS_Assert(0);
-			}
-			for (uint8_t i = 0;
-			     i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
-			     i++) {
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id;
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-				pios_rcvr_max_channel++;
-			}
-		}
-#endif	/* PIOS_INCLUDE_SPEKTRUM */
-		break;
-	case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
-#if defined(PIOS_INCLUDE_SBUS)
-		if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) {
-			uint32_t pios_sbus_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, 0)) {
-				PIOS_Assert(0);
-			}
-			for (uint8_t i = 0;
-			     i < SBUS_NUMBER_OF_CHANNELS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
-			     i++) {
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_sbus_rcvr_id;
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-				pios_rcvr_max_channel++;
-			}
-		}
-#endif  /* PIOS_INCLUDE_SBUS */
-		break;
 	}
 
+#if defined(PIOS_INCLUDE_GCSRCVR)
+	GCSReceiverInitialize();
+	PIOS_GCSRCVR_Init();
+	uint32_t pios_gcsrcvr_rcvr_id;
+	if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) {
+		PIOS_Assert(0);
+	}
+	pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
+#endif	/* PIOS_INCLUDE_GCSRCVR */
+
 	/* Remap AFIO pin */
 	GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
-	PIOS_Servo_Init();
+
+#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
+	PIOS_Servo_Init(&pios_servo_cfg);
+#else
+	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
+#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
 	
 	PIOS_ADC_Init();
 	PIOS_GPIO_Init();

flight/CopterControl/System/pios_board_posix.c

@@ -42,7 +42,6 @@ void PIOS_Board_Init(void) {
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
-	UAVObjectsInitializeAll();
 
 	/* Initialize the alarms library */
 	AlarmsInitialize();

flight/CopterControl/System/taskmonitor.c

@@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void)
 {
 	lock = xSemaphoreCreateRecursiveMutex();
 	memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
+	lastMonitorTime = 0;
+#if defined(DIAGNOSTICS)
 	lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
+#endif
 	return 0;
 }
 
@@ -73,6 +76,7 @@ int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
  */
 void TaskMonitorUpdateAll(void)
 {
+#if defined(DIAGNOSTICS)
 	TaskInfoData data;
 	int n;
 	
@@ -123,4 +127,5 @@ void TaskMonitorUpdateAll(void)
 	
 	// Done
 	xSemaphoreGiveRecursive(lock);
+#endif
 }

flight/INS/Makefile

@@ -363,7 +363,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf: $(OUTDIR)/$(TARGET).elf

flight/Libraries/ahrs_comm_objects.c

@@ -24,6 +24,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include "pios.h"
 #include "ahrs_spi_comm.h"
 #include "pios_debug.h"
 
@@ -79,6 +80,7 @@ CREATEHANDLE(10, FirmwareIAPObj);
 static void ObjectUpdatedCb(UAVObjEvent * ev);
 
 #define ADDHANDLE(idx,obj) {\
+		obj##Initialize();\
 		int n = idx;\
 		objectHandles[n].data = &obj;\
 		objectHandles[n].uavHandle = obj##Handle();\

flight/Modules/AHRSComms/ahrs_comms.c

@@ -52,6 +52,8 @@
 
 #include "ahrs_comms.h"
 #include "ahrs_spi_comm.h"
+#include "ahrsstatus.h"
+#include "ahrscalibration.h"
 
 // Private constants
 #define STACK_SIZE configMINIMAL_STACK_SIZE-128
@@ -71,7 +73,7 @@ static void ahrscommsTask(void *parameters);
  */
 int32_t AHRSCommsStart(void)
 {
-	// Start main task
+	// Start main task	
 	xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
 	TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle);
 	PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS);
@@ -85,6 +87,11 @@ int32_t AHRSCommsStart(void)
  */
 int32_t AHRSCommsInitialize(void)
 {
+	AhrsStatusInitialize();
+	AHRSCalibrationInitialize();
+	AttitudeRawInitialize();
+	VelocityActualInitialize();
+	PositionActualInitialize();
 
 	return 0;
 }

flight/Modules/Actuator/actuator.c

@@ -41,7 +41,8 @@
 #include "flightstatus.h"
 #include "mixersettings.h"
 #include "mixerstatus.h"
-
+#include "cameradesired.h"
+#include "manualcontrolcommand.h"
 
 // Private constants
 #define MAX_QUEUE_SIZE 2
@@ -73,7 +74,7 @@ static void actuatorTask(void* parameters);
 static void actuator_update_rate(UAVObjEvent *);
 static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
 static void setFailsafe();
-static float MixerCurve(const float throttle, const float* curve);
+static float MixerCurve(const float throttle, const float* curve, uint8_t elements);
 static bool set_channel(uint8_t mixer_channel, uint16_t value);
 float ProcessMixer(const int index, const float curve1, const float curve2,
 		   MixerSettingsData* mixerSettings, ActuatorDesiredData* desired,
@@ -108,9 +109,17 @@ int32_t ActuatorInitialize()
 	// Create object queue
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 
+	ActuatorSettingsInitialize();
+	ActuatorDesiredInitialize();
+	MixerSettingsInitialize();
+	ActuatorCommandInitialize();
+#if defined(DIAGNOSTICS)
+	MixerStatusInitialize();
+#endif
+
 	// Listen for ExampleObject1 updates
 	ActuatorDesiredConnectQueue(queue);
-	
+
 	// If settings change, update the output rate
 	ActuatorSettingsConnectCallback(actuator_update_rate);
 
@@ -160,7 +169,7 @@ static void actuatorTask(void* parameters)
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	while (1)
-	{		
+	{
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
 
 		// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
@@ -177,11 +186,13 @@ static void actuatorTask(void* parameters)
 		lastSysTime = thisSysTime;
 
 		FlightStatusGet(&flightStatus);
-		MixerStatusGet(&mixerStatus);
 		MixerSettingsGet (&mixerSettings);
 		ActuatorDesiredGet(&desired);
 		ActuatorCommandGet(&command);
 
+#if defined(DIAGNOSTICS)
+		MixerStatusGet(&mixerStatus);
+#endif
 		ActuatorSettingsMotorsSpinWhileArmedGet(&MotorsSpinWhileArmed);
 		ActuatorSettingsChannelMaxGet(ChannelMax);
 		ActuatorSettingsChannelMinGet(ChannelMin);
@@ -196,7 +207,7 @@ static void actuatorTask(void* parameters)
 				nMixers ++;
 			}
 		}
-		if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers. 
+		if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
 		{
 			setFailsafe(); // So that channels like PWM buzzer keep working
 			continue;
@@ -207,23 +218,30 @@ static void actuatorTask(void* parameters)
 		bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
 		bool positiveThrottle = desired.Throttle >= 0.00;
 		bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
+
+		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
 		
-		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
 		//The source for the secondary curve is selectable
 		float curve2 = 0;
 		AccessoryDesiredData accessory;
 		switch(mixerSettings.Curve2Source) {
 			case MIXERSETTINGS_CURVE2SOURCE_THROTTLE:
-				curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2);
+				curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
 				break;
 			case MIXERSETTINGS_CURVE2SOURCE_ROLL:
-				curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2);
+				curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
 				break;
 			case MIXERSETTINGS_CURVE2SOURCE_PITCH:
-				curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2);
+				curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2,
+				MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
 				break;
 			case MIXERSETTINGS_CURVE2SOURCE_YAW:
-				curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2);
+				curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
+				break;
+			case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE:
+				ManualControlCommandCollectiveGet(&curve2);
+				curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2,
+				MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
 				break;
 			case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0:
 			case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1:
@@ -232,12 +250,12 @@ static void actuatorTask(void* parameters)
 			case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4:
 			case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
 				if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0)
-					curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2);
+					curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM);
-				else 
+				else
-					curve2 = 0;				
+					curve2 = 0;
 				break;
 		}
-		
+
 		for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
 		{
 			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
@@ -246,53 +264,82 @@ static void actuatorTask(void* parameters)
 				command.Channel[ct] = 0;
 				continue;
 			}
-			
+
-			status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
+			if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
-			
+				status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
+			else
+				status[ct] = -1;
+
+
+
 			// Motors have additional protection for when to be on
-			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {					
+			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
 
 				// If not armed or motors aren't meant to spin all the time
 				if( !armed ||
 				   (!spinWhileArmed && !positiveThrottle))
 				{
 					filterAccumulator[ct] = 0;
-					lastResult[ct] = 0;					
+					lastResult[ct] = 0;
 					status[ct] = -1;  //force min throttle
-				} 
+				}
-				// If armed meant to keep spinning, 
+				// If armed meant to keep spinning,
 				else if ((spinWhileArmed && !positiveThrottle) ||
 					 (status[ct] < 0) )
-					status[ct] = 0;					
+					status[ct] = 0;
 			}
-			
+
 			// If an accessory channel is selected for direct bypass mode
 			// In this configuration the accessory channel is scaled and mapped
 			// directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
-			// these also will not be updated in failsafe mode.  I'm not sure what 
+			// these also will not be updated in failsafe mode.  I'm not sure what
 			// the correct behavior is since it seems domain specific.  I don't love
 			// this code
-			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && 
+			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
-			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY2))
+			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5))
 			{
 				if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0)
 					status[ct] = accessory.AccessoryVal;
 				else
 					status[ct] = -1;
 			}
-			
+			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) &&
+			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW))
+			{
+				CameraDesiredData cameraDesired;
+				if( CameraDesiredGet(&cameraDesired) == 0 ) {
+					switch(mixers[ct].type) {
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAROLL:
+							status[ct] = cameraDesired.Roll;
+							break;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH:
+							status[ct] = cameraDesired.Pitch;
+							break;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
+							status[ct] = cameraDesired.Yaw;
+							break;
+						default:
+							break;
+					}
+				}
+				else
+					status[ct] = -1;
+			}
+
 			command.Channel[ct] = scaleChannel(status[ct],
 							   ChannelMax[ct],
 							   ChannelMin[ct],
 							   ChannelNeutral[ct]);
 		}
+#if defined(DIAGNOSTICS)
 		MixerStatusSet(&mixerStatus);
+#endif
 
 		// Store update time
 		command.UpdateTime = 1000*dT;
 		if(1000*dT > command.MaxUpdateTime)
 			command.MaxUpdateTime = 1000*dT;
-		
+
 		// Update output object
 		ActuatorCommandSet(&command);
 		// Update in case read only (eg. during servo configuration)
@@ -300,7 +347,7 @@ static void actuatorTask(void* parameters)
 
 		// Update servo outputs
 		bool success = true;
-		
+
 		for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 		{
 			success &= set_channel(n, command.Channel[n]);
@@ -309,7 +356,7 @@ static void actuatorTask(void* parameters)
 		if(!success) {
 			command.NumFailedUpdates++;
 			ActuatorCommandSet(&command);
-			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); 
+			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
 		}
 
 	}
@@ -383,12 +430,9 @@ float ProcessMixer(const int index, const float curve1, const float curve2,
  *Interpolate a throttle curve. Throttle input should be in the range 0 to 1.
  *Output is in the range 0 to 1.
  */
-
+static float MixerCurve(const float throttle, const float* curve, uint8_t elements)
-#define MIXER_CURVE_ENTRIES 5
-
-static float MixerCurve(const float throttle, const float* curve)
 {
-	float scale = throttle * MIXER_CURVE_ENTRIES;
+	float scale = throttle * (elements - 1);
 	int idx1 = scale;
 	scale -= (float)idx1; //remainder
 	if(curve[0] < -1)
@@ -401,12 +445,12 @@ static float MixerCurve(const float throttle, const float* curve)
 		scale = 0;
 	}
 	int idx2 = idx1 + 1;
-	if(idx2 >= MIXER_CURVE_ENTRIES)
+	if(idx2 >= elements)
 	{
-		idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table
+		idx2 = elements -1; //clamp to highest entry in table
-		if(idx1 >= MIXER_CURVE_ENTRIES)
+		if(idx1 >= elements)
 		{
-			idx1 = MIXER_CURVE_ENTRIES -1;
+			idx1 = elements -1;
 		}
 	}
 	return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale));
@@ -463,7 +507,7 @@ static void setFailsafe()
 	// Reset ActuatorCommand to safe values
 	for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 	{
-		
+
 		if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
 		{
 			Channel[n] = ChannelMin[n];
@@ -510,10 +554,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 }
 #else
 static bool set_channel(uint8_t mixer_channel, uint16_t value) {
-	
+
 	ActuatorSettingsData settings;
 	ActuatorSettingsGet(&settings);
-	
+
 	switch(settings.ChannelType[mixer_channel]) {
 		case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: {
 			// This is for buzzers that take a PWM input
@@ -565,7 +609,7 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 			PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value);
 			return true;
 #if defined(PIOS_INCLUDE_I2C_ESC)
-		case ACTUATORSETTINGS_CHANNELTYPE_MK: 
+		case ACTUATORSETTINGS_CHANNELTYPE_MK:
 			return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);
 		case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
 			return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value);
@@ -573,10 +617,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 #endif
 		default:
 			return false;
-	}			
+	}
-	
+
 	return false;
-	
+
 }
 #endif
 

flight/Modules/Altitude/altitude.c

@@ -69,6 +69,12 @@ static void altitudeTask(void *parameters);
  */
 int32_t AltitudeStart()
 {
+	
+	BaroAltitudeInitialize();
+#if defined(PIOS_INCLUDE_HCSR04)
+	SonarAltitudeInitialze();
+#endif
+	
 	// Start main task
 	xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
 	TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);

flight/Modules/Attitude/attitude.c

@@ -112,6 +112,10 @@ int32_t AttitudeStart(void)
  */
 int32_t AttitudeInitialize(void)
 {
+	AttitudeActualInitialize();
+	AttitudeRawInitialize();
+	AttitudeSettingsInitialize();
+
 	// Initialize quaternion
 	AttitudeActualData attitude;
 	AttitudeActualGet(&attitude);

flight/Modules/Battery/battery.c

@@ -79,6 +79,9 @@ MODULE_INITCALL(BatteryInitialize, 0)
 
 int32_t BatteryInitialize(void)
 {
+	BatteryStateInitialze();
+	BatterySettingsInitialize();
+	
 	static UAVObjEvent ev;
 
 	memset(&ev,0,sizeof(UAVObjEvent));

flight/Modules/CameraStab/camerastab.c

@@ -0,0 +1,141 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup CameraStab Camera Stabilization Module
+ * @brief Camera stabilization module
+ * Updates accessory outputs with values appropriate for camera stabilization
+ * @{
+ *
+ * @file       camerastab.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Stabilize camera against the roll pitch and yaw of aircraft
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: Accessory
+ *
+ * This module will periodically calculate the output values for stabilizing the camera
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+
+#include "accessorydesired.h"
+#include "attitudeactual.h"
+#include "camerastabsettings.h"
+#include "cameradesired.h"
+
+//
+// Configuration
+//
+#define SAMPLE_PERIOD_MS		10
+
+// Private types
+
+// Private variables
+
+// Private functions
+static void attitudeUpdated(UAVObjEvent* ev);
+static float bound(float val);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t CameraStabInitialize(void)
+{
+	static UAVObjEvent ev;
+	ev.obj = AttitudeActualHandle();
+	ev.instId = 0;
+	ev.event = 0;
+
+	CameraStabSettingsInitialize();
+	CameraDesiredInitialize();
+
+	EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
+
+	return 0;
+}
+
+static void attitudeUpdated(UAVObjEvent* ev)
+{
+	if (ev->obj != AttitudeActualHandle())
+		return;
+
+	float attitude;
+	float output;
+	AccessoryDesiredData accessory;
+
+	CameraStabSettingsData cameraStab;
+	CameraStabSettingsGet(&cameraStab);
+
+	// Read any input channels
+	float inputs[3] = {0,0,0};
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL];
+	}
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH];
+	}
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW];
+	}
+
+	// Set output channels
+	AttitudeActualRollGet(&attitude);
+	output = bound((attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]);
+	CameraDesiredRollSet(&output);
+
+	AttitudeActualPitchGet(&attitude);
+	output = bound((attitude + inputs[1])  / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]);
+	CameraDesiredPitchSet(&output);
+
+	AttitudeActualYawGet(&attitude);
+	output = bound((attitude + inputs[2])  / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]);
+	CameraDesiredYawSet(&output);
+
+}
+
+float bound(float val)
+{
+	return (val > 1) ? 1 : 
+		(val < -1) ? -1 :
+		val;
+}
+/**
+  * @}
+  */
+
+/**
+ * @}
+ */

flight/Modules/CameraStab/inc/camerastab.h

@@ -1,14 +1,14 @@
 /**
  ******************************************************************************
  * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
+ * @{ 
- * @addtogroup GSPModule GPS Module
+ * @addtogroup BatteryModule Battery Module
- * @brief Process GPS information
+ * @{ 
- * @{
  *
- * @file       GTOP_BIN.h
+ * @file       battery.h
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      GPS module, handles GPS and NMEA stream
+ * @brief      Module to read the battery Voltage and Current periodically and set alarms appropriately.
+ *
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -27,16 +27,16 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
+#ifndef BATTERY_H
+#define BATTERY_H
 
-#ifndef GTOP_BIN_H
+#include "openpilot.h"
-#define GTOP_BIN_H
 
-#include <stdint.h>
+int32_t CameraStabInitialize(void);
-#include "gps_mode.h"
 
-#ifdef ENABLE_GPS_BINARY_GTOP
+#endif // BATTERY_H
-	extern int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors);
-	extern void GTOP_BIN_init(void);
-#endif
 
-#endif
+/**
+  * @}
+  * @}
+  */

flight/Modules/FirmwareIAP/firmwareiap.c

@@ -91,6 +91,9 @@ static void resetTask(UAVObjEvent *);
 MODULE_INITCALL(FirmwareIAPInitialize, 0)
 int32_t FirmwareIAPInitialize()
 {
+	
+	FirmwareIAPObjInitialize();
+	
 	const struct pios_board_info * bdinfo = &pios_board_info_blob;
 
 	data.BoardType= bdinfo->board_type;

flight/Modules/FlightPlan/flightplan.c

@@ -74,7 +74,11 @@ int32_t FlightPlanStart()
 int32_t FlightPlanInitialize()
 {
 	taskHandle = NULL;
-
+	
+	FlightPlanStatusInitialize();
+	FlightPlanControlInitialize();
+	FlightPlanSettingsInitialize();
+	
 	// Listen for object updates
 	FlightPlanControlConnectCallback(&objectUpdatedCb);
 

flight/Modules/GPS/GPS.c

@@ -35,17 +35,12 @@
 
 #include <stdbool.h>
 
-#ifdef ENABLE_GPS_BINARY_GTOP
+#include "NMEA.h"
-	#include "GTOP_BIN.h"
-#endif
-
-#if defined(ENABLE_GPS_ONESENTENCE_GTOP) || defined(ENABLE_GPS_NMEA)
-	#include "NMEA.h"
-#endif
 
 #include "gpsposition.h"
 #include "homelocation.h"
 #include "gpstime.h"
+#include "gpssatellites.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
 
@@ -62,25 +57,16 @@ static float GravityAccel(float latitude, float longitude, float altitude);
 // ****************
 // Private constants
 
-//#define FULL_COLD_RESTART             // uncomment this to tell the GPS to do a FULL COLD restart
-//#define DISABLE_GPS_THRESHOLD          //
-
 #define GPS_TIMEOUT_MS                  500
-#define GPS_COMMAND_RESEND_TIMEOUT_MS   2000
+#define NMEA_MAX_PACKET_LENGTH          96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
+// same as in COM buffer
+
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
-	#ifdef ENABLE_GPS_BINARY_GTOP
+	#define STACK_SIZE_BYTES            800
-		#define STACK_SIZE_BYTES            800
-	#else
-		#define STACK_SIZE_BYTES            800
-	#endif
 #else
-	#ifdef ENABLE_GPS_BINARY_GTOP
+	#define STACK_SIZE_BYTES            650
-		#define STACK_SIZE_BYTES            440
-	#else
-		#define STACK_SIZE_BYTES            440
-	#endif
 #endif
 
 #define TASK_PRIORITY                   (tskIDLE_PRIORITY + 1)
@@ -92,9 +78,7 @@ static uint32_t gpsPort;
 
 static xTaskHandle gpsTaskHandle;
 
-#ifndef ENABLE_GPS_BINARY_GTOP
+static char* gps_rx_buffer;
-	static char gps_rx_buffer[128];
-#endif
 
 static uint32_t timeOfLastCommandMs;
 static uint32_t timeOfLastUpdateMs;
@@ -124,9 +108,19 @@ int32_t GPSStart(void)
  */
 int32_t GPSInitialize(void)
 {
+	GPSPositionInitialize();
+	GPSTimeInitialize();
+	GPSSatellitesInitialize();
+#ifdef PIOS_GPS_SETS_HOMELOCATION
+	HomeLocationInitialize();
+#endif
+	
 	// TODO: Get gps settings object
 	gpsPort = PIOS_COM_GPS;
 
+	gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+	PIOS_Assert(gps_rx_buffer);
+
 	return 0;
 }
 MODULE_INITCALL(GPSInitialize, GPSStart)
@@ -142,45 +136,11 @@ static void gpsTask(void *parameters)
 	uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
 	GPSPositionData GpsData;
 	
-#ifdef ENABLE_GPS_BINARY_GTOP
-	GTOP_BIN_init();
-#else
 	uint8_t rx_count = 0;
 	bool start_flag = false;
 	bool found_cr = false;
 	int32_t gpsRxOverflow = 0;
-#endif
 	
-#ifdef FULL_COLD_RESTART
-	// tell the GPS to do a FULL COLD restart
-	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n");
-	timeOfLastCommandMs = timeNowMs;
-	while (timeNowMs - timeOfLastCommandMs < 300)	// delay for 300ms to let the GPS sort itself out
-	{
-		vTaskDelay(xDelay);	// Block task until next update
-		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
-	}
-#endif
-
-#ifdef DISABLE_GPS_THRESHOLD
-	PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n");
-#endif
-
-#ifdef ENABLE_GPS_BINARY_GTOP
-	// switch to GTOP binary mode
-	PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n");
-#endif
-	
-#ifdef ENABLE_GPS_ONESENTENCE_GTOP
-	// switch to single sentence mode
-	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n");
-#endif
-
-#ifdef ENABLE_GPS_NMEA
-	// switch to NMEA mode
-	PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n");
-#endif
-
 	numUpdates = 0;
 	numChecksumErrors = 0;
 	numParsingErrors = 0;
@@ -191,108 +151,87 @@ static void gpsTask(void *parameters)
 	// Loop forever
 	while (1)
 	{
-		#ifdef ENABLE_GPS_BINARY_GTOP
+		uint8_t c;
-			// GTOP BINARY GPS mode
+		// NMEA or SINGLE-SENTENCE GPS mode
 
-			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
+		// This blocks the task until there is something on the buffer
+		while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
+		{
+		
+			// detect start while acquiring stream
+			if (!start_flag && (c == '$'))
 			{
-				uint8_t c;
+				start_flag = true;
-				PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0);
+				found_cr = false;
+				rx_count = 0;
+			}
+			else
+			if (!start_flag)
+				continue;
+		
+			if (rx_count >= NMEA_MAX_PACKET_LENGTH)
+			{
+				// The buffer is already full and we haven't found a valid NMEA sentence.
+				// Flush the buffer and note the overflow event.
+				gpsRxOverflow++;
+				start_flag = false;
+				found_cr = false;
+				rx_count = 0;
+			}
+			else
+			{
+				gps_rx_buffer[rx_count] = c;
+				rx_count++;
+			}
+		
+			// look for ending '\r\n' sequence
+			if (!found_cr && (c == '\r') )
+				found_cr = true;
+			else
+			if (found_cr && (c != '\n') )
+				found_cr = false;  // false end flag
+			else
+			if (found_cr && (c == '\n') )
+			{
+				// The NMEA functions require a zero-terminated string
+				// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
+				gps_rx_buffer[rx_count-2] = 0;
 
-				if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0)
+				// prepare to parse next sentence
-				{
+				start_flag = false;
-					numUpdates++;
+				found_cr = false;
+				rx_count = 0;
+				// Our rxBuffer must look like this now:
+				//   [0]           = '$'
+				//   ...           = zero or more bytes of sentence payload
+				//   [end_pos - 1] = '\r'
+				//   [end_pos]     = '\n'
+				//
+				// Prepare to consume the sentence from the buffer
+			
+				// Validate the checksum over the sentence
+				if (!NMEA_checksum(&gps_rx_buffer[1]))
+				{	// Invalid checksum.  May indicate dropped characters on Rx.
+					//PIOS_DEBUG_PinHigh(2);
+					++numChecksumErrors;
+					//PIOS_DEBUG_PinLow(2);
+				}
+				else
+				{	// Valid checksum, use this packet to update the GPS position
+					if (!NMEA_update_position(&gps_rx_buffer[1])) {
+						//PIOS_DEBUG_PinHigh(2);
+						++numParsingErrors;
+						//PIOS_DEBUG_PinLow(2);
+					}
+					else
+						++numUpdates;
 
 					timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
 					timeOfLastUpdateMs = timeNowMs;
 					timeOfLastCommandMs = timeNowMs;
 				}
 			}
-
+		}
-		#else
-			// NMEA or SINGLE-SENTENCE GPS mode
-
-			// This blocks the task until there is something on the buffer
-			while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
-			{
-				uint8_t c;
-				PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0);
-			
-				// detect start while acquiring stream
-				if (!start_flag && (c == '$'))
-				{
-					start_flag = true;
-					found_cr = false;
-					rx_count = 0;
-				}
-				else
-				if (!start_flag)
-					continue;
-			
-				if (rx_count >= sizeof(gps_rx_buffer))
-				{
-					// The buffer is already full and we haven't found a valid NMEA sentence.
-					// Flush the buffer and note the overflow event.
-					gpsRxOverflow++;
-					start_flag = false;
-					found_cr = false;
-					rx_count = 0;
-				}
-				else
-				{
-					gps_rx_buffer[rx_count] = c;
-					rx_count++;
-				}
-			
-				// look for ending '\r\n' sequence
-				if (!found_cr && (c == '\r') )
-					found_cr = true;
-				else
-				if (found_cr && (c != '\n') )
-					found_cr = false;  // false end flag
-				else
-				if (found_cr && (c == '\n') )
-				{
-					// The NMEA functions require a zero-terminated string
-					// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
-					gps_rx_buffer[rx_count-2] = 0;
-
-					// prepare to parse next sentence
-					start_flag = false;
-					found_cr = false;
-					rx_count = 0;
-					// Our rxBuffer must look like this now:
-					//   [0]           = '$'
-					//   ...           = zero or more bytes of sentence payload
-					//   [end_pos - 1] = '\r'
-					//   [end_pos]     = '\n'
-					//
-					// Prepare to consume the sentence from the buffer
-				
-					// Validate the checksum over the sentence
-					if (!NMEA_checksum(&gps_rx_buffer[1]))
-					{	// Invalid checksum.  May indicate dropped characters on Rx.
-						//PIOS_DEBUG_PinHigh(2);
-						++numChecksumErrors;
-						//PIOS_DEBUG_PinLow(2);
-					}
-					else
-					{	// Valid checksum, use this packet to update the GPS position
-						if (!NMEA_update_position(&gps_rx_buffer[1])) {
-							//PIOS_DEBUG_PinHigh(2);
-							++numParsingErrors;
-							//PIOS_DEBUG_PinLow(2);
-						}
-						else
-							++numUpdates;
-
-						timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
-						timeOfLastUpdateMs = timeNowMs;
-						timeOfLastCommandMs = timeNowMs;
-					}
-				}
-			}
-		#endif
 
 		// Check for GPS timeout
 		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
@@ -305,30 +244,6 @@ static void gpsTask(void *parameters)
 			GPSPositionSet(&GpsData);
 			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
 
-			if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)
-			{	// resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command
-				timeOfLastCommandMs = timeNowMs;
-
-				#ifdef ENABLE_GPS_BINARY_GTOP
-					GTOP_BIN_init();
-					// switch to binary mode
-					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n");
-				#endif
-
-				#ifdef ENABLE_GPS_ONESENTENCE_GTOP
-					// switch to single sentence mode
-					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n");
-				#endif
-
-				#ifdef ENABLE_GPS_NMEA
-					// switch to NMEA mode
-					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n");
-				#endif
-
-				#ifdef DISABLE_GPS_TRESHOLD
-					PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n");
-				#endif
-			}
 		}
 		else
 		{	// we appear to be receiving GPS sentences OK, we've had an update
@@ -354,8 +269,6 @@ static void gpsTask(void *parameters)
 				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
 		}
 
-		// Block task until next update
-		vTaskDelay(xDelay);
 	}
 }
 

flight/Modules/GPS/GTOP_BIN.c

@@ -1,262 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup GSPModule GPS Module
- * @brief Process GPS information
- * @{
- *
- * @file       GTOP_BIN.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      GPS module, handles GPS and NMEA stream
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#include "openpilot.h"
-#include "pios.h"
-#include "GTOP_BIN.h"
-#include "gpsposition.h"
-#include "gpstime.h"
-#include "gpssatellites.h"
-
-#include <string.h>	// memmove
-
-#ifdef ENABLE_GPS_BINARY_GTOP
-
-// ************
-// the structure of the binary packet
-
-typedef struct
-{
-	uint32_t  utc_time;
-
-	int32_t   latitude;
-	uint8_t   ns_indicator;
-
-	int32_t   longitude;
-	uint8_t   ew_indicator;
-
-	uint8_t   fix_quality;
-
-	uint8_t   satellites_used;
-
-	uint16_t  hdop;
-
-	int32_t   msl_altitude;
-
-	int32_t   geoidal_seperation;
-
-	uint8_t   fix_type;
-
-	int32_t   course_over_ground;
-
-	int32_t   speed_over_ground;
-
-	uint8_t   day;
-	uint8_t   month;
-	uint16_t  year;
-}  __attribute__((__packed__)) t_gps_bin_packet_data;
-
-typedef struct
-{
-	uint16_t              header;
-	t_gps_bin_packet_data data;
-	uint8_t               asterisk;
-	uint8_t               checksum;
-	uint16_t              end_word;
-}  __attribute__((__packed__)) t_gps_bin_packet;
-
-// ************
-
-// buffer that holds the incoming binary packet
-static uint8_t gps_rx_buffer[sizeof(t_gps_bin_packet)] __attribute__ ((aligned(4)));
-
-// number of bytes currently in the rx buffer
-static int16_t gps_rx_buffer_wr = 0;
-
-// ************
-// endian swapping functions
-
-static uint16_t swap2Bytes(uint16_t data)
-{
-	return (((data >> 8) & 0x00ff) |
-			((data << 8) & 0xff00));
-}
-
-static uint32_t swap4Bytes(uint32_t data)
-{
-	return (((data >> 24) & 0x000000ff) |
-			((data >>  8) & 0x0000ff00) |
-			((data <<  8) & 0x00ff0000) |
-			((data << 24) & 0xff000000));
-}
-
-// ************
-/**
- * Parses a complete binary packet and update the GPSPosition and GPSTime UAVObjects
- *
- * param[in] .. b = a new received byte from the GPS
- *
- * return '0' if we have found a valid binary packet
- * return <0 if any errors were encountered with the packet or no packet found
- */
-
-int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors)
-{
-	if (gps_rx_buffer_wr >= sizeof(gps_rx_buffer))
-	{	// make room for the new byte .. this will actually never get executed, just here as a safe guard really
-		memmove(gps_rx_buffer, gps_rx_buffer + 1, sizeof(gps_rx_buffer) - 1);
-		gps_rx_buffer_wr = sizeof(gps_rx_buffer) - 1;
-	}
-
-	// add the new byte into the buffer
-	gps_rx_buffer[gps_rx_buffer_wr++] = b;
-
-	int16_t i = 0;
-
-	while (gps_rx_buffer_wr > 0)
-	{
-		t_gps_bin_packet *rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + i);
-
-		// scan for the start of a binary packet (the header bytes)
-		while (gps_rx_buffer_wr - i >= sizeof(rx_packet->header))
-		{
-			if (rx_packet->header == 0x2404)
-				break;   // found a valid header marker
-			rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + ++i);
-		}
-
-		// remove unwanted bytes before the start of the packet header
-		if (i > 0)
-		{
-			gps_rx_buffer_wr -= i;
-			if (gps_rx_buffer_wr > 0)
-				memmove(gps_rx_buffer, gps_rx_buffer + i, gps_rx_buffer_wr);
-			i = 0;
-		}
-
-		if (gps_rx_buffer_wr < sizeof(t_gps_bin_packet))
-			break;   // not yet enough bytes for a complete binary packet
-
-		// we have enough bytes for a complete binary packet
-
-		// check to see if certain parameters in the binary packet are valid
-		if (rx_packet->header != 0x2404 ||
-			rx_packet->end_word != 0x0A0D ||
-			rx_packet->asterisk != 0x2A ||
-			(rx_packet->data.ns_indicator != 1 && rx_packet->data.ns_indicator != 2) ||
-			(rx_packet->data.ew_indicator != 1 && rx_packet->data.ew_indicator != 2) ||
-			(rx_packet->data.fix_quality > 2) ||
-			(rx_packet->data.fix_type < 1 || rx_packet->data.fix_type > 3) )
-		{   // invalid packet
-			if (parsing_errors) *parsing_errors++;
-			i++;
-			continue;
-		}
-
-		{   // check the checksum is valid
-			uint8_t *p = (uint8_t *)&rx_packet->data;
-			uint8_t checksum = 0;
-			for (int i = 0; i < sizeof(t_gps_bin_packet_data); i++)
-				checksum ^= *p++;
-
-			if (checksum != rx_packet->checksum)
-			{	// checksum error
-				if (chksum_errors) *chksum_errors++;
-				i++;
-				continue;
-			}
-		}
-
-		// we now have a valid complete binary packet, update the GpsData and GpsTime objects
-
-		// correct the endian order of the parameters
-		rx_packet->data.utc_time = swap4Bytes(rx_packet->data.utc_time);
-		rx_packet->data.latitude = swap4Bytes(rx_packet->data.latitude);
-		rx_packet->data.longitude = swap4Bytes(rx_packet->data.longitude);
-		rx_packet->data.hdop = swap2Bytes(rx_packet->data.hdop);
-		rx_packet->data.msl_altitude = swap4Bytes(rx_packet->data.msl_altitude);
-		rx_packet->data.geoidal_seperation = swap4Bytes(rx_packet->data.geoidal_seperation);
-		rx_packet->data.course_over_ground = swap4Bytes(rx_packet->data.course_over_ground);
-		rx_packet->data.speed_over_ground = swap4Bytes(rx_packet->data.speed_over_ground);
-		rx_packet->data.year = swap2Bytes(rx_packet->data.year);
-
-		// set the gps time object
-		GPSTimeData GpsTime;
-//		GPSTimeGet(&GpsTime);
-			uint32_t utc_time = rx_packet->data.utc_time / 1000;
-			GpsTime.Second = utc_time % 100;          // seconds
-			GpsTime.Minute = (utc_time / 100) % 100;  // minutes
-			GpsTime.Hour = utc_time / 10000;          // hours
-			GpsTime.Day = rx_packet->data.day;        // day
-			GpsTime.Month = rx_packet->data.month;    // month
-			GpsTime.Year = rx_packet->data.year;      // year
-		GPSTimeSet(&GpsTime);
-
-		// set the gps position object
-		GPSPositionData	GpsData;
-//		GPSPositionGet(&GpsData);
-			switch (rx_packet->data.fix_type)
-			{
-				case 1: GpsData.Status = GPSPOSITION_STATUS_NOFIX; break;
-				case 2: GpsData.Status = GPSPOSITION_STATUS_FIX2D; break;
-				case 3: GpsData.Status = GPSPOSITION_STATUS_FIX3D; break;
-				default: GpsData.Status = GPSPOSITION_STATUS_NOGPS; break;
-			}
-			GpsData.Latitude        = rx_packet->data.latitude  * (rx_packet->data.ns_indicator == 1 ? +1 : -1) * 10;   // degrees * 10e6
-			GpsData.Longitude       = rx_packet->data.longitude * (rx_packet->data.ew_indicator == 1 ? +1 : -1) * 10;	// degrees * 10e6
-			GpsData.Altitude        = (float)rx_packet->data.msl_altitude / 1000;                                       // meters
-			GpsData.GeoidSeparation = (float)rx_packet->data.geoidal_seperation / 1000;                                 // meters
-			GpsData.Heading         = (float)rx_packet->data.course_over_ground / 1000;                                 // degrees
-			GpsData.Groundspeed     = (float)rx_packet->data.speed_over_ground / 3600;                                  // m/s
-			GpsData.Satellites      = rx_packet->data.satellites_used;                                                  //
-			GpsData.PDOP            = 99.99;                                                                            // not available in binary mode
-			GpsData.HDOP            = (float)rx_packet->data.hdop / 100;                                                //
-			GpsData.VDOP            = 99.99;                                                                            // not available in binary mode
-		GPSPositionSet(&GpsData);
-
-		// set the number of satellites
-//		GPSSatellitesData SattelliteData;
-////		GPSSatellitesGet(&SattelliteData);
-//			memset(&SattelliteData, 0, sizeof(SattelliteData));
-//			SattelliteData.SatsInView = rx_packet->data.satellites_used;                                                    //
-//		GPSSatellitesSet(&SattelliteData);
-
-		// remove the spent binary packet from the buffer
-		gps_rx_buffer_wr -= sizeof(t_gps_bin_packet);
-		if (gps_rx_buffer_wr > 0)
-			memmove(gps_rx_buffer, gps_rx_buffer + sizeof(t_gps_bin_packet), gps_rx_buffer_wr);
-
-		return 0;  // found a valid packet
-	}
-
-	return -1;     // no valid packet found
-}
-
-// ************
-
-void GTOP_BIN_init(void)
-{
-	gps_rx_buffer_wr = 0;
-}
-
-// ************
-
-#endif // ENABLE_GPS_BINARY_GTOP
-

flight/Modules/GPS/NMEA.c

@@ -40,8 +40,6 @@
 
 
 
-#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)
-
 // Debugging
 #ifdef ENABLE_DEBUG_MSG
 //#define DEBUG_MSG_IN			///< define to display the incoming NMEA messages
@@ -54,7 +52,6 @@
 //#define NMEA_DEBUG_GSA		///< define to enable debug of GSA messages
 //#define NMEA_DEBUG_GSV		///< define to enable debug of GSV messages
 //#define NMEA_DEBUG_ZDA		///< define to enable debug of ZDA messages
-//#define NMEA_DEBUG_PGTOP	///< define to enable debug of PGTOP messages
 #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
 #else
 #define DEBUG_MSG(format, ...)
@@ -69,56 +66,45 @@ struct nmea_parser {
 	uint32_t cnt;
 };
 
-	#ifdef ENABLE_GPS_NMEA
+static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
+static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
+static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
+static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
+static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
+static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-		static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
-	#endif
 
-	static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
 
-	static struct nmea_parser nmea_parsers[] = {
+static struct nmea_parser nmea_parsers[] = {
-
+	{
-	#ifdef ENABLE_GPS_NMEA
+		.prefix = "GPGGA",
-		{
+		.handler = nmeaProcessGPGGA,
-			.prefix = "GPGGA",
+		.cnt = 0,
-			.handler = nmeaProcessGPGGA,
+	},
-			.cnt = 0,
+	{
-		},
+		.prefix = "GPVTG",
-		{
+		.handler = nmeaProcessGPVTG,
-			.prefix = "GPVTG",
+		.cnt = 0,
-			.handler = nmeaProcessGPVTG,
+	},
-			.cnt = 0,
+	{
-		},
+		.prefix = "GPGSA",
-		{
+		.handler = nmeaProcessGPGSA,
-			.prefix = "GPGSA",
+		.cnt = 0,
-			.handler = nmeaProcessGPGSA,
+	},
-			.cnt = 0,
+	{
-		},
+		.prefix = "GPRMC",
-		{
+		.handler = nmeaProcessGPRMC,
-			.prefix = "GPRMC",
+		.cnt = 0,
-			.handler = nmeaProcessGPRMC,
+	},
-			.cnt = 0,
+	{
-		},
+		.prefix = "GPZDA",
-		{
+		.handler = nmeaProcessGPZDA,
-			.prefix = "GPZDA",
+		.cnt = 0,
-			.handler = nmeaProcessGPZDA,
+	},
-			.cnt = 0,
+	{
-		},
+		.prefix = "GPGSV",
-		{
+		.handler = nmeaProcessGPGSV,
-			.prefix = "GPGSV",
+		.cnt = 0,
-			.handler = nmeaProcessGPGSV,
+	},
-			.cnt = 0,
-		},
-	#endif
-        {
-         .prefix = "PGTOP",
-         .handler = nmeaProcessPGTOP,
-         .cnt = 0,
-         },
 };
 
 static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
@@ -229,7 +215,6 @@ static float NMEA_real_to_float(char *nmea_real)
 	return (((float)whole) + fract * pow(10, -fract_units));
 }
 
-#ifdef ENABLE_GPS_NMEA
 /*
  * Parse a field in the format:
  *    DD[D]MM.mmmm[mm]
@@ -287,7 +272,6 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool
 
 	return true;
 }
-#endif	// ENABLE_GPS_NMEA
 
 
 /**
@@ -376,7 +360,6 @@ bool NMEA_update_position(char *nmea_sentence)
 	return true;
 }
 
-#ifdef ENABLE_GPS_NMEA
 
 /**
  * Parse an NMEA GPGGA sentence and update the given UAVObject
@@ -675,83 +658,3 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	return true;
 }
 
-#endif	// ENABLE_GPS_NMEA
-
-/**
- * Parse an NMEA PGTOP sentence and update the given UAVObject
- * \param[in] A pointer to a GPSPosition UAVObject to be updated.
- * \param[in] An NMEA sentence with a valid checksum
- */
-static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam)
-{
-	if (nbParam != 17)
-		return false;
-
-	GPSTimeData gpst;
-	GPSTimeGet(&gpst);
-
-	*gpsDataUpdated = true;
-
-	// get UTC time [hhmmss.sss]
-	float hms = NMEA_real_to_float(param[1]);
-	gpst.Second = (int)hms % 100;
-	gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
-	gpst.Hour = (int)hms / 10000;
-
-	// get latitude decimal degrees
-	GpsData->Latitude = NMEA_real_to_float(param[2])*1e7;
-	if (param[3][0] == 'S')
-		GpsData->Latitude = -GpsData->Latitude;
-
-
-	// get longitude decimal degrees
-	GpsData->Longitude = NMEA_real_to_float(param[4])*1e7;
-	if (param[5][0] == 'W')
-		GpsData->Longitude = -GpsData->Longitude;
-
-	// get number of satellites used in GPS solution
-	GpsData->Satellites = atoi(param[7]);
-
-	// next field: HDOP
-	GpsData->HDOP = NMEA_real_to_float(param[8]);
-
-	// get altitude (in meters mm.m)
-	GpsData->Altitude = NMEA_real_to_float(param[9]);
-
-	// next field: geoid separation
-	GpsData->GeoidSeparation = NMEA_real_to_float(param[10]);
-
-	// Mode: 1=Fix not available, 2=2D, 3=3D
-	switch (atoi(param[11])) {
-	case 1:
-			GpsData->Status = GPSPOSITION_STATUS_NOFIX;
-			break;
-	case 2:
-			GpsData->Status = GPSPOSITION_STATUS_FIX2D;
-			break;
-	case 3:
-			GpsData->Status = GPSPOSITION_STATUS_FIX3D;
-			break;
-	default:
-			/* Unhandled */
-			return false;
-			break;
-	}
-
-	// get course over ground in degrees [ddd.dd]
-	GpsData->Heading = NMEA_real_to_float(param[12]);
-
-	// get speed in km/h
-	GpsData->Groundspeed = NMEA_real_to_float(param[13]);
-	// to m/s
-	GpsData->Groundspeed /= 3.6;
-
-	gpst.Day = atoi(param[14]);
-	gpst.Month = atoi(param[15]);
-	gpst.Year = atoi(param[16]);
-	GPSTimeSet(&gpst);
-
-	return true;
-}
-
-#endif	// #if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)

flight/Modules/GPS/inc/GPS.h

@@ -34,8 +34,6 @@
 #ifndef GPS_H
 #define GPS_H
 
-#include "gps_mode.h"
-
 int32_t GPSInitialize(void);
 
 #endif // GPS_H

flight/Modules/GPS/inc/NMEA.h

@@ -33,11 +33,8 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include "gps_mode.h"
 
-#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP)
+extern bool NMEA_update_position(char *nmea_sentence);
-	extern bool NMEA_update_position(char *nmea_sentence);
+extern bool NMEA_checksum(char *nmea_sentence);
-	extern bool NMEA_checksum(char *nmea_sentence);
-#endif
 
 #endif /* NMEA_H */

flight/Modules/GPS/inc/gps_mode.h

@@ -1,58 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{ 
- * @addtogroup GSPModule GPS Module
- * @brief Process GPS information
- * @{ 
- *
- * @file       gps_mode.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Include file of the GPS module.
- * 	       As with all modules only the initialize function is exposed all other
- * 	       interactions with the module take place through the event queue and
- *             objects.
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef GPS_MODE_H
-#define GPS_MODE_H
-
-// ****************
-// you MUST have one of these uncommented - and ONLY one
-
-//#define ENABLE_GPS_BINARY_GTOP      // uncomment this if we are using GTOP BINARY mode
-//#define ENABLE_GPS_ONESENTENCE_GTOP // uncomment this if we are using GTOP SINGLE SENTENCE mode
-#define ENABLE_GPS_NMEA               // uncomment this if we are using NMEA mode
-
-// ****************
-// make sure they have defined a protocol to use
-
-#if !defined(ENABLE_GPS_BINARY_GTOP) && !defined(ENABLE_GPS_ONESENTENCE_GTOP) && !defined(ENABLE_GPS_NMEA)
-	#error YOU MUST SELECT THE DESIRED GPS PROTOCOL IN gps_mode.h!
-#endif
-
-// ****************
-
-#endif
-
-/**
-  * @}
-  * @}
-  */

flight/Modules/Guidance/guidance.c

@@ -97,6 +97,12 @@ int32_t GuidanceStart()
  */
 int32_t GuidanceInitialize()
 {
+
+	GuidanceSettingsInitialize();
+	PositionDesiredInitialize();
+	NedAccelInitialize();
+	VelocityDesiredInitialize();
+
 	// Create object queue
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 	

flight/Modules/ManualControl/inc/manualcontrol.h

@@ -105,4 +105,11 @@ int32_t ManualControlInitialize();
 ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
 )
 
+#define assumptions_channelcount ( \
+( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM ) && \
+( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM ) && \
+( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM ) && \
+( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM ) && \
+( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM ) )
+
 #endif // MANUALCONTROL_H

flight/Modules/ManualControl/manualcontrol.c

@@ -43,6 +43,7 @@
 #include "flighttelemetrystats.h"
 #include "flightstatus.h"
 #include "accessorydesired.h"
+#include "receiveractivity.h"
 
 // Private constants
 #if defined(PIOS_MANUAL_STACK_SIZE)
@@ -80,6 +81,7 @@ static void updateActuatorDesired(ManualControlCommandData * cmd);
 static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
 static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
 static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void setArmedIfChanged(uint8_t val);
 
 static void manualControlTask(void *parameters);
 static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
@@ -87,20 +89,30 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
 static bool okToArm(void);
 static bool validInputRange(int16_t min, int16_t max, uint16_t value);
 
-#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
+#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
+#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
+struct rcvr_activity_fsm {
+	ManualControlSettingsChannelGroupsOptions group;
+	uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
+	uint8_t sample_count;
+};
+static struct rcvr_activity_fsm activity_fsm;
+
+static void resetRcvrActivity(struct rcvr_activity_fsm * fsm);
+static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
+
+#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount)
 
 /**
- * Module initialization
+ * Module starting
  */
 int32_t ManualControlStart()
 {
-	/* Check the assumptions about uavobject enum's are correct */
-	if(!assumptions)
-		return -1;
 	// Start main task
 	xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
 	TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
 	PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
+
 	return 0;
 }
 
@@ -110,6 +122,17 @@ int32_t ManualControlStart()
 int32_t ManualControlInitialize()
 {
 
+	/* Check the assumptions about uavobject enum's are correct */
+	if(!assumptions)
+		return -1;
+
+	AccessoryDesiredInitialize();
+	ManualControlCommandInitialize();
+	FlightStatusInitialize();
+	StabilizationDesiredInitialize();
+	ReceiverActivityInitialize();
+	ManualControlSettingsInitialize();
+
 	return 0;
 }
 MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
@@ -138,10 +161,14 @@ static void manualControlTask(void *parameters)
 	flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
 	armState = ARM_STATE_DISARMED;
 
+	/* Initialize the RcvrActivty FSM */
+	portTickType lastActivityTime = xTaskGetTickCount();
+	resetRcvrActivity(&activity_fsm);
+
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	while (1) {
-		float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
+		float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
 
 		// Wait until next update
 		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
@@ -150,6 +177,18 @@ static void manualControlTask(void *parameters)
 		// Read settings
 		ManualControlSettingsGet(&settings);
 
+		/* Update channel activity monitor */
+		if (flightStatus.Armed == ARM_STATE_DISARMED) {
+			if (updateRcvrActivity(&activity_fsm)) {
+				/* Reset the aging timer because activity was detected */
+				lastActivityTime = lastSysTime;
+			}
+		}
+		if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
+			resetRcvrActivity(&activity_fsm);
+			lastActivityTime = lastSysTime;
+		}
+
 		if (ManualControlCommandReadOnly(&cmd)) {
 			FlightTelemetryStatsData flightTelemStats;
 			FlightTelemetryStatsGet(&flightTelemStats);
@@ -164,34 +203,64 @@ static void manualControlTask(void *parameters)
 
 		if (!ManualControlCommandReadOnly(&cmd)) {
 
+			bool valid_input_detected = true;
+			
 			// Read channel values in us
-			for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
+			for (uint8_t n = 0; 
-				if (pios_rcvr_channel_to_id_map[n].id) {
+			     n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
-					cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n].id,
+			     ++n) {
-									pios_rcvr_channel_to_id_map[n].channel);
+				extern uint32_t pios_rcvr_group_map[];
+
+				if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					cmd.Channel[n] = PIOS_RCVR_INVALID;
 				} else {
-					cmd.Channel[n] = -1;
+					cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
+									settings.ChannelNumber[n]);
 				}
-				scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n],	settings.ChannelMin[n], settings.ChannelNeutral[n]);
+				
+				// If a channel has timed out this is not valid data and we shouldn't update anything
+				// until we decide to go to failsafe
+				if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
+					valid_input_detected = false;
+				else
+					scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n],	settings.ChannelMin[n], settings.ChannelNeutral[n]);
 			}
 
 			// Check settings, if error raise alarm
-			if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
+			if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-			    settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
+				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-			    settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
+				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-			    settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
+				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
-			    settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
+				settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
+				// Check all channel mappings are valid
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
+				// Check the driver is exists
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
+				cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER) {
+
 				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
 				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
 				ManualControlCommandSet(&cmd);
+
+				// Need to do this here since we don't process armed status.  Since this shouldn't happen in flight (changed config) 
+				// immediately disarm
+				setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+
 				continue;
 			}
 
 			// decide if we have valid manual input or not
-			bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
+			valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
-			     validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
+			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
-			     validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
+			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
-			     validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
+			     validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
 
 			// Implement hysteresis loop on connection status
 			if (valid_input_detected && (++connected_count > 10)) {
@@ -209,55 +278,88 @@ static void manualControlTask(void *parameters)
 				cmd.Roll = 0;
 				cmd.Yaw = 0;
 				cmd.Pitch = 0;
+				cmd.Collective = 0;
 				//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
 				// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
 				// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
 				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				ManualControlCommandSet(&cmd);
+				
-			} else {
-				AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
-
-				// Scale channels to -1 -> +1 range
-				cmd.Roll           = scaledChannel[settings.Roll];
-				cmd.Pitch          = scaledChannel[settings.Pitch];
-				cmd.Yaw            = scaledChannel[settings.Yaw];
-				cmd.Throttle       = scaledChannel[settings.Throttle];
-				flightMode         = scaledChannel[settings.FlightMode];
-
 				AccessoryDesiredData accessory;
 				// Set Accessory 0
-				if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
-					accessory.AccessoryVal = scaledChannel[settings.Accessory0];
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = 0;
 					if(AccessoryDesiredInstSet(0, &accessory) != 0)
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
 				}
 				// Set Accessory 1
-				if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
-					accessory.AccessoryVal = scaledChannel[settings.Accessory1];
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = 0;
 					if(AccessoryDesiredInstSet(1, &accessory) != 0)
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
 				}
-				// Set Accsesory 2
+				// Set Accessory 2
-				if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
-					accessory.AccessoryVal = scaledChannel[settings.Accessory2];
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = 0;
+					if(AccessoryDesiredInstSet(2, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+
+			} else {
+				AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
+
+				// Scale channels to -1 -> +1 range
+				cmd.Roll           = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
+				cmd.Pitch          = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
+				cmd.Yaw            = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
+				cmd.Throttle       = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
+				flightMode         = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
+
+				if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_INVALID &&
+				   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER &&
+				   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT)
+					cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
+				   
+				AccessoryDesiredData accessory;
+				// Set Accessory 0
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
+					if(AccessoryDesiredInstSet(0, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+				// Set Accessory 1
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
+					if(AccessoryDesiredInstSet(1, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+				// Set Accessory 2
+				if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
+					MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+					accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
 					if(AccessoryDesiredInstSet(2, &accessory) != 0)
 						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
 				}
 
 
 				processFlightMode(&settings, flightMode);
-				processArm(&cmd, &settings);
-
-				// Update cmd object
-				ManualControlCommandSet(&cmd);
 
 			}
 
+			// Process arming outside conditional so system will disarm when disconnected
+			processArm(&cmd, &settings);
+			
+			// Update cmd object
+			ManualControlCommandSet(&cmd);
+
 		} else {
 			ManualControlCommandGet(&cmd);	/* Under GCS control */
 		}
 
-
 		FlightStatusGet(&flightStatus);
 
 		// Depending on the mode update the Stabilization or Actuator objects
@@ -279,6 +381,145 @@ static void manualControlTask(void *parameters)
 	}
 }
 
+static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
+{
+	ReceiverActivityData data;
+	bool updated = false;
+
+	/* Clear all channel activity flags */
+	ReceiverActivityGet(&data);
+	if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE &&
+		data.ActiveChannel != 255) {
+		data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
+		data.ActiveChannel = 255;
+		updated = true;
+	}
+	if (updated) {
+		ReceiverActivitySet(&data);
+	}
+
+	/* Reset the FSM state */
+	fsm->group        = 0;
+	fsm->sample_count = 0;
+}
+
+static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) {
+	for (uint8_t channel = 1; channel <= max_channels; channel++) {
+		// Subtract 1 because channels are 1 indexed
+		samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
+	}
+}
+
+static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm)
+{
+	bool activity_updated = false;
+
+	/* Compare the current value to the previous sampled value */
+	for (uint8_t channel = 1;
+	     channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP;
+	     channel++) {
+		uint16_t delta;
+		uint16_t prev = fsm->prev[channel - 1];   // Subtract 1 because channels are 1 indexed
+		uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); 
+		if (curr > prev) {
+			delta = curr - prev;
+		} else {
+			delta = prev - curr;
+		}
+
+		if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) {
+			/* Mark this channel as active */
+			ReceiverActivityActiveGroupOptions group;
+
+			/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
+			switch (fsm->group) {
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: 
+				group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
+				break;
+			default:
+				PIOS_Assert(0);
+				break;
+			}
+
+			ReceiverActivityActiveGroupSet(&group);
+			ReceiverActivityActiveChannelSet(&channel);
+			activity_updated = true;
+		}
+	}
+	return (activity_updated);
+}
+
+static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
+{
+	bool activity_updated = false;
+
+	if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+		/* We're out of range, reset things */
+		resetRcvrActivity(fsm);
+	}
+
+	extern uint32_t pios_rcvr_group_map[];
+	if (!pios_rcvr_group_map[fsm->group]) {
+		/* Unbound group, skip it */
+		goto group_completed;
+	}
+
+	if (fsm->sample_count == 0) {
+		/* Take a sample of each channel in this group */
+		updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
+					fsm->prev,
+					NELEMENTS(fsm->prev));
+		fsm->sample_count++;
+		return (false);
+	}
+
+	/* Compare with previous sample */
+	activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
+
+group_completed:
+	/* Reset the sample counter */
+	fsm->sample_count = 0;
+
+	/* Find the next active group, but limit search so we can't loop forever here */
+	for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) {
+		/* Move to the next group */
+		fsm->group++;
+		if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
+			/* Wrap back to the first group */
+			fsm->group = 0;
+		}
+		if (pios_rcvr_group_map[fsm->group]) {
+			/* 
+			 * Found an active group, take a sample here to avoid an
+			 * extra 20ms delay in the main thread so we can speed up
+			 * this algorithm.
+			 */
+			updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
+						fsm->prev,
+						NELEMENTS(fsm->prev));
+			fsm->sample_count++;
+			break;
+		}
+	}
+
+	return (activity_updated);
+}
+
 static void updateActuatorDesired(ManualControlCommandData * cmd)
 {
 	ActuatorDesiredData actuator;
@@ -440,7 +681,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
 	} else {
 		// Not really needed since this function not called when disconnected
 		if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
-			return;
+			lowThrottle = true;
 
 		// The throttle is not low, in case we where arming or disarming, abort
 		if (!lowThrottle) {
@@ -584,4 +825,3 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
   * @}
   * @}
   */
-

flight/Modules/Stabilization/stabilization.c

@@ -40,8 +40,6 @@
 #include "attitudeactual.h"
 #include "attituderaw.h"
 #include "flightstatus.h"
-#include "systemsettings.h"
-#include "ahrssettings.h"
 #include "manualcontrol.h" // Just to get a macro
 #include "CoordinateConversions.h"
 
@@ -115,6 +113,11 @@ int32_t StabilizationStart()
 int32_t StabilizationInitialize()
 {
 	// Initialize variables
+	StabilizationSettingsInitialize();
+	ActuatorDesiredInitialize();
+#if defined(DIAGNOSTICS)
+	RateDesiredInitialize();
+#endif
 
 	// Create object queue
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
@@ -147,7 +150,6 @@ static void stabilizationTask(void* parameters)
 	RateDesiredData rateDesired;
 	AttitudeActualData attitudeActual;
 	AttitudeRawData attitudeRaw;
-	SystemSettingsData systemSettings;
 	FlightStatusData flightStatus;
 
 	SettingsUpdatedCb((UAVObjEvent *) NULL);
@@ -175,8 +177,10 @@ static void stabilizationTask(void* parameters)
 		StabilizationDesiredGet(&stabDesired);
 		AttitudeActualGet(&attitudeActual);
 		AttitudeRawGet(&attitudeRaw);
+
+#if defined(DIAGNOSTICS)
 		RateDesiredGet(&rateDesired);
-		SystemSettingsGet(&systemSettings);
+#endif
 
 #if defined(PIOS_QUATERNION_STABILIZATION)
 		// Quaternion calculation of error in each axis.  Uses more memory.
@@ -231,6 +235,9 @@ static void stabilizationTask(void* parameters)
 			{
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
 					rateDesiredAxis[i] = attitudeDesiredAxis[i];
+
+					// Zero attitude and axis lock accumulators
+					pids[PID_ROLL + i].iAccumulator = 0;
 					axis_lock_accum[i] = 0;
 					break;
 
@@ -245,11 +252,20 @@ static void stabilizationTask(void* parameters)
 
 					rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
 
+					// Zero attitude and axis lock accumulators
+					pids[PID_ROLL + i].iAccumulator = 0;
 					axis_lock_accum[i] = 0;
 					break;
 				}
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
 					rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);
+					
+					if(rateDesiredAxis[i] > settings.MaximumRate[i])
+						rateDesiredAxis[i] = settings.MaximumRate[i];
+					else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
+						rateDesiredAxis[i] = -settings.MaximumRate[i];
+					
+					
 					axis_lock_accum[i] = 0;
 					break;
 
@@ -268,21 +284,24 @@ static void stabilizationTask(void* parameters)
 
 						rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);
 					}
+					
+					if(rateDesiredAxis[i] > settings.MaximumRate[i])
+						rateDesiredAxis[i] = settings.MaximumRate[i];
+					else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
+						rateDesiredAxis[i] = -settings.MaximumRate[i];
+
 					break;
 			}
 		}
 
 		uint8_t shouldUpdate = 1;
+#if defined(DIAGNOSTICS)
 		RateDesiredSet(&rateDesired);
+#endif
 		ActuatorDesiredGet(&actuatorDesired);
 		//Calculate desired command
 		for(int8_t ct=0; ct< MAX_AXES; ct++)
 		{
-			if(rateDesiredAxis[ct] > settings.MaximumRate[ct])
-				rateDesiredAxis[ct] = settings.MaximumRate[ct];
-			else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])
-				rateDesiredAxis[ct] = -settings.MaximumRate[ct];
-
 			switch(stabDesired.StabilizationMode[ct])
 			{
 				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
@@ -300,14 +319,20 @@ static void stabilizationTask(void* parameters)
 					case ROLL:
 						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
 						shouldUpdate = 1;
+						pids[PID_RATE_ROLL].iAccumulator = 0;
+						pids[PID_ROLL].iAccumulator = 0;
 						break;
 					case PITCH:
 						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
 						shouldUpdate = 1;
+						pids[PID_RATE_PITCH].iAccumulator = 0;
+						pids[PID_PITCH].iAccumulator = 0;
 						break;
 					case YAW:
 						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);
 						shouldUpdate = 1;
+						pids[PID_RATE_YAW].iAccumulator = 0;
+						pids[PID_YAW].iAccumulator = 0;
 						break;
 				}
 					break;

flight/Modules/System/systemmod.c

@@ -43,7 +43,9 @@
 #include "objectpersistence.h"
 #include "flightstatus.h"
 #include "systemstats.h"
+#include "systemsettings.h"
 #include "i2cstats.h"
+#include "taskinfo.h"
 #include "watchdogstatus.h"
 #include "taskmonitor.h"
 #include "pios_config.h"
@@ -73,16 +75,18 @@
 static uint32_t idleCounter;
 static uint32_t idleCounterClear;
 static xTaskHandle systemTaskHandle;
-static int32_t stackOverflow;
+static bool stackOverflow;
+static bool mallocFailed;
 
 // Private functions
 static void objectUpdatedCb(UAVObjEvent * ev);
 static void updateStats();
-static void updateI2Cstats();
-static void updateWDGstats();
 static void updateSystemAlarms();
 static void systemTask(void *parameters);
-
+#if defined(DIAGNOSTICS)
+static void updateI2Cstats();
+static void updateWDGstats();
+#endif
 /**
  * Create the module task.
  * \returns 0 on success or -1 if initialization failed
@@ -90,7 +94,8 @@ static void systemTask(void *parameters);
 int32_t SystemModStart(void)
 {
 	// Initialize vars
-	stackOverflow = 0;
+	stackOverflow = false;
+	mallocFailed = false;
 	// Create system task
 	xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
 	// Register task
@@ -106,6 +111,16 @@ int32_t SystemModStart(void)
 int32_t SystemModInitialize(void)
 {
 
+	// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
+	SystemSettingsInitialize();
+	SystemStatsInitialize();
+	ObjectPersistenceInitialize();
+#if defined(DIAGNOSTICS)
+	TaskInfoInitialize();
+	I2CStatsInitialize();
+	WatchdogStatusInitialize();
+#endif
+
 	SystemModStart();
 
 	return 0;
@@ -137,9 +152,10 @@ static void systemTask(void *parameters)
 
 		// Update the system alarms
 		updateSystemAlarms();
+#if defined(DIAGNOSTICS)
 		updateI2Cstats();
 		updateWDGstats();
-		
+#endif
 		// Update the task status object
 		TaskMonitorUpdateAll();
 
@@ -230,6 +246,11 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 				   || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) {
 				retval = UAVObjDeleteMetaobjects();
 			}
+		} else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) {
+			retval = -1;
+#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
+			retval = PIOS_FLASHFS_Format();
+#endif
 		}
 		if(retval == 0) { 
 			objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
@@ -241,9 +262,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 /**
  * Called periodically to update the I2C statistics 
  */
-#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
+#if defined(DIAGNOSTICS)
-static void updateI2Cstats() {} //Posix and win32 don't have I2C
-#else
 static void updateI2Cstats() 
 {
 #if defined(PIOS_INCLUDE_I2C)
@@ -263,7 +282,6 @@ static void updateI2Cstats()
 	I2CStatsSet(&i2cStats);
 #endif
 }
-#endif
 
 static void updateWDGstats() 
 {
@@ -272,6 +290,8 @@ static void updateWDGstats()
 	watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
 	WatchdogStatusSet(&watchdogStatus);
 }
+#endif
+
 
 /**
  * Called periodically to update the system stats
@@ -399,12 +419,19 @@ static void updateSystemAlarms()
 	}
 
 	// Check for stack overflow
-	if (stackOverflow == 1) {
+	if (stackOverflow) {
 		AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
 	} else {
 		AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
 	}
 
+	// Check for malloc failures
+	if (mallocFailed) {
+		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
+	} else {
+		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
+	}
+
 #if defined(PIOS_INCLUDE_SDCARD)
 	// Check for SD card
 	if (PIOS_SDCARD_IsMounted() == 0) {
@@ -443,9 +470,29 @@ void vApplicationIdleHook(void)
 /**
  * Called by the RTOS when a stack overflow is detected.
  */
+#define DEBUG_STACK_OVERFLOW 0
 void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
 {
-	stackOverflow = 1;
+	stackOverflow = true;
+#if DEBUG_STACK_OVERFLOW
+	static volatile bool wait_here = true;
+	while(wait_here);
+	wait_here = true;
+#endif
+}
+
+/**
+ * Called by the RTOS when a malloc call fails.
+ */
+#define DEBUG_MALLOC_FAILURES 0
+void vApplicationMallocFailedHook(void)
+{
+	mallocFailed = true;
+#if DEBUG_MALLOC_FAILURES
+	static volatile bool wait_here = true;
+	while(wait_here);
+	wait_here = true;
+#endif
 }
 
 /**

flight/Modules/Telemetry/telemetry.c

@@ -67,6 +67,7 @@ static uint32_t txErrors;
 static uint32_t txRetries;
 static TelemetrySettingsData settings;
 static uint32_t timeOfLastObjectUpdate;
+static UAVTalkConnection uavTalkCon;
 
 // Private functions
 static void telemetryTxTask(void *parameters);
@@ -88,7 +89,13 @@ static void updateSettings();
  */
 int32_t TelemetryStart(void)
 {
-
+	// Process all registered objects and connect queue for updates
+	UAVObjIterate(&registerObject);
+    
+	// Listen to objects of interest
+	GCSTelemetryStatsConnectQueue(priorityQueue);
+	TelemetrySettingsConnectQueue(priorityQueue);
+    
 	// Start telemetry tasks
 	xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
 	xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
@@ -111,6 +118,10 @@ int32_t TelemetryStart(void)
 int32_t TelemetryInitialize(void)
 {
 	UAVObjEvent ev;
+    
+	FlightTelemetryStatsInitialize();
+	GCSTelemetryStatsInitialize();
+	TelemetrySettingsInitialize();
 
 	// Initialize vars
 	timeOfLastObjectUpdate = 0;
@@ -120,25 +131,19 @@ int32_t TelemetryInitialize(void)
 #if defined(PIOS_TELEM_PRIORITY_QUEUE)
 	priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 #endif
-	
+
-	// Get telemetry settings object
+    // Get telemetry settings object
 	updateSettings();
-
+    
 	// Initialise UAVTalk
-	UAVTalkInitialize(&transmitData);
+	uavTalkCon = UAVTalkInitialize(&transmitData,256);
-
+    
-	// Process all registered objects and connect queue for updates
-	UAVObjIterate(&registerObject);
-
 	// Create periodic event that will be used to update the telemetry stats
 	txErrors = 0;
 	txRetries = 0;
 	memset(&ev, 0, sizeof(UAVObjEvent));
 	EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
-
+    
-	// Listen to objects of interest
-	GCSTelemetryStatsConnectQueue(priorityQueue);
-	TelemetrySettingsConnectQueue(priorityQueue);
 
 	return 0;
 }
@@ -235,7 +240,7 @@ static void processObjEvent(UAVObjEvent * ev)
 			if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
 				// Send update to GCS (with retries)
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS);	// call blocks until ack is received or timeout
+					success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS);	// call blocks until ack is received or timeout
 					++retries;
 				}
 				// Update stats
@@ -246,7 +251,7 @@ static void processObjEvent(UAVObjEvent * ev)
 			} else if (ev->event == EV_UPDATE_REQ) {
 				// Request object update from GCS (with retries)
 				while (retries < MAX_RETRIES && success == -1) {
-					success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS);	// call blocks until update is received or timeout
+					success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);	// call blocks until update is received or timeout
 					++retries;
 				}
 				// Update stats
@@ -326,7 +331,7 @@ static void telemetryRxTask(void *parameters)
 			bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
 			if (bytes_to_process > 0) {
 				for (uint8_t i = 0; i < bytes_to_process; i++) {
-					UAVTalkProcessInputStream(serial_data[i]);
+					UAVTalkProcessInputStream(uavTalkCon,serial_data[i]);
 				}
 			}
 		} else {
@@ -426,8 +431,8 @@ static void updateTelemetryStats()
 	uint32_t timeNow;
 
 	// Get stats
-	UAVTalkGetStats(&utalkStats);
+	UAVTalkGetStats(uavTalkCon, &utalkStats);
-	UAVTalkResetStats();
+	UAVTalkResetStats(uavTalkCon);
 
 	// Get object data
 	FlightTelemetryStatsGet(&flightStats);

flight/OpenPilot/Makefile

@@ -135,7 +135,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
 else
 ## TESTCODE
 SRC += $(OPTESTS)/test_common.c
@@ -165,6 +164,7 @@ SRC += $(PIOSSTM32F10X)/pios_ppm.c
 SRC += $(PIOSSTM32F10X)/pios_pwm.c
 SRC += $(PIOSSTM32F10X)/pios_spektrum.c
 SRC += $(PIOSSTM32F10X)/pios_sbus.c
+SRC += $(PIOSSTM32F10X)/pios_tim.c
 SRC += $(PIOSSTM32F10X)/pios_debug.c
 SRC += $(PIOSSTM32F10X)/pios_gpio.c
 SRC += $(PIOSSTM32F10X)/pios_exti.c
@@ -388,6 +388,9 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -g$(DEBUGF) -DDEBUG
 endif
 
+# OP has enough memory to always enable optional objects
+CFLAGS += -DDIAGNOSTICS
+
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
 CFLAGS += $(CDEFS)
@@ -512,7 +515,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf:  $(OUTDIR)/$(TARGET).elf

flight/OpenPilot/System/alarms.c

@@ -41,14 +41,12 @@ static xSemaphoreHandle lock;
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
 
 /**
- * Initialize the alarms library
+ * Initialize the alarms library
  */
 int32_t AlarmsInitialize(void)
 {
+	SystemAlarmsInitialize();
 	lock = xSemaphoreCreateRecursiveMutex();
-	//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
-	//AlarmsClearAll();
-	//AlarmsDefaultAll();
 	return 0;
 }
 
@@ -56,7 +54,7 @@ int32_t AlarmsInitialize(void)
  * Set an alarm
  * @param alarm The system alarm to be modified
  * @param severity The alarm severity
- * @return 0 if success, -1 if an error
+ * @return 0 if success, -1 if an error
  */
 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
 {
@@ -151,7 +149,7 @@ void AlarmsClearAll()
 
 /**
  * Check if there are any alarms with the given or higher severity
- * @return 0 if no alarms are found, 1 if at least one alarm is found
+ * @return 0 if no alarms are found, 1 if at least one alarm is found
  */
 int32_t AlarmsHasWarnings()
 {
@@ -208,5 +206,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 /**
  * @}
  * @}
- */
+ */
 

flight/OpenPilot/System/inc/FreeRTOSConfig.h

@@ -26,6 +26,7 @@
 #define configUSE_PREEMPTION		1
 #define configUSE_IDLE_HOOK		1
 #define configUSE_TICK_HOOK		0
+#define configUSE_MALLOC_FAILED_HOOK    1
 #define configCPU_CLOCK_HZ		( ( unsigned long ) 72000000 )
 #define configTICK_RATE_HZ		( ( portTickType ) 1000 )
 #define configMAX_PRIORITIES		( ( unsigned portBASE_TYPE ) 5 )
@@ -39,7 +40,7 @@
 #define configUSE_RECURSIVE_MUTEXES	1
 #define configUSE_COUNTING_SEMAPHORES	0
 #define configUSE_ALTERNATIVE_API	0
-#define configCHECK_FOR_STACK_OVERFLOW	2
+//#define configCHECK_FOR_STACK_OVERFLOW	2
 #define configQUEUE_REGISTRY_SIZE	10
 
 /* Co-routine definitions. */
@@ -72,7 +73,9 @@ NVIC value of 255. */
 #define configLIBRARY_KERNEL_INTERRUPT_PRIORITY	15
 
 /* Enable run time stats collection */
-#if defined(DEBUG)
+#if defined(DIAGNOSTICS)
+#define configCHECK_FOR_STACK_OVERFLOW	2
+
 #define configGENERATE_RUN_TIME_STATS 1
 #define INCLUDE_uxTaskGetRunTime 1
 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
@@ -81,6 +84,8 @@ do {\
 (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
 } while(0)
 #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
+#else
+#define configCHECK_FOR_STACK_OVERFLOW	1
 #endif
 
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)

flight/OpenPilot/System/inc/pios_config.h

@@ -47,7 +47,7 @@
 #define PIOS_INCLUDE_SPEKTRUM
 //#define PIOS_INCLUDE_SBUS
 #define PIOS_INCLUDE_PWM
-//#define PIOS_INCLUDE_PPM
+#define PIOS_INCLUDE_PPM
 
 #define PIOS_INCLUDE_TELEMETRY_RF
 
@@ -60,6 +60,7 @@
 //#define PIOS_INCLUDE_HCSR04
 #define PIOS_INCLUDE_OPAHRS
 #define PIOS_INCLUDE_COM
+#define PIOS_INCLUDE_GPS
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_SETTINGS
 #define PIOS_INCLUDE_FREERTOS

flight/OpenPilot/System/inc/pios_config_posix.h

@@ -36,6 +36,7 @@
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS
 #define PIOS_INCLUDE_COM
+#define PIOS_INCLUDE_GPS
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_TELEMETRY_RF
 #define PIOS_INCLUDE_UDP

flight/OpenPilot/System/pios_board.c

@@ -30,6 +30,7 @@
 #include <pios.h>
 #include <openpilot.h>
 #include <uavobjectsinit.h>
+#include <hwsettings.h>
 #include "manualcontrolsettings.h"
 
 //#define I2C_DEBUG_PIN			0
@@ -301,6 +302,89 @@ void PIOS_ADC_handler() {
 	PIOS_ADC_DMA_Handler();
 }
 
+#include "pios_tim_priv.h"
+
+static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = {
+	.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
+	.TIM_CounterMode = TIM_CounterMode_Up,
+	.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
+	.TIM_RepetitionCounter = 0x0000,
+};
+
+static const struct pios_tim_clock_cfg tim_4_cfg = {
+	.timer = TIM4,
+	.time_base_init = &tim_4_8_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_8_cfg = {
+	.timer = TIM8,
+	.time_base_init = &tim_4_8_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM8_CC_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = {
+	.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
+	.TIM_ClockDivision = TIM_CKD_DIV1,
+	.TIM_CounterMode = TIM_CounterMode_Up,
+	.TIM_Period = 0xFFFF,
+	.TIM_RepetitionCounter = 0x0000,
+};
+
+static const struct pios_tim_clock_cfg tim_1_cfg = {
+	.timer = TIM1,
+	.time_base_init = &tim_1_3_5_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM1_CC_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_3_cfg = {
+	.timer = TIM3,
+	.time_base_init = &tim_1_3_5_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM3_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+static const struct pios_tim_clock_cfg tim_5_cfg = {
+	.timer = TIM5,
+	.time_base_init = &tim_1_3_5_time_base,
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel                   = TIM5_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
 #if defined(PIOS_INCLUDE_USART)
 
 #include "pios_usart_priv.h"
@@ -496,7 +580,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
   },
 };
 
-#include <pios_spektrum_priv.h>
 static const struct pios_spektrum_cfg pios_spektrum_cfg = {
 	.bind = {
 		.gpio = GPIOA,
@@ -535,65 +618,106 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = {
  * Pios servo configuration structures
  */
 #include <pios_servo_priv.h>
-static const struct pios_servo_channel pios_servo_channels[] = {
+static const struct pios_tim_channel pios_tim_servoport_all_pins[] = {
 	{
 		.timer = TIM4,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_1,
-		.channel = TIM_Channel_1,
+		.pin = {
-		.pin = GPIO_Pin_6,
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_6,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM4,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_2,
-		.channel = TIM_Channel_2,
+		.pin = {
-		.pin = GPIO_Pin_7,
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_7,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM4,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_3,
-		.channel = TIM_Channel_3,
+		.pin = {
-		.pin = GPIO_Pin_8,
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_8,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM4,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_4,
-		.channel = TIM_Channel_4,
+		.pin = {
-		.pin = GPIO_Pin_9,
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_9,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM8,
-		.port = GPIOC,
+		.timer_chan = TIM_Channel_1,
-		.channel = TIM_Channel_1,
+		.pin = {
-		.pin = GPIO_Pin_6,
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_6,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM8,
-		.port = GPIOC,
+		.timer_chan = TIM_Channel_2,
-		.channel = TIM_Channel_2,
+		.pin = {
-		.pin = GPIO_Pin_7,
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_7,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM8,
-		.port = GPIOC,
+		.timer_chan = TIM_Channel_3,
-		.channel = TIM_Channel_3,
+		.pin = {
-		.pin = GPIO_Pin_8,
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_8,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM8,
-		.port = GPIOC,
+		.timer_chan = TIM_Channel_4,
-		.channel = TIM_Channel_4,
+		.pin = {
-		.pin = GPIO_Pin_9,
+			.gpio = GPIOC,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_9,
+				.GPIO_Mode = GPIO_Mode_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 	
 };
 
 const struct pios_servo_cfg pios_servo_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1),
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_oc_init = {
 		.TIM_OCMode = TIM_OCMode_PWM1,
 		.TIM_OutputState = TIM_OutputState_Enable,
@@ -604,127 +728,127 @@ const struct pios_servo_cfg pios_servo_cfg = {
 		.TIM_OCIdleState = TIM_OCIdleState_Reset,
 		.TIM_OCNIdleState = TIM_OCNIdleState_Reset,
 	},
-	.gpio_init = {
+	.channels = pios_tim_servoport_all_pins,
-		.GPIO_Mode = GPIO_Mode_AF_PP,
+	.num_channels = NELEMENTS(pios_tim_servoport_all_pins),
-		.GPIO_Speed = GPIO_Speed_2MHz,
-	},
-	.remap = 0,
-	.channels = pios_servo_channels,
-	.num_channels = NELEMENTS(pios_servo_channels),
 };
 
 
 /* 
  * PWM Inputs 
  */
-#if defined(PIOS_INCLUDE_PWM)
+#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
 #include <pios_pwm_priv.h>
-static const struct pios_pwm_channel pios_pwm_channels[] = {
+static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = {
 	{
 		.timer = TIM1,
-		.port = GPIOA,
+		.timer_chan = TIM_Channel_2,
-		.ccr = TIM_IT_CC2,
+		.pin = {
-		.channel = TIM_Channel_2,
+			.gpio = GPIOA,
-		.pin = GPIO_Pin_9,
+			.init = {
-	}, 
+				.GPIO_Pin = GPIO_Pin_9,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},
 	{
 		.timer = TIM1,
-		.port = GPIOA,
+		.timer_chan = TIM_Channel_3,
-		.ccr = TIM_IT_CC3,
+		.pin = {
-		.channel = TIM_Channel_3,
+			.gpio = GPIOA,
-		.pin = GPIO_Pin_10,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_10,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM5,
-		.port = GPIOA,
+		.timer_chan = TIM_Channel_1,
-		.ccr = TIM_IT_CC1,
+		.pin = {
-		.channel = TIM_Channel_1,
+			.gpio = GPIOA,
-		.pin = GPIO_Pin_0
+			.init = {
+				.GPIO_Pin = GPIO_Pin_0,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{
 		.timer = TIM1,
-		.port = GPIOA,
+		.timer_chan = TIM_Channel_1,
-		.ccr = TIM_IT_CC1,
+		.pin = {
-		.channel = TIM_Channel_1,
+			.gpio = GPIOA,
-		.pin = GPIO_Pin_8,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_8,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 
 	{ 
 		.timer = TIM3,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_4,
-		.ccr = TIM_IT_CC4,
+		.pin = {
-		.channel = TIM_Channel_4,
+			.gpio = GPIOB,
-		.pin = GPIO_Pin_1,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_1,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	},  	
 	{
 		.timer = TIM3,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_3,
-		.ccr = TIM_IT_CC3,
+		.pin = {
-		.channel = TIM_Channel_3,
+			.gpio = GPIOB,
-		.pin = GPIO_Pin_0,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_0,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
 	}, 		
 	{
 		.timer = TIM3,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_1,
-		.ccr = TIM_IT_CC1,
+		.pin = {
-		.channel = TIM_Channel_1,
+			.gpio = GPIOB,
-		.pin = GPIO_Pin_4,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_4,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+		.remap = GPIO_PartialRemap_TIM3,
 	}, 		
 	{
 		.timer = TIM3,
-		.port = GPIOB,
+		.timer_chan = TIM_Channel_2,
-		.ccr = TIM_IT_CC2,
+		.pin = {
-		.channel = TIM_Channel_2,
+			.gpio = GPIOB,
-		.pin = GPIO_Pin_5,
+			.init = {
+				.GPIO_Pin = GPIO_Pin_5,
+				.GPIO_Mode = GPIO_Mode_IPD,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+		.remap = GPIO_PartialRemap_TIM3,
 	}, 		
 };
 
-void TIM1_CC_IRQHandler();
-void TIM3_IRQHandler();
-void TIM5_IRQHandler();
-void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler")));
-void TIM3_IRQHandler() __attribute__ ((alias ("PIOS_TIM3_irq_handler")));
-void TIM5_IRQHandler() __attribute__ ((alias ("PIOS_TIM5_irq_handler")));
 const struct pios_pwm_cfg pios_pwm_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = 0xFFFF,
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_ic_init = {
 		.TIM_ICPolarity = TIM_ICPolarity_Rising,
 		.TIM_ICSelection = TIM_ICSelection_DirectTI,
 		.TIM_ICPrescaler = TIM_ICPSC_DIV1,
 		.TIM_ICFilter = 0x0,		
 	},
-	.gpio_init = {
+	.channels = pios_tim_rcvrport_all_channels,
-		.GPIO_Mode = GPIO_Mode_IPD,
+	.num_channels = NELEMENTS(pios_tim_rcvrport_all_channels),
-		.GPIO_Speed = GPIO_Speed_2MHz,
-	},
-	.remap = GPIO_PartialRemap_TIM3,
-	.irq = {
-		.init    = {
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-	.channels = pios_pwm_channels,
-	.num_channels = NELEMENTS(pios_pwm_channels),
 };
-void PIOS_TIM1_CC_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM1);
-}
-void PIOS_TIM3_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM3);
-}
-void PIOS_TIM5_irq_handler()
-{
-	PIOS_PWM_irq_handler(TIM5);
-}
 #endif
 
 /*
@@ -732,42 +856,7 @@ void PIOS_TIM5_irq_handler()
  */
 #if defined(PIOS_INCLUDE_PPM)
 #include <pios_ppm_priv.h>
-void TIM6_IRQHandler();
-void TIM6_IRQHandler() __attribute__ ((alias ("PIOS_TIM6_irq_handler")));
-static const struct pios_ppmsv_cfg pios_ppmsv_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,	/* For 1 uS accuracy */
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = ((1000000 / 25) - 1), /* 25 Hz */
-		.TIM_RepetitionCounter = 0x0000,
-	},
-	.irq = {
-		.init    = {
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-		},
-	},
-	.timer = TIM6,
-	.ccr = TIM_IT_Update,
-};
-
-void PIOS_TIM6_irq_handler(void)
-{
-	PIOS_PPMSV_irq_handler();
-}
-
-void TIM1_CC_IRQHandler();
-void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler")));
 static const struct pios_ppm_cfg pios_ppm_cfg = {
-	.tim_base_init = {
-		.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1,	/* For 1 uS accuracy */
-		.TIM_ClockDivision = TIM_CKD_DIV1,
-		.TIM_CounterMode = TIM_CounterMode_Up,
-		.TIM_Period = 0xFFFF,
-		.TIM_RepetitionCounter = 0x0000,
-	},
 	.tim_ic_init = {
 			.TIM_ICPolarity = TIM_ICPolarity_Rising,
 			.TIM_ICSelection = TIM_ICSelection_DirectTI,
@@ -775,30 +864,11 @@ static const struct pios_ppm_cfg pios_ppm_cfg = {
 			.TIM_ICFilter = 0x0,
 			.TIM_Channel = TIM_Channel_2,
 	},
-	.gpio_init = {
+	/* Use only the first channel for ppm */
-			.GPIO_Mode = GPIO_Mode_IPD,
+	.channels = &pios_tim_rcvrport_all_channels[0],
-			.GPIO_Speed = GPIO_Speed_2MHz,
+	.num_channels = 1,
-			.GPIO_Pin = GPIO_Pin_9,
-	},
-	.remap = 0,
-	.irq = {
-		.init    = {
-			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
-			.NVIC_IRQChannelSubPriority        = 0,
-			.NVIC_IRQChannelCmd                = ENABLE,
-			.NVIC_IRQChannel = TIM1_CC_IRQn,
-		},
-	},
-	.timer = TIM1,
-	.port = GPIOA,
-	.ccr = TIM_IT_CC2,
 };
 
-void PIOS_TIM1_CC_irq_handler(void)
-{
-	PIOS_PPM_irq_handler();
-}
-
 #endif //PPM
 
 #if defined(PIOS_INCLUDE_I2C)
@@ -964,8 +1034,12 @@ static const struct stm32_gpio pios_debug_pins[] = {
 #if defined(PIOS_INCLUDE_RCVR)
 #include "pios_rcvr_priv.h"
 
-struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
+/* One slot per selectable receiver group.
-uint32_t pios_rcvr_max_channel;
+ *  eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS
+ * NOTE: No slot in this map for NONE.
+ */
+uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
+
 #endif /* PIOS_INCLUDE_RCVR */
 
 #if defined(PIOS_INCLUDE_USB_HID)
@@ -1003,8 +1077,9 @@ void PIOS_Board_Init(void) {
 	/* Remap AFIO pin */
 	//GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);
 
-	/* Debug services */
+#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS
-	PIOS_DEBUG_Init();
+	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
+#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
 
 	/* Delay system */
 	PIOS_DELAY_Init();	
@@ -1023,7 +1098,8 @@ void PIOS_Board_Init(void) {
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
-	UAVObjectsInitializeAll();
+
+	HwSettingsInitialize();
 
 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
@@ -1036,6 +1112,14 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Set up pulse timers */
+	PIOS_TIM_InitClock(&tim_1_cfg);
+	PIOS_TIM_InitClock(&tim_3_cfg);
+	PIOS_TIM_InitClock(&tim_5_cfg);
+
+	PIOS_TIM_InitClock(&tim_4_cfg);
+	PIOS_TIM_InitClock(&tim_8_cfg);
+
 	/* Prepare the AHRS Comms upper layer protocol */
 	AhrsInitComms();
 
@@ -1047,126 +1131,128 @@ void PIOS_Board_Init(void) {
 	/* Bind the AHRS comms layer to the AHRS SPI link */
 	AhrsConnect(pios_spi_ahrs_id);
 
-	/* Initialize the PiOS library */
+	/* Configure the main IO port */
-#if defined(PIOS_INCLUDE_COM)
+	uint8_t hwsettings_op_mainport;
+	HwSettingsOP_MainPortGet(&hwsettings_op_mainport);
+
+	switch (hwsettings_op_mainport) {
+	case HWSETTINGS_OP_MAINPORT_DISABLED:
+		break;
+	case HWSETTINGS_OP_MAINPORT_TELEMETRY:
 #if defined(PIOS_INCLUDE_TELEMETRY_RF)
-	{
+		{
-		uint32_t pios_usart_telem_rf_id;
+			uint32_t pios_usart_telem_rf_id;
-		if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
+			if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) {
-			PIOS_Assert(0);
+				PIOS_Assert(0);
-		}
+			}
 
-		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
-		uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
+			uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
-		PIOS_Assert(rx_buffer);
+			PIOS_Assert(rx_buffer);
-		PIOS_Assert(tx_buffer);
+			PIOS_Assert(tx_buffer);
-		if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
+			if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id,
-						  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
+							  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
-						  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
+							  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
-			PIOS_Assert(0);
+				PIOS_Assert(0);
+			}
 		}
-	}
 #endif /* PIOS_INCLUDE_TELEMETRY_RF */
-
+		break;
-#if defined(PIOS_INCLUDE_GPS)
-	{
-		uint32_t pios_usart_gps_id;
-		if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
-			PIOS_Assert(0);
-		}
-		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
-		PIOS_Assert(rx_buffer);
-		if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
-				  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
-				  NULL, 0)) {
-			PIOS_Assert(0);
-		}
 	}
-#endif	/* PIOS_INCLUDE_GPS */
-#endif
 
-	PIOS_Servo_Init();
+	/* Configure the flexi port */
+	uint8_t hwsettings_op_flexiport;
+	HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport);
+
+	switch (hwsettings_op_flexiport) {
+	case HWSETTINGS_OP_FLEXIPORT_DISABLED:
+		break;
+	case HWSETTINGS_OP_FLEXIPORT_GPS:
+#if defined(PIOS_INCLUDE_GPS)
+		{
+			uint32_t pios_usart_gps_id;
+			if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
+				PIOS_Assert(0);
+			}
+			uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
+			PIOS_Assert(rx_buffer);
+			if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
+						rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
+						NULL, 0)) {
+				PIOS_Assert(0);
+			}
+		}
+#endif	/* PIOS_INCLUDE_GPS */
+		break;
+	}
+
+#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS
+	PIOS_Servo_Init(&pios_servo_cfg);
+#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
+
 	PIOS_ADC_Init();
 	PIOS_GPIO_Init();
 
-	/* Configure the selected receiver */
+	/* Configure the rcvr port */
-	uint8_t manualcontrolsettings_inputmode;
+	uint8_t hwsettings_rcvrport;
-	ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode);
+	HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport);
 
-	switch (manualcontrolsettings_inputmode) {
+
-		case MANUALCONTROLSETTINGS_INPUTMODE_PWM:
+	switch (hwsettings_rcvrport) {
-#if defined(PIOS_INCLUDE_PWM)
+	case HWSETTINGS_OP_RCVRPORT_DISABLED:
-#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
+		break;
-#error More receiver inputs than available devices
+	case HWSETTINGS_OP_RCVRPORT_DEBUG:
-#endif
+		/* Not supported yet */
-			PIOS_PWM_Init();
+		break;
-			uint32_t pios_pwm_rcvr_id;
+	case HWSETTINGS_OP_RCVRPORT_SPEKTRUM1:
-			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) {
-			  PIOS_Assert(0);
-			}
-			for (uint8_t i = 0;
-			     i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
-			     i++) {
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id;
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-				pios_rcvr_max_channel++;
-			}
-#endif  /* PIOS_INCLUDE_PWM */
-			break;
-		case MANUALCONTROLSETTINGS_INPUTMODE_PPM:
-#if defined(PIOS_INCLUDE_PPM)
-#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
-#error More receiver inputs than available devices
-#endif
-			PIOS_PPM_Init();
-			uint32_t pios_ppm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) {
-			  PIOS_Assert(0);
-			}
-			for (uint8_t i = 0;
-			     i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
-			     i++) {
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id;
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-				pios_rcvr_max_channel++;
-			}
-#endif	/* PIOS_INCLUDE_PPM */
-			break;
-		case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM:
 #if defined(PIOS_INCLUDE_SPEKTRUM)
-#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
+		{
-#error More receiver inputs than available devices
+			uint32_t pios_usart_spektrum_id;
-#endif
+			if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
-			{
+				PIOS_Assert(0);
-				uint32_t pios_usart_spektrum_id;
-				if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) {
-					PIOS_Assert(0);
-				}
-
-				uint32_t pios_spektrum_id;
-				if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
-					PIOS_Assert(0);
-				}
-
-				uint32_t pios_spektrum_rcvr_id;
-				if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) {
-					PIOS_Assert(0);
-				}
-				for (uint8_t i = 0;
-				     i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
-				     i++) {
-					pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id;
-					pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
-					pios_rcvr_max_channel++;
-				}
 			}
+
+			uint32_t pios_spektrum_id;
+			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
+				PIOS_Assert(0);
+			}
+
+			uint32_t pios_spektrum_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id;
+		}
 #endif
-			break;
+		break;
-		case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
+	case HWSETTINGS_OP_RCVRPORT_PWM:
-#if defined(PIOS_INCLUDE_SBUS)
+#if defined(PIOS_INCLUDE_PWM)
-#error SBUS NOT ON OP YET
+		{
-#endif  /* PIOS_INCLUDE_SBUS */
+			uint32_t pios_pwm_id;
-			break;
+			PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
+
+			uint32_t pios_pwm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
+		}
+#endif	/* PIOS_INCLUDE_PWM */
+		break;
+	case HWSETTINGS_OP_RCVRPORT_PPM:
+#if defined(PIOS_INCLUDE_PPM)
+		{
+			uint32_t pios_ppm_id;
+			PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
+
+			uint32_t pios_ppm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
+		}
+#endif	/* PIOS_INCLUDE_PPM */
+		break;
 	}
 
 #if defined(PIOS_INCLUDE_USB_HID)

flight/OpenPilot/System/pios_board_posix.c

@@ -29,6 +29,11 @@
 #include <openpilot.h>
 #include <uavobjectsinit.h>
 
+#include "attituderaw.h"
+#include "attitudeactual.h"
+#include "positionactual.h"
+#include "velocityactual.h"
+
 #include "pios_rcvr_priv.h"
 
 struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
@@ -66,6 +71,7 @@ const struct pios_udp_cfg pios_udp_aux_cfg = {
 
 #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
+#define PIOS_COM_GPS_RX_BUF_LEN 96
 
 /*
  * Board specific number of devices.
@@ -154,7 +160,7 @@ void PIOS_Board_Init(void) {
 #if defined(PIOS_INCLUDE_GPS)
 	{
 		uint32_t pios_udp_gps_id;
-		if (PIOS_USART_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
+		if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
 			PIOS_Assert(0);
 		}
 		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
@@ -168,6 +174,11 @@ void PIOS_Board_Init(void) {
 #endif	/* PIOS_INCLUDE_GPS */
 #endif
 
+	// Initialize these here as posix has no AHRSComms
+	AttitudeRawInitialize();
+	AttitudeActualInitialize();
+	VelocityActualInitialize();
+	PositionActualInitialize();
 
 }
 

flight/OpenPilot/System/taskmonitor.c

@@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void)
 {
 	lock = xSemaphoreCreateRecursiveMutex();
 	memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
+	lastMonitorTime = 0;
+#if defined(DIAGNOSTICS)
 	lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
+#endif
 	return 0;
 }
 
@@ -91,6 +94,7 @@ int32_t TaskMonitorRemove(TaskInfoRunningElem task)
  */
 void TaskMonitorUpdateAll(void)
 {
+#if defined(DIAGNOSTICS)
 	TaskInfoData data;
 	int n;
 
@@ -142,4 +146,5 @@ void TaskMonitorUpdateAll(void)
 
 	// Done
 	xSemaphoreGiveRecursive(lock);
+#endif
 }

flight/OpenPilot/UAVObjects.inc

@@ -69,6 +69,10 @@ UAVOBJSRCFILENAMES += velocityactual
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus
 UAVOBJSRCFILENAMES += flightstatus
+UAVOBJSRCFILENAMES += hwsettings
+UAVOBJSRCFILENAMES += receiveractivity
+UAVOBJSRCFILENAMES += cameradesired
+UAVOBJSRCFILENAMES += camerastabsettings
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/PiOS.posix/inc/pios_initcall.h

@@ -51,7 +51,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[];
 extern void InitModules();
 extern void StartModules();
 
-#define UAVOBJ_INITCALL(fn)
 #define MODULE_INITCALL(ifn, sfn)
 
 #define MODULE_TASKCREATE_ALL { \

flight/PiOS.posix/posix/pios_debug.c

@@ -79,8 +79,15 @@ void PIOS_DEBUG_Panic(const char *msg)
 	PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr);
 #endif
 
-	// Stay put
+	// tell the user whats going on on commandline too
-	while (1) ;
+	fprintf(stderr,"CRITICAL ERROR: %s\n",msg);
+
+	// this helps debugging: causing a div by zero allows a backtrace
+	// and/or ends execution
+	int b = 0;
+	int a = (2/b);
+	b=a;
+
 }
 
 /**

flight/PiOS.posix/posix/pios_udp.c

@@ -64,6 +64,7 @@ static pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
 {
   if (udp >= pios_udp_num_devices) {
     /* Undefined UDP port for this board (see pios_board.c) */
+	PIOS_Assert(0);
     return NULL;
   }
 
@@ -154,6 +155,8 @@ int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
 
   printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);
 
+  *udp_id = pios_udp_num_devices-1;
+
   return res;
 }
 

flight/PiOS.win32/inc/pios_initcall.h

@@ -38,8 +38,7 @@
  * and we cannot define a linker script for each of them atm
  */
 
-#define UAVOBJ_INITCALL(fn)
+#define MODULE_INITCALL(ifn, sfn)
-#define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags)
 
 #define MODULE_TASKCREATE_ALL
 

flight/PiOS/Boards/STM32103CB_CC_Rev1.h

@@ -206,22 +206,25 @@ extern uint32_t pios_com_telem_usb_id;
 // PIOS_RCVR
 // See also pios_board.c
 //------------------------
-#define PIOS_RCVR_MAX_DEVS                      1
+#define PIOS_RCVR_MAX_DEVS                      3
 #define PIOS_RCVR_MAX_CHANNELS			12
 
 //-------------------------
 // Receiver PPM input
 //-------------------------
+#define PIOS_PPM_MAX_DEVS			1
 #define PIOS_PPM_NUM_INPUTS                     12
 
 //-------------------------
 // Receiver PWM input
 //-------------------------
+#define PIOS_PWM_MAX_DEVS			1
 #define PIOS_PWM_NUM_INPUTS                     6
 
 //-------------------------
 // Receiver SPEKTRUM input
 //-------------------------
+#define PIOS_SPEKTRUM_MAX_DEVS			2
 #define PIOS_SPEKTRUM_NUM_INPUTS                12
 
 //-------------------------
@@ -230,6 +233,11 @@ extern uint32_t pios_com_telem_usb_id;
 #define PIOS_SERVO_UPDATE_HZ                    50
 #define PIOS_SERVOS_INITIAL_POSITION            0 /* dont want to start motors, have no pulse till settings loaded */
 
+//--------------------------
+// Timer controller settings
+//--------------------------
+#define PIOS_TIM_MAX_DEVS			3
+
 //-------------------------
 // GPIO
 //-------------------------

flight/PiOS/Boards/STM3210E_OP.h

@@ -185,16 +185,19 @@ extern uint32_t pios_com_aux_id;
 //-------------------------
 // Receiver PPM input
 //-------------------------
+#define PIOS_PPM_MAX_DEVS			1
 #define PIOS_PPM_NUM_INPUTS                     12
 
 //-------------------------
 // Receiver PWM input
 //-------------------------
+#define PIOS_PWM_MAX_DEVS			1
 #define PIOS_PWM_NUM_INPUTS                     8
 
 //-------------------------
 // Receiver SPEKTRUM input
 //-------------------------
+#define PIOS_SPEKTRUM_MAX_DEVS			1
 #define PIOS_SPEKTRUM_NUM_INPUTS                12
 
 //-------------------------
@@ -203,6 +206,11 @@ extern uint32_t pios_com_aux_id;
 #define PIOS_SERVO_UPDATE_HZ                    50
 #define PIOS_SERVOS_INITIAL_POSITION            0 /* dont want to start motors, have no pulse till settings loaded */
 
+//--------------------------
+// Timer controller settings
+//--------------------------
+#define PIOS_TIM_MAX_DEVS			3
+
 //-------------------------
 // ADC
 // PIOS_ADC_PinGet(0) = Temperature Sensor (On-board)

flight/PiOS/Common/pios_com.c

@@ -67,12 +67,12 @@ static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
 	return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_com_dev * PIOS_COM_alloc(void)
 {
 	struct pios_com_dev * com_dev;
 
-	com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev));
+	com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev));
 	if (!com_dev) return (NULL);
 
 	com_dev->magic = PIOS_COM_DEV_MAGIC;

flight/PiOS/Common/pios_flashfs_objlist.c

@@ -116,6 +116,19 @@ int32_t PIOS_FLASHFS_Init()
 	return 0;
 }
 
+/**
+ * @brief Erase the whole flash chip and create the file system
+ * @return 0 if successful, -1 if not
+ */
+int32_t PIOS_FLASHFS_Format() 
+{
+	if(PIOS_Flash_W25X_EraseChip() != 0)
+		return -1;
+	if(PIOS_FLASHFS_ClearObjectTableHeader() != 0)
+		return -1;
+	return 0;
+}
+
 /**
  * @brief Erase the headers for all objects in the flash chip
  * @return 0 if successful, -1 if not

flight/PiOS/Common/pios_gcsrcvr.c

@@ -0,0 +1,75 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup   PIOS_GCSRCVR GCS Receiver Input Functions
+ * @brief		Code to read the channels within the GCS Receiver UAVObject
+ * @{
+ *
+ * @file       pios_gcsrcvr.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      GCS Input functions (STM32 dependent)
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/* Project Includes */
+#include "pios.h"
+
+#if defined(PIOS_INCLUDE_GCSRCVR)
+
+#include "pios_gcsrcvr_priv.h"
+
+static GCSReceiverData gcsreceiverdata;
+
+/* Provide a RCVR driver */
+static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel);
+
+const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = {
+	.read = PIOS_GCSRCVR_Get,
+};
+
+static void gcsreceiver_updated(UAVObjEvent * ev)
+{
+	if (ev->obj == GCSReceiverHandle()) {
+		GCSReceiverGet(&gcsreceiverdata);
+	}
+}
+
+void PIOS_GCSRCVR_Init(void)
+{
+	/* Register uavobj callback */
+	GCSReceiverConnectCallback (gcsreceiver_updated);
+}
+
+static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel)
+{
+	if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) {
+		/* channel is out of range */
+		return -1;
+	}
+
+	return (gcsreceiverdata.Channel[channel]);
+}
+
+#endif	/* PIOS_INCLUDE_GCSRCVR */
+
+/** 
+  * @}
+  * @}
+  */

flight/PiOS/Common/pios_rcvr.c

@@ -20,12 +20,12 @@ static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev)
   return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC);
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
 {
   struct pios_rcvr_dev * rcvr_dev;
 
-  rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev));
+  rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev));
   if (!rcvr_dev) return (NULL);
 
   rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
@@ -76,8 +76,26 @@ out_fail:
   return(-1);
 }
 
+/**
+ * @brief Reads an input channel from the appropriate driver
+ * @param[in] rcvr_id driver to read from
+ * @param[in] channel channel to read
+ * @returns Unitless input value
+ *  @retval PIOS_RCVR_TIMEOUT indicates a failsafe or timeout from that channel
+ *  @retval PIOS_RCVR_INVALID invalid channel for this driver (usually out of range supported)
+ *  @retval PIOS_RCVR_NODRIVER driver was not initialized
+ */
 int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel)
 {
+	// Publicly facing API uses channel 1 for first channel
+	if(channel == 0)
+		return PIOS_RCVR_INVALID;
+	else
+		channel--;
+
+  if (rcvr_id == 0) 
+    return PIOS_RCVR_NODRIVER;
+
   struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id;
 
   if (!PIOS_RCVR_validate(rcvr_dev)) {

flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h

@@ -1025,12 +1025,12 @@ typedef struct
   */
 
 void TIM_DeInit(TIM_TypeDef* TIMx);
-void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);
+void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);
-void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
+void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
-void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
+void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
-void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
+void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
-void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct);
+void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct);
-void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct);
+void TIM_ICInit(TIM_TypeDef* TIMx, const TIM_ICInitTypeDef* TIM_ICInitStruct);
 void TIM_PWMIConfig(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct);
 void TIM_BDTRConfig(TIM_TypeDef* TIMx, TIM_BDTRInitTypeDef *TIM_BDTRInitStruct);
 void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct);

flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c

@@ -221,7 +221,7 @@ void TIM_DeInit(TIM_TypeDef* TIMx)
   *   structure that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct)
+void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct)
 {
   uint16_t tmpcr1 = 0;
 
@@ -274,7 +274,7 @@ void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseIn
   *   that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
+void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
 {
   uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
    
@@ -357,7 +357,7 @@ void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
   *   that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
+void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
 {
   uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
    
@@ -439,7 +439,7 @@ void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
   *   that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
+void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
 {
   uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
    
@@ -518,7 +518,7 @@ void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
   *   that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
+void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct)
 {
   uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0;
    
@@ -582,7 +582,7 @@ void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct)
   *   that contains the configuration information for the specified TIM peripheral.
   * @retval None
   */
-void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct)
+void TIM_ICInit(TIM_TypeDef* TIMx, const TIM_ICInitTypeDef* TIM_ICInitStruct)
 {
   /* Check the parameters */
   assert_param(IS_TIM_CHANNEL(TIM_ICInitStruct->TIM_Channel));  

flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld

@@ -18,16 +18,6 @@ SECTIONS
         *(.rodata .rodata* .gnu.linkonce.r.*)
     } > BL_FLASH
 
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } > BL_FLASH
-
     .ARM.extab :
     {
         *(.ARM.extab* .gnu.linkonce.armextab.*)

flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld

@@ -1,5 +1,5 @@
 /* This is the size of the stack for all FreeRTOS IRQs */
-_irq_stack_size = 0x180;
+_irq_stack_size = 0x1A0;
 /* This is the size of the stack for early init: life span is until scheduler starts */
 _init_stack_size = 0x100;
 
@@ -22,16 +22,6 @@ SECTIONS
         *(.rodata .rodata* .gnu.linkonce.r.*)
     } > FLASH
 
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } >FLASH
-
     /* module sections */
     .initcallmodule.init :
     {

flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld

@@ -207,16 +207,6 @@ SECTIONS
     } > BL_FLASH
  
     
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } > BL_FLASH
-
     /* the program code is stored in the .text section, which goes to Flash */
     .text :
     {

flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld

@@ -206,16 +206,6 @@ SECTIONS
         . = ALIGN(4);
     } > FLASH
  
-    
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } > FLASH
 
     /* the program code is stored in the .text section, which goes to Flash */
     .text :

flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld

@@ -179,17 +179,6 @@ SECTIONS
         . = ALIGN(4);
     } > BL_FLASH
  
-    
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } > BL_FLASH
-
     /* the program code is stored in the .text section, which goes to Flash */
     .text :
     {

flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld

@@ -1,7 +1,7 @@
 /* This is the size of the stack for early init and for all FreeRTOS IRQs */
-_irq_stack_size = 0x400;
+_irq_stack_size = 0x800;
 /* This is the size of the stack for early init: life span is until scheduler starts */
-_init_stack_size = 0x400;
+_init_stack_size = 0x800;
 
 /* Check valid alignment for VTOR */
 ASSERT(ORIGIN(FLASH) == ALIGN(ORIGIN(FLASH), 0x80), "Start of memory region flash not aligned for startup vector table");
@@ -181,17 +181,6 @@ SECTIONS
         *(.flashtext)            /* Startup code */
         . = ALIGN(4);
     } >FLASH
- 
-    
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } >FLASH
 
     /* module sections */
     .initcallmodule.init :

flight/PiOS/STM32F10x/pios_debug.c

@@ -34,39 +34,41 @@
 // Global variables
 const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED";
 
-#include <pios_servo_priv.h>
+#ifdef PIOS_ENABLE_DEBUG_PINS
-extern const struct pios_servo_channel pios_servo_channels[];
+static const struct pios_tim_channel * debug_channels;
-#define PIOS_SERVO_GPIO_PORT_1TO4 pios_servo_channels[0].port
+static uint8_t debug_num_channels;
-#define PIOS_SERVO_GPIO_PORT_5TO8 pios_servo_channels[4].port
+#endif	/* PIOS_ENABLE_DEBUG_PINS */
-#define PIOS_SERVO_GPIO_PIN_1  pios_servo_channels[0].pin
-#define PIOS_SERVO_GPIO_PIN_2  pios_servo_channels[1].pin
-#define PIOS_SERVO_GPIO_PIN_3  pios_servo_channels[2].pin
-#define PIOS_SERVO_GPIO_PIN_4  pios_servo_channels[3].pin
-#define PIOS_SERVO_GPIO_PIN_5  pios_servo_channels[4].pin
-#define PIOS_SERVO_GPIO_PIN_6  pios_servo_channels[5].pin
-#define PIOS_SERVO_GPIO_PIN_7  pios_servo_channels[6].pin
-#define PIOS_SERVO_GPIO_PIN_8  pios_servo_channels[7].pin
-/* Private Function Prototypes */
 
 /**
 * Initialise Debug-features
 */
-void PIOS_DEBUG_Init(void)
+void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
-	// Initialise Servo pins as standard output pins
+	PIOS_Assert(channels);
-	GPIO_InitTypeDef GPIO_InitStructure;
+	PIOS_Assert(num_channels);
-	GPIO_StructInit(&GPIO_InitStructure);
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
-	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
-	GPIO_InitStructure.GPIO_Pin = PIOS_SERVO_GPIO_PIN_1 | PIOS_SERVO_GPIO_PIN_2 | PIOS_SERVO_GPIO_PIN_3 | PIOS_SERVO_GPIO_PIN_4;
-	GPIO_Init(PIOS_SERVO_GPIO_PORT_1TO4, &GPIO_InitStructure);
-	GPIO_InitStructure.GPIO_Pin = PIOS_SERVO_GPIO_PIN_5 | PIOS_SERVO_GPIO_PIN_6 | PIOS_SERVO_GPIO_PIN_7 | PIOS_SERVO_GPIO_PIN_8;
-	GPIO_Init(PIOS_SERVO_GPIO_PORT_5TO8, &GPIO_InitStructure);
 
-	// Drive all pins low
+	/* Store away the GPIOs we've been given */
-	PIOS_SERVO_GPIO_PORT_1TO4->BRR = PIOS_SERVO_GPIO_PIN_1 | PIOS_SERVO_GPIO_PIN_2 | PIOS_SERVO_GPIO_PIN_3 | PIOS_SERVO_GPIO_PIN_4;
+	debug_channels = channels;
-	PIOS_SERVO_GPIO_PORT_5TO8->BRR = PIOS_SERVO_GPIO_PIN_5 | PIOS_SERVO_GPIO_PIN_6 | PIOS_SERVO_GPIO_PIN_7 | PIOS_SERVO_GPIO_PIN_8;
+	debug_num_channels = num_channels;
+
+	/* Configure the GPIOs we've been given */
+	for (uint8_t i = 0; i < num_channels; i++) {
+		const struct pios_tim_channel * chan = &channels[i];
+
+		// Initialise pins as standard output pins
+		GPIO_InitTypeDef GPIO_InitStructure;
+		GPIO_StructInit(&GPIO_InitStructure);
+		GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+		GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+		GPIO_InitStructure.GPIO_Pin = chan->init->GPIO_Pin;
+
+		/* Initialize the GPIO */
+		GPIO_Init(chan->init->port, &GPIO_InitStructure);
+
+		/* Set the pin low */
+		GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
+	}
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
@@ -74,14 +76,17 @@ void PIOS_DEBUG_Init(void)
 * Set debug-pin high
 * \param pin 0 for S1 output
 */
-void PIOS_DEBUG_PinHigh(uint8_t Pin)
+void PIOS_DEBUG_PinHigh(uint8_t pin)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
-	if (Pin < 4) {
+	if (!debug_channels || pin >= debug_num_channels) {
-		PIOS_SERVO_GPIO_PORT_1TO4->BSRR = (PIOS_SERVO_GPIO_PIN_1 << Pin);
+		return;
-	} else if (Pin <= 7) {
-		PIOS_SERVO_GPIO_PORT_5TO8->BSRR = (PIOS_SERVO_GPIO_PIN_5 << (Pin - 4));
 	}
+
+	const struct pios_tim_channel * chan = &debug_channels[pin];
+
+	GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_Set);
+
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
@@ -89,14 +94,17 @@ void PIOS_DEBUG_PinHigh(uint8_t Pin)
 * Set debug-pin low
 * \param pin 0 for S1 output
 */
-void PIOS_DEBUG_PinLow(uint8_t Pin)
+void PIOS_DEBUG_PinLow(uint8_t pin)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
-	if (Pin < 4) {
+	if (!debug_channels || pin >= debug_num_channels) {
-		PIOS_SERVO_GPIO_PORT_1TO4->BRR = (PIOS_SERVO_GPIO_PIN_1 << Pin);
+		return;
-	} else if (Pin <= 7) {
-		PIOS_SERVO_GPIO_PORT_5TO8->BRR = (PIOS_SERVO_GPIO_PIN_5 << (Pin - 4));
 	}
+
+	const struct pios_tim_channel * chan = &debug_channels[pin];
+
+	GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET);
+
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 
@@ -104,11 +112,22 @@ void PIOS_DEBUG_PinLow(uint8_t Pin)
 void PIOS_DEBUG_PinValue8Bit(uint8_t value)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
+	if (!debug_channels) {
+		return;
+	}
+
 	uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6)   ) | ((value & 0x0F)<<6);
 	uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4));
+
 	PIOS_IRQ_Disable();
-	PIOS_SERVO_GPIO_PORT_1TO4->BSRR = bsrr_l;
+
-	PIOS_SERVO_GPIO_PORT_5TO8->BSRR = bsrr_h;
+	/* 
+	 * This is sketchy since it assumes a particular ordering
+	 * and bitwise layout of the channels provided to the debug code.
+	 */
+	debug_channels[0].init.port->BSRR = bsrr_l;
+	debug_channels[4].init.port->BSRR = bsrr_h;
+
 	PIOS_IRQ_Enable();
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
@@ -116,8 +135,16 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value)
 void PIOS_DEBUG_PinValue4BitL(uint8_t value)
 {
 #ifdef PIOS_ENABLE_DEBUG_PINS
+	if (!debug_channels) {
+		return;
+	}
+
+	/* 
+	 * This is sketchy since it assumes a particular ordering
+	 * and bitwise layout of the channels provided to the debug code.
+	 */
 	uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6);
-	PIOS_SERVO_GPIO_PORT_1TO4->BSRR = bsrr_l;
+	debug_channels[0].init.port->BSRR = bsrr_l;
 #endif // PIOS_ENABLE_DEBUG_PINS
 }
 

flight/PiOS/STM32F10x/pios_i2c.c

@@ -823,12 +823,12 @@ static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter)
 	return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC);
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
-static struct pios_i2c_dev * PIOS_I2C_alloc(void)
+static struct pios_i2c_adapter * PIOS_I2C_alloc(void)
 {
-	struct pios_i2c_dev * i2c_adapter;
+	struct pios_i2c_adapter * i2c_adapter;
 
-	i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter));
+	i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter));
 	if (!i2c_adapter) return(NULL);
 
 	i2c_adapter->magic = PIOS_I2C_DEV_MAGIC;

flight/PiOS/STM32F10x/pios_ppm.c

@@ -47,108 +47,139 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
 #define PIOS_PPM_IN_MIN_SYNC_PULSE_US		3800	// microseconds
 #define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US	750	// microseconds
 #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US	2250   // microseconds
-#define PIOS_PPM_INPUT_INVALID			0
 
 /* Local Variables */
 static TIM_ICInitTypeDef TIM_ICInitStructure;
-static uint8_t PulseIndex;
-static uint32_t PreviousTime;
-static uint32_t CurrentTime;
-static uint32_t DeltaTime;
-static uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS];
-static uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS];
-static uint32_t LargeCounter;
-static int8_t NumChannels;
-static int8_t NumChannelsPrevFrame;
-static uint8_t NumChannelCounter;
-
-static uint8_t supv_timer = 0;
-static bool Tracking;
-static bool Fresh;
 
 static void PIOS_PPM_Supervisor(uint32_t ppm_id);
 
-void PIOS_PPM_Init(void)
+enum pios_ppm_dev_magic {
+	PIOS_PPM_DEV_MAGIC = 0xee014d8b,
+};
+
+struct pios_ppm_dev {
+	enum pios_ppm_dev_magic     magic;
+	const struct pios_ppm_cfg * cfg;
+
+	uint8_t PulseIndex;
+	uint32_t PreviousTime;
+	uint32_t CurrentTime;
+	uint32_t DeltaTime;
+	uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS];
+	uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS];
+	uint32_t LargeCounter;
+	int8_t NumChannels;
+	int8_t NumChannelsPrevFrame;
+	uint8_t NumChannelCounter;
+
+	uint8_t supv_timer;
+	bool Tracking;
+	bool Fresh;
+};
+
+static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev)
 {
-	/* Flush counter variables */
+	return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC);
-	int32_t i;
+}
 
-	PulseIndex = 0;
+#if defined(PIOS_INCLUDE_FREERTOS)
-	PreviousTime = 0;
+static struct pios_ppm_dev * PIOS_PPM_alloc(void)
-	CurrentTime = 0;
+{
-	DeltaTime = 0;
+	struct pios_ppm_dev * ppm_dev;
-	LargeCounter = 0;
-	NumChannels = -1;
-	NumChannelsPrevFrame = -1;
-	NumChannelCounter = 0;
-	Tracking = FALSE;
-	Fresh = FALSE;
 
-	for (i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
+	ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev));
-		CaptureValue[i] = 0;
+	if (!ppm_dev) return(NULL);
-		CaptureValueNewFrame[i] = 0;
+
+	ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
+	return(ppm_dev);
+}
+#else
+static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS];
+static uint8_t pios_ppm_num_devs;
+static struct pios_ppm_dev * PIOS_PPM_alloc(void)
+{
+	struct pios_ppm_dev * ppm_dev;
+
+	if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) {
+		return (NULL);
 	}
 
-	NVIC_InitTypeDef NVIC_InitStructure = pios_ppm_cfg.irq.init;
+	ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++];
+	ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
 
-	/* Enable appropriate clock to timer module */
+	return (ppm_dev);
-	switch((int32_t) pios_ppm_cfg.timer) {
+}
-		case (int32_t)TIM1:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
-			RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
-			break;
-		case (int32_t)TIM2:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
-			break;
-		case (int32_t)TIM3:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
-			break;
-		case (int32_t)TIM4:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-			break;
-#ifdef STM32F10X_HD
-		case (int32_t)TIM5:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
-			break;
-		case (int32_t)TIM6:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
-			break;
-		case (int32_t)TIM7:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
-			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
-			break;
-		case (int32_t)TIM8:
-			NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn;
-			RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
-			break;
 #endif
+
+static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+const static struct pios_tim_callbacks tim_callbacks = {
+	.overflow = PIOS_PPM_tim_overflow_cb,
+	.edge     = PIOS_PPM_tim_edge_cb,
+};
+
+extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg)
+{
+	PIOS_DEBUG_Assert(ppm_id);
+	PIOS_DEBUG_Assert(cfg);
+
+	struct pios_ppm_dev * ppm_dev;
+
+	ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc();
+	if (!ppm_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	ppm_dev->cfg = cfg;
+
+	/* Set up the state variables */
+	ppm_dev->PulseIndex = 0;
+	ppm_dev->PreviousTime = 0;
+	ppm_dev->CurrentTime = 0;
+	ppm_dev->DeltaTime = 0;
+	ppm_dev->LargeCounter = 0;
+	ppm_dev->NumChannels = -1;
+	ppm_dev->NumChannelsPrevFrame = -1;
+	ppm_dev->NumChannelCounter = 0;
+	ppm_dev->Tracking = FALSE;
+	ppm_dev->Fresh = FALSE;
+
+	for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
+		/* Flush counter variables */
+		ppm_dev->CaptureValue[i] = 0;
+		ppm_dev->CaptureValueNewFrame[i] = 0;
+
 	}
-	/* Enable timer interrupts */
-	NVIC_Init(&NVIC_InitStructure);
 
-	/* Configure input pins */
+	uint32_t tim_id;
-	GPIO_InitTypeDef GPIO_InitStructure = pios_ppm_cfg.gpio_init;
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) {
-	GPIO_Init(pios_ppm_cfg.port, &GPIO_InitStructure);
+		return -1;
+	}
 
-	/* Configure timer for input capture */
+	/* Configure the channels to be in capture/compare mode */
-	TIM_ICInitStructure = pios_ppm_cfg.tim_ic_init;
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-	TIM_ICInit(pios_ppm_cfg.timer, &TIM_ICInitStructure);
+		const struct pios_tim_channel * chan = &cfg->channels[i];
 
-	/* Configure timer clocks */
+		/* Configure timer for input capture */
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_ppm_cfg.tim_base_init;
+		TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
-	TIM_InternalClockConfig(pios_ppm_cfg.timer);
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-	TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure);
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 
-	/* Enable the Capture Compare Interrupt Request */
+		/* Enable the Capture Compare Interrupt Request */
-	TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr | TIM_IT_Update, ENABLE);
+		switch (chan->timer_chan) {
-
+		case TIM_Channel_1:
-	/* Enable timers */
+			TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE);
-	TIM_Cmd(pios_ppm_cfg.timer, ENABLE);
+			break;
+		case TIM_Channel_2:
+			TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE);
+			break;
+		case TIM_Channel_3:
+			TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE);
+			break;
+		case TIM_Channel_4:
+			TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE);
+			break;
+		}
+	}
 
 	/* Setup local variable which stays in this scope */
 	/* Doing this here and using a local variable saves doing it in the ISR */
@@ -156,9 +187,16 @@ void PIOS_PPM_Init(void)
 	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 	TIM_ICInitStructure.TIM_ICFilter = 0x0;
 
-	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, 0)) {
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
 		PIOS_DEBUG_Assert(0);
 	}
+
+	*ppm_id = (uint32_t)ppm_dev;
+
+	return(0);
+
+out_fail:
+	return(-1);
 }
 
 /**
@@ -169,142 +207,146 @@ void PIOS_PPM_Init(void)
 */
 static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
 {
-	/* Return error if channel not available */
+	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id;
-	if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
+
-		return -1;
+	if (!PIOS_PPM_validate(ppm_dev)) {
+		/* Invalid device specified */
+		return PIOS_RCVR_INVALID;
 	}
-	return CaptureValue[channel];
+
+	if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
+		/* Channel out of range */
+		return PIOS_RCVR_INVALID;
+	}
+	return ppm_dev->CaptureValue[channel];
 }
 
-/**
+static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
-* Handle TIMx global interrupt request
-* Some work and testing still needed, need to detect start of frame and decode pulses
-*
-*/
-void PIOS_PPM_irq_handler(void)
 {
-	/* Timer Overflow Interrupt
+	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
-	 * The time between timer overflows must be greater than the PPM
-	 * frame period. If a full frame has not decoded in the between timer
-	 * overflows then capture values should be cleared.
-	 */
 
-	if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) {
+	if (!PIOS_PPM_validate(ppm_dev)) {
-		/* Clear TIMx overflow interrupt pending bit */
+		/* Invalid device specified */
-		TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update);
+		return;
-
-		/* If sharing a timer with a servo output the ARR register will
-		   be set according to the PWM period. When timer reaches the
-		   ARR value a timer overflow interrupt will fire. We use the
-		   interrupt accumulate a 32-bit timer. */
-		LargeCounter = LargeCounter + pios_ppm_cfg.timer->ARR;
 	}
 
-	/* Signal edge interrupt */
+	ppm_dev->LargeCounter += count;
-	if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) {
-		PreviousTime = CurrentTime;
 
-		switch((int32_t) pios_ppm_cfg.ccr) {
+	return;
-			case (int32_t)TIM_IT_CC1:
+}
-				CurrentTime = TIM_GetCapture1(pios_ppm_cfg.timer);
+
-				break;
+
-			case (int32_t)TIM_IT_CC2:
+static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
-				CurrentTime = TIM_GetCapture2(pios_ppm_cfg.timer);
+{
-				break;
+	/* Recover our device context */
-			case (int32_t)TIM_IT_CC3:
+	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context;
-				CurrentTime = TIM_GetCapture3(pios_ppm_cfg.timer);
+
-				break;
+	if (!PIOS_PPM_validate(ppm_dev)) {
-			case (int32_t)TIM_IT_CC4:
+		/* Invalid device specified */
-				CurrentTime = TIM_GetCapture4(pios_ppm_cfg.timer);
+		return;
-				break;
+	}
+
+	if (chan_idx >= ppm_dev->cfg->num_channels) {
+		/* Channel out of range */
+		return;
+	}
+
+	/* Shift the last measurement out */
+	ppm_dev->PreviousTime = ppm_dev->CurrentTime;
+
+	/* Grab the new count */
+	ppm_dev->CurrentTime = count;
+
+	/* Convert to 32-bit timer result */
+	ppm_dev->CurrentTime += ppm_dev->LargeCounter;
+
+	/* Capture computation */		
+	ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime;
+
+	ppm_dev->PreviousTime = ppm_dev->CurrentTime;
+
+	/* Sync pulse detection */
+	if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) {
+		if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame
+			&& ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS
+			&& ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS)
+		{
+			/* If we see n simultaneous frames of the same
+			   number of channels we save it as our frame size */
+			if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT)
+				ppm_dev->NumChannelCounter++;
+			else
+				ppm_dev->NumChannels = ppm_dev->PulseIndex;
+		} else {
+			ppm_dev->NumChannelCounter = 0;
 		}
 
-		/* Clear TIMx Capture compare interrupt pending bit */
+		/* Check if the last frame was well formed */
-		TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr);
+		if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
-
+			/* The last frame was well formed */
-		/* Convert to 32-bit timer result */
+			for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) {
-		CurrentTime = CurrentTime + LargeCounter;
+				ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
-
-		/* Capture computation */		
-		DeltaTime = CurrentTime - PreviousTime;
-
-		PreviousTime = CurrentTime;
-
-		/* Sync pulse detection */
-		if (DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) {
-			if (PulseIndex == NumChannelsPrevFrame
-			 && PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS
-			 && PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS)
-			{
-				/* If we see n simultaneous frames of the same
-				 number of channels we save it as our frame size */
-				if (NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT)
-					NumChannelCounter++;
-				else
-					NumChannels = PulseIndex;
-			} else {
-				NumChannelCounter = 0;
 			}
-
+			for (uint32_t i = ppm_dev->NumChannels;
-			/* Check if the last frame was well formed */
+			     i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
-			if (PulseIndex == NumChannels && Tracking) {
+				ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
-				/* The last frame was well formed */
-				for (uint32_t i = 0; i < NumChannels; i++) {
-					CaptureValue[i] = CaptureValueNewFrame[i];
-				}
-				for (uint32_t i = NumChannels;
-				     i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
-					CaptureValue[i] = PIOS_PPM_INPUT_INVALID;
-				}
 			}
+		}
 
-			Fresh = TRUE;
+		ppm_dev->Fresh = TRUE;
-			Tracking = TRUE;
+		ppm_dev->Tracking = TRUE;
-			NumChannelsPrevFrame = PulseIndex;
+		ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex;
-			PulseIndex = 0;
+		ppm_dev->PulseIndex = 0;
 
-			/* We rely on the supervisor to set CaptureValue to invalid
+		/* We rely on the supervisor to set CaptureValue to invalid
-			 if no valid frame is found otherwise we ride over it */
+		   if no valid frame is found otherwise we ride over it */
 
-		} else if (Tracking) {
+	} else if (ppm_dev->Tracking) {
-			/* Valid pulse duration 0.75 to 2.5 ms*/
+		/* Valid pulse duration 0.75 to 2.5 ms*/
-			if (DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US
+		if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US
-			    && DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US
+			&& ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US
-			    && PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) {
+			&& ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) {
-
+			
-				CaptureValueNewFrame[PulseIndex] = DeltaTime;
+			ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime;
-				PulseIndex++;
+			ppm_dev->PulseIndex++;
-			} else {
+		} else {
-				/* Not a valid pulse duration */
+			/* Not a valid pulse duration */
-				Tracking = FALSE;
+			ppm_dev->Tracking = FALSE;
-				for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
+			for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
-					CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID;
+				ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
-				}
 			}
 		}
 	}
 }
 
 static void PIOS_PPM_Supervisor(uint32_t ppm_id) {
+	/* Recover our device context */
+	struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id;
+
+	if (!PIOS_PPM_validate(ppm_dev)) {
+		/* Invalid device specified */
+		return;
+	}
+
 	/* 
 	 * RTC runs at 625Hz so divide down the base rate so
 	 * that this loop runs at 25Hz.
 	 */
-	if(++supv_timer < 25) {
+	if(++(ppm_dev->supv_timer) < 25) {
 		return;
 	}
-	supv_timer = 0;
+	ppm_dev->supv_timer = 0;
 
-	if (!Fresh) {
+	if (!ppm_dev->Fresh) {
-		Tracking = FALSE;
+		ppm_dev->Tracking = FALSE;
 
 		for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) {
-			CaptureValue[i] = PIOS_PPM_INPUT_INVALID;
+			ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
-			CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID;
+			ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
 		}
 	}
 
-	Fresh = FALSE;
+	ppm_dev->Fresh = FALSE;
 }
 
 #endif

flight/PiOS/STM32F10x/pios_pwm.c

@@ -42,96 +42,131 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
 };
 
 /* Local Variables */
-static uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
+/* 100 ms timeout without updates on channels */
-static uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
+const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
-static uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
-static uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
 
-static uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
+enum pios_pwm_dev_magic {
+	PIOS_PWM_DEV_MAGIC = 0xab30293c,
+};
+
+struct pios_pwm_dev {
+	enum pios_pwm_dev_magic     magic;
+	const struct pios_pwm_cfg * cfg;
+
+	uint8_t CaptureState[PIOS_PWM_NUM_INPUTS];
+	uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
+	uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
+	uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
+	uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
+	uint32_t us_since_update[PIOS_PWM_NUM_INPUTS];
+};
+
+static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev)
+{
+	return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC);
+}
+
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_pwm_dev * PIOS_PWM_alloc(void)
+{
+	struct pios_pwm_dev * pwm_dev;
+
+	pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev));
+	if (!pwm_dev) return(NULL);
+
+	pwm_dev->magic = PIOS_PWM_DEV_MAGIC;
+	return(pwm_dev);
+}
+#else
+static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS];
+static uint8_t pios_pwm_num_devs;
+static struct pios_pwm_dev * PIOS_PWM_alloc(void)
+{
+	struct pios_pwm_dev * pwm_dev;
+
+	if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) {
+		return (NULL);
+	}
+
+	pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++];
+	pwm_dev->magic = PIOS_PWM_DEV_MAGIC;
+
+	return (pwm_dev);
+}
+#endif
+
+static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
+const static struct pios_tim_callbacks tim_callbacks = {
+	.overflow = PIOS_PWM_tim_overflow_cb,
+	.edge     = PIOS_PWM_tim_edge_cb,
+};
 
 /**
 * Initialises all the pins
 */
-void PIOS_PWM_Init(void)
+int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg)
 {
-	for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) {
+	PIOS_DEBUG_Assert(pwm_id);
-		/* Flush counter variables */
+	PIOS_DEBUG_Assert(cfg);
-		CaptureState[i] = 0;
+
-		RiseValue[i] = 0;
+	struct pios_pwm_dev * pwm_dev;
-		FallValue[i] = 0;
+
-		CaptureValue[i] = 0;
+	pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc();
-		
+	if (!pwm_dev) goto out_fail;
-		NVIC_InitTypeDef NVIC_InitStructure = pios_pwm_cfg.irq.init;
+
-		GPIO_InitTypeDef GPIO_InitStructure = pios_pwm_cfg.gpio_init;
+	/* Bind the configuration to the device instance */
-		TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_pwm_cfg.tim_base_init;
+	pwm_dev->cfg = cfg;
-		TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
+
-		
+	for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
-		struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
+		/* Flush counter variables */
-		
+		pwm_dev->CaptureState[i] = 0;
-		/* Enable appropriate clock to timer module */
+		pwm_dev->RiseValue[i] = 0;
-		switch((int32_t) channel.timer) {
+		pwm_dev->FallValue[i] = 0;
-			case (int32_t)TIM1:
+		pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
-				NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
+	}
-				RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
+
-				break;
+	uint32_t tim_id;
-			case (int32_t)TIM2:
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) {
-				NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
+		return -1;
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
+	}
-				break;
+
-			case (int32_t)TIM3:
+	/* Configure the channels to be in capture/compare mode */
-				NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+		const struct pios_tim_channel * chan = &cfg->channels[i];
-				break;
-			case (int32_t)TIM4:
-				NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-				break;
-#ifdef STM32F10X_HD
-				
-			case (int32_t)TIM5:
-				NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
-				break;
-			case (int32_t)TIM6:
-				NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
-				break;
-			case (int32_t)TIM7:
-				NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn;
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
-				break;
-			case (int32_t)TIM8:
-				NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn;
-				RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
-				break;
-#endif
-		}
-		NVIC_Init(&NVIC_InitStructure);
 
-		/* Enable GPIO */
-		GPIO_InitStructure.GPIO_Pin = channel.pin;
-		GPIO_Init(channel.port, &GPIO_InitStructure);
-		
 		/* Configure timer for input capture */
-		TIM_ICInitStructure.TIM_Channel = channel.channel;
+		TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
-		TIM_ICInit(channel.timer, &TIM_ICInitStructure);
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
-		
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
-		/* Configure timer clocks */
-		TIM_InternalClockConfig(channel.timer);
-		if(channel.timer->PSC != ((PIOS_MASTER_CLOCK / 1000000) - 1)) 
-			TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure);		
 		
 		/* Enable the Capture Compare Interrupt Request */
-		TIM_ITConfig(channel.timer, channel.ccr, ENABLE);
+		switch (chan->timer_chan) {
+		case TIM_Channel_1:
+			TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE);
+			break;
+		case TIM_Channel_2:
+			TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE);
+			break;
+		case TIM_Channel_3:
+			TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE);
+			break;
+		case TIM_Channel_4:
+			TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE);
+			break;
+		}
+
+		// Need the update event for that timer to detect timeouts
+		TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE);
 
-		/* Enable timers */
-		TIM_Cmd(channel.timer, ENABLE);
 	}
 
-	if(pios_pwm_cfg.remap) {
+	*pwm_id = (uint32_t) pwm_dev;
-		/* Warning, I don't think this will work for multiple remaps at once */
+
-		GPIO_PinRemapConfig(pios_pwm_cfg.remap, ENABLE);
+	return (0);
-	}
+
+out_fail:
+	return (-1);
 }
 
 /**
@@ -142,75 +177,101 @@ void PIOS_PWM_Init(void)
 */
 static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
 {
-	/* Return error if channel not available */
+	struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id;
-	if (channel >= pios_pwm_cfg.num_channels) {
+
-		return -1;
+	if (!PIOS_PWM_validate(pwm_dev)) {
+		/* Invalid device specified */
+		return PIOS_RCVR_INVALID;
 	}
-	return CaptureValue[channel];
+
+	if (channel >= PIOS_PWM_NUM_INPUTS) {
+		/* Channel out of range */
+		return PIOS_RCVR_INVALID;
+	}
+	return pwm_dev->CaptureValue[channel];
 }
 
-void PIOS_PWM_irq_handler(TIM_TypeDef * timer)
+static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
 {
-	uint16_t val = 0;
+	struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
-	for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) {
+
-		struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
+	if (!PIOS_PWM_validate(pwm_dev)) {
-		if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) {
+		/* Invalid device specified */
-			
+		return;
-			TIM_ClearITPendingBit(channel.timer, channel.ccr);
-			
-			switch(channel.channel) {
-				case TIM_Channel_1:
-					val = TIM_GetCapture1(channel.timer);
-					break;
-				case TIM_Channel_2:
-					val = TIM_GetCapture2(channel.timer);
-					break;
-				case TIM_Channel_3:
-					val = TIM_GetCapture3(channel.timer);
-					break;
-				case TIM_Channel_4:
-					val = TIM_GetCapture4(channel.timer);
-					break;					
-			}
-			
-			if (CaptureState[i] == 0) {
-				RiseValue[i] = val; 
-			} else {
-				FallValue[i] = val;
-			}
-			
-			// flip state machine and capture value here
-			/* Simple rise or fall state machine */
-			TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
-			if (CaptureState[i] == 0) {
-				/* Switch states */
-				CaptureState[i] = 1;
-				
-				/* Switch polarity of input capture */
-				TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
-				TIM_ICInitStructure.TIM_Channel = channel.channel;
-				TIM_ICInit(channel.timer, &TIM_ICInitStructure);				
-			} else {
-				/* Capture computation */
-				if (FallValue[i] > RiseValue[i]) {
-					CaptureValue[i] = (FallValue[i] - RiseValue[i]);
-				} else {
-					CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]);
-				}
-				
-				/* Switch states */
-				CaptureState[i] = 0;
-				
-				/* Increase supervisor counter */
-				CapCounter[i]++;
-				
-				/* Switch polarity of input capture */
-				TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
-				TIM_ICInitStructure.TIM_Channel = channel.channel;
-				TIM_ICInit(channel.timer, &TIM_ICInitStructure);				
-			}
-		}		
 	}
+
+	if (channel >= pwm_dev->cfg->num_channels) {
+		/* Channel out of range */
+		return;
+	}
+
+	pwm_dev->us_since_update[channel] += count;
+	if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
+		pwm_dev->CaptureState[channel] = 0;
+		pwm_dev->RiseValue[channel] = 0;
+		pwm_dev->FallValue[channel] = 0;
+		pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT;
+		pwm_dev->us_since_update[channel] = 0;
+	}
+
+	return;
+}
+
+static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
+{
+	/* Recover our device context */
+	struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context;
+
+	if (!PIOS_PWM_validate(pwm_dev)) {
+		/* Invalid device specified */
+		return;
+	}
+
+	if (chan_idx >= pwm_dev->cfg->num_channels) {
+		/* Channel out of range */
+		return;
+	}
+
+	const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx];
+
+	if (pwm_dev->CaptureState[chan_idx] == 0) {
+		pwm_dev->RiseValue[chan_idx] = count;
+		pwm_dev->us_since_update[chan_idx] = 0;
+	} else {
+		pwm_dev->FallValue[chan_idx] = count;
+	}
+			
+	// flip state machine and capture value here
+	/* Simple rise or fall state machine */
+	TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init;
+	if (pwm_dev->CaptureState[chan_idx] == 0) {
+		/* Switch states */
+		pwm_dev->CaptureState[chan_idx] = 1;
+
+		/* Switch polarity of input capture */
+		TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
+	} else {
+		/* Capture computation */
+		if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) {
+			pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
+		} else {
+			pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]);
+		}
+
+		/* Switch states */
+		pwm_dev->CaptureState[chan_idx] = 0;
+
+		/* Increase supervisor counter */
+		pwm_dev->CapCounter[chan_idx]++;
+
+		/* Switch polarity of input capture */
+		TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+		TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
+		TIM_ICInit(chan->timer, &TIM_ICInitStructure);
+	}
+	
 }
 
 #endif

flight/PiOS/STM32F10x/pios_sbus.c

@@ -34,8 +34,6 @@
 
 #if defined(PIOS_INCLUDE_SBUS)
 
-/* Global Variables */
-
 /* Provide a RCVR driver */
 static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel);
 
@@ -59,27 +57,23 @@ static void PIOS_SBUS_Supervisor(uint32_t sbus_id);
 static void reset_channels(void)
 {
 	for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) {
-		channel_data[i] = 0;
+		channel_data[i] = PIOS_RCVR_TIMEOUT;
 	}
 }
 
 /**
  * unroll_channels() function computes channel_data[] from received_data[]
- * For efficiency it unrolls first 8 channels without loops. If other
+ * For efficiency it unrolls first 8 channels without loops and does the
- * 8 channels are needed they can be unrolled using the same code
+ * same for other 8 channels. Also 2 discrete channels will be set.
- * starting from s[11] instead of s[0]. Two extra digital channels are
- * accessible using (s[22] & SBUS_FLAG_DGx) logical expressions.
  */
 static void unroll_channels(void)
 {
 	uint8_t *s = received_data;
 	uint16_t *d = channel_data;
 
-#if (SBUS_NUMBER_OF_CHANNELS != 8)
-#error Current S.Bus code unrolls only first 8 channels
-#endif
-
 #define F(v,s) ((v) >> s) & 0x7ff
+
+	/* unroll channels 1-8 */
 	*d++ = F(s[0] | s[1] << 8, 0);
 	*d++ = F(s[1] | s[2] << 8, 3);
 	*d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6);
@@ -88,6 +82,20 @@ static void unroll_channels(void)
 	*d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7);
 	*d++ = F(s[8] | s[9] << 8, 2);
 	*d++ = F(s[9] | s[10] << 8, 5);
+
+	/* unroll channels 9-16 */
+	*d++ = F(s[11] | s[12] << 8, 0);
+	*d++ = F(s[12] | s[13] << 8, 3);
+	*d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6);
+	*d++ = F(s[15] | s[16] << 8, 1);
+	*d++ = F(s[16] | s[17] << 8, 4);
+	*d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7);
+	*d++ = F(s[19] | s[20] << 8, 2);
+	*d++ = F(s[20] | s[21] << 8, 5);
+
+	/* unroll discrete channels 17 and 18 */
+	*d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN;
+	*d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN;
 }
 
 /**
@@ -183,7 +191,7 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel)
 {
 	/* return error if channel is not available */
 	if (channel >= SBUS_NUMBER_OF_CHANNELS) {
-		return -1;
+		return PIOS_RCVR_INVALID;
 	}
 	return channel_data[channel];
 }

flight/PiOS/STM32F10x/pios_servo.c

@@ -31,97 +31,55 @@
 /* Project Includes */
 #include "pios.h"
 #include "pios_servo_priv.h"
+#include "pios_tim_priv.h"
 
 /* Private Function Prototypes */
 
+static const struct pios_servo_cfg * servo_cfg;
+
 /**
 * Initialise Servos
 */
-void PIOS_Servo_Init(void)
+int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg)
 {
-#ifndef PIOS_ENABLE_DEBUG_PINS
+	uint32_t tim_id;
-#if defined(PIOS_INCLUDE_SERVO)
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) {
+		return -1;
+	}
 
+	/* Store away the requested configuration */
+	servo_cfg = cfg;
 
-	for (uint8_t i = 0; i < pios_servo_cfg.num_channels; i++) {
+	/* Configure the channels to be in output compare mode */
-		GPIO_InitTypeDef GPIO_InitStructure = pios_servo_cfg.gpio_init;
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-		TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init;
+		const struct pios_tim_channel * chan = &cfg->channels[i];
-		TIM_OCInitTypeDef TIM_OCInitStructure = pios_servo_cfg.tim_oc_init;
-
-		struct pios_servo_channel channel = pios_servo_cfg.channels[i];
-
-		/* Enable appropriate clock to timer module */
-		switch((int32_t) channel.timer) {
-			case (int32_t)TIM1:
-				RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
-				break;
-			case (int32_t)TIM2:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
-				break;
-			case (int32_t)TIM3:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
-				break;
-			case (int32_t)TIM4:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-				break;
-			case (int32_t)TIM5:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
-				break;
-			case (int32_t)TIM6:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
-				break;
-			case (int32_t)TIM7:
-				RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
-				break;
-			case (int32_t)TIM8:
-				RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
-				break;
-		}
-
-		/* Enable GPIO */
-		GPIO_InitStructure.GPIO_Pin = channel.pin;
-		GPIO_Init(channel.port, &GPIO_InitStructure);
-
-		/* Enable time base */
-		TIM_TimeBaseInit(channel.timer,  &TIM_TimeBaseStructure);
-
-		channel.timer->PSC = (PIOS_MASTER_CLOCK / 1000000) - 1;
 
 		/* Set up for output compare function */
-		switch(channel.channel) {
+		switch(chan->timer_chan) {
 			case TIM_Channel_1:
-				TIM_OC1Init(channel.timer, &TIM_OCInitStructure);
+				TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
-				TIM_OC1PreloadConfig(channel.timer, TIM_OCPreload_Enable);
+				TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 				break;
 			case TIM_Channel_2:
-				TIM_OC2Init(channel.timer, &TIM_OCInitStructure);
+				TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
-				TIM_OC2PreloadConfig(channel.timer, TIM_OCPreload_Enable);
+				TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 				break;
 			case TIM_Channel_3:
-				TIM_OC3Init(channel.timer, &TIM_OCInitStructure);
+				TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
-				TIM_OC3PreloadConfig(channel.timer, TIM_OCPreload_Enable);
+				TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 				break;
 			case TIM_Channel_4:
-				TIM_OC4Init(channel.timer, &TIM_OCInitStructure);
+				TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
-				TIM_OC4PreloadConfig(channel.timer, TIM_OCPreload_Enable);
+				TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
 				break;
 		}
 
-		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
+		TIM_ARRPreloadConfig(chan->timer, ENABLE);
-		TIM_ARRPreloadConfig(channel.timer, ENABLE);
+		TIM_CtrlPWMOutputs(chan->timer, ENABLE);
-		TIM_CtrlPWMOutputs(channel.timer, ENABLE);
+		TIM_Cmd(chan->timer, ENABLE);
-		TIM_Cmd(channel.timer, ENABLE);
-
 	}
 
-	if(pios_servo_cfg.remap) {
+	return 0;
-		/* Warning, I don't think this will work for multiple remaps at once */
-		GPIO_PinRemapConfig(pios_servo_cfg.remap, ENABLE);
-	}
-
-
-#endif // PIOS_INCLUDE_SERVO
-#endif // PIOS_ENABLE_DEBUG_PINS
 }
 
 /**
@@ -131,31 +89,31 @@ void PIOS_Servo_Init(void)
 */
 void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks)
 {
-#ifndef PIOS_ENABLE_DEBUG_PINS
+	if (!servo_cfg) {
-#if defined(PIOS_INCLUDE_SERVO)
+		return;
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init;
+	}
+
+	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
 	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 	TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1;
 
 	uint8_t set = 0;
 
-	for(uint8_t i = 0; (i < pios_servo_cfg.num_channels) && (set < banks); i++) {
+	for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
 		bool new = true;
-		struct pios_servo_channel channel = pios_servo_cfg.channels[i];
+		const struct pios_tim_channel * chan = &servo_cfg->channels[i];
 
 		/* See if any previous channels use that same timer */
 		for(uint8_t j = 0; (j < i) && new; j++)
-			new &= channel.timer != pios_servo_cfg.channels[j].timer;
+			new &= chan->timer != servo_cfg->channels[j].timer;
 
 		if(new) {
 			TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
-			TIM_TimeBaseInit(channel.timer,  &TIM_TimeBaseStructure);
+			TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
 			set++;
 		}
 	}
-#endif // PIOS_INCLUDE_SERVO
-#endif // PIOS_ENABLE_DEBUG_PINS
 }
 
 /**
@@ -163,29 +121,27 @@ void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks)
 * \param[in] Servo Servo number (0-7)
 * \param[in] Position Servo position in microseconds
 */
-void PIOS_Servo_Set(uint8_t Servo, uint16_t Position)
+void PIOS_Servo_Set(uint8_t servo, uint16_t position)
 {
-#ifndef PIOS_ENABLE_DEBUG_PINS
-#if defined(PIOS_INCLUDE_SERVO)
 	/* Make sure servo exists */
-	if (Servo < pios_servo_cfg.num_channels && Servo >= 0) {
+	if (!servo_cfg || servo >= servo_cfg->num_channels) {
-		/* Update the position */
+		return;
-
+	}
-		switch(pios_servo_cfg.channels[Servo].channel) {
+
-			case TIM_Channel_1:
+	/* Update the position */
-				TIM_SetCompare1(pios_servo_cfg.channels[Servo].timer, Position);
+	const struct pios_tim_channel * chan = &servo_cfg->channels[servo];
-				break;
+	switch(chan->timer_chan) {
-			case TIM_Channel_2:
+		case TIM_Channel_1:
-				TIM_SetCompare2(pios_servo_cfg.channels[Servo].timer, Position);
+			TIM_SetCompare1(chan->timer, position);
-				break;
+			break;
-			case TIM_Channel_3:
+		case TIM_Channel_2:
-				TIM_SetCompare3(pios_servo_cfg.channels[Servo].timer, Position);
+			TIM_SetCompare2(chan->timer, position);
-				break;
+			break;
-			case TIM_Channel_4:
+		case TIM_Channel_3:
-				TIM_SetCompare4(pios_servo_cfg.channels[Servo].timer, Position);
+			TIM_SetCompare3(chan->timer, position);
-				break;
+			break;
-		}
+		case TIM_Channel_4:
+			TIM_SetCompare4(chan->timer, position);
+			break;
 	}
-#endif // PIOS_INCLUDE_SERVO
-#endif // PIOS_ENABLE_DEBUG_PINS
 }

flight/PiOS/STM32F10x/pios_spektrum.c

@@ -8,7 +8,6 @@
  *
  * @file       pios_spektrum.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * 	        Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
  * @brief      USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent)
  * @see        The GNU Public License (GPL) Version 3
  *
@@ -31,10 +30,11 @@
 
 /* Project Includes */
 #include "pios.h"
-#include "pios_spektrum_priv.h"
 
 #if defined(PIOS_INCLUDE_SPEKTRUM)
 
+#include "pios_spektrum_priv.h"
+
 /**
  * @Note Framesyncing:
  * The code resets the watchdog timer whenever a single byte is received, so what watchdog code
@@ -52,22 +52,80 @@ const struct pios_rcvr_driver pios_spektrum_rcvr_driver = {
 	.read = PIOS_SPEKTRUM_Get,
 };
 
-/* Local Variables */
+enum pios_spektrum_dev_magic {
-static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS];
+	PIOS_SPEKTRUM_DEV_MAGIC = 0xa9b9c9d9,
-static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 };
+};
-uint8_t sync_of = 0;
+
-uint16_t supv_timer=0;
+struct pios_spektrum_fsm {
+	uint16_t channel;
+	uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS];
+	uint16_t CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS];
+	uint8_t prev_byte;
+	uint8_t sync;
+	uint8_t bytecount;
+	uint8_t datalength;
+	uint8_t frame_error;
+	uint8_t sync_of;
+};
+
+struct pios_spektrum_dev {
+	enum pios_spektrum_dev_magic     magic;
+	const struct pios_spektrum_cfg * cfg;
+
+	struct pios_spektrum_fsm fsm;
+
+	uint16_t supv_timer;
+};
+
+static bool PIOS_SPEKTRUM_validate(struct pios_spektrum_dev * spektrum_dev)
+{
+	return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC);
+}
+
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void)
+{
+	struct pios_spektrum_dev * spektrum_dev;
+
+	spektrum_dev = (struct pios_spektrum_dev *)pvPortMalloc(sizeof(*spektrum_dev));
+	if (!spektrum_dev) return(NULL);
+
+	spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC;
+	return(spektrum_dev);
+}
+#else
+static struct pios_spektrum_dev pios_spektrum_devs[PIOS_SPEKTRUM_MAX_DEVS];
+static uint8_t pios_spektrum_num_devs;
+static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void)
+{
+	struct pios_spektrum_dev * spektrum_dev;
+
+	if (pios_spektrum_num_devs >= PIOS_SPEKTRUM_MAX_DEVS) {
+		return (NULL);
+	}
+
+	spektrum_dev = &pios_spektrum_devs[pios_spektrum_num_devs++];
+	spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC;
+
+	return (spektrum_dev);
+}
+#endif
 
 static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id);
-static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg);
+static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind);
-static int32_t PIOS_SPEKTRUM_Decode(uint8_t b);
+static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b);
 
 static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
 {
+	struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)context;
+
+	bool valid = PIOS_SPEKTRUM_validate(spektrum_dev);
+	PIOS_Assert(valid);
+
 	/* process byte(s) and clear receive timer */
 	for (uint8_t i = 0; i < buf_len; i++) {
-		PIOS_SPEKTRUM_Decode(buf[i]);
+		PIOS_SPEKTRUM_UpdateFSM(&(spektrum_dev->fsm), buf[i]);
-		supv_timer = 0;
+		spektrum_dev->supv_timer = 0;
 	}
 
 	/* Always signal that we can accept another byte */
@@ -82,23 +140,114 @@ static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint
 	return (buf_len);
 }
 
+static void PIOS_SPEKTRUM_ResetFSM(struct pios_spektrum_fsm * fsm)
+{
+	fsm->channel = 0;
+	fsm->prev_byte = 0xFF;
+	fsm->sync = 0;
+	fsm->bytecount = 0;
+	fsm->datalength = 0;
+	fsm->frame_error = 0;
+	fsm->sync_of = 0;
+}
+
+/**
+* Decodes a byte
+* \param[in] b byte which should be spektrum decoded
+* \return 0 if no error
+* \return -1 if USART not available
+* \return -2 if buffer full (retry)
+*/
+static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b)
+{
+	fsm->bytecount++;
+	if (fsm->sync == 0) {
+		/* Known sync bytes, 0x01, 0x02, 0x12, 0xb2 */
+		/* 0xb2 DX8 3bind pulses only */
+		if (fsm->bytecount == 2) {
+			if ((b == 0x01) || (b == 0x02) || (b == 0xb2)) {
+				fsm->datalength=0; // 10bit
+				fsm->sync = 1;
+				fsm->bytecount = 2;
+			}
+			else if(b == 0x12) {
+				fsm->datalength=1; // 11bit
+				fsm->sync = 1;
+				fsm->bytecount = 2;
+			}
+			else
+			{
+				fsm->bytecount = 0;
+			}
+		}
+	} else {
+		if ((fsm->bytecount % 2) == 0) {
+			uint16_t data;
+			uint8_t channeln;
+
+			fsm->channel = (fsm->prev_byte << 8) + b;
+			channeln = (fsm->channel >> (10+fsm->datalength)) & 0x0F;
+			data = fsm->channel & (0x03FF+(0x0400*fsm->datalength));
+			if(channeln==0 && data<10) // discard frame if throttle misbehaves
+			{
+				fsm->frame_error=1;
+			}
+			if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !fsm->frame_error)
+				fsm->CaptureValueTemp[channeln] = data;
+		}
+	}
+	if (fsm->bytecount == 16) {
+		fsm->bytecount = 0;
+		fsm->sync = 0;
+		fsm->sync_of = 0;
+		if (!fsm->frame_error)
+		{
+			for(int i=0;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
+			{
+				fsm->CaptureValue[i] = fsm->CaptureValueTemp[i];
+			}
+		}
+		fsm->frame_error=0;
+	}
+	fsm->prev_byte = b;
+	return 0;
+}
+
 /**
 * Bind and Initialise Spektrum satellite receiver
 */
-int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind)
+int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind)
 {
-	// TODO: need setting flag for bind on next powerup
+	PIOS_DEBUG_Assert(spektrum_id);
+	PIOS_DEBUG_Assert(cfg);
+	PIOS_DEBUG_Assert(driver);
+
+	struct pios_spektrum_dev * spektrum_dev;
+
+	spektrum_dev = (struct pios_spektrum_dev *) PIOS_SPEKTRUM_alloc();
+	if (!spektrum_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	spektrum_dev->cfg = cfg;
+
 	if (bind) {
-		PIOS_SPEKTRUM_Bind(cfg);
+		PIOS_SPEKTRUM_Bind(cfg,bind);
 	}
 
-	(driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0);
+	PIOS_SPEKTRUM_ResetFSM(&(spektrum_dev->fsm));
 
-	if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) {
+	*spektrum_id = (uint32_t)spektrum_dev;
+
+	(driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, *spektrum_id);
+
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, *spektrum_id)) {
 		PIOS_DEBUG_Assert(0);
 	}
 
 	return (0);
+
+out_fail:
+	return(-1);
 }
 
 /**
@@ -109,11 +258,16 @@ int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cf
 */
 static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel)
 {
+	struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id;
+
+	if(!PIOS_SPEKTRUM_validate(spektrum_dev))
+		return PIOS_RCVR_INVALID;
+
 	/* Return error if channel not available */
 	if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) {
-		return -1;
+		return PIOS_RCVR_INVALID;
 	}
-	return CaptureValue[channel];
+	return spektrum_dev->fsm.CaptureValue[channel];
 }
 
 /**
@@ -121,9 +275,15 @@ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel)
 * \output true Successful bind
 * \output false Bind failed
 */
-static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg)
+static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind)
 {
-#define BIND_PULSES 5
+	GPIO_InitTypeDef GPIO_InitStructure;
+	GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin;
+	GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed;
+	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
+
+	/* just to limit bind pulses */
+	bind=(bind<=10)?bind:10;
 
 	GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
 	/* RX line, set high */
@@ -132,7 +292,7 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg)
 	/* on CC works upto 140ms, I guess bind window is around 20-140ms after powerup */
 	PIOS_DELAY_WaitmS(60);
 
-	for (int i = 0; i < BIND_PULSES ; i++) {
+	for (int i = 0; i < bind ; i++) {
 		/* RX line, drive low for 120us */
 		GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
 		PIOS_DELAY_WaituS(120);
@@ -141,118 +301,44 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg)
 		PIOS_DELAY_WaituS(120);
 	}
 	/* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */
+	GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure);
 
 	return true;
 }
 
-/**
-* Decodes a byte
-* \param[in] b byte which should be spektrum decoded
-* \return 0 if no error
-* \return -1 if USART not available
-* \return -2 if buffer full (retry)
-* \note Applications shouldn't call these functions directly
-*/
-static int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
-{
-	static uint16_t channel = 0; /*, sync_word = 0;*/
-	uint8_t channeln = 0, frame = 0;
-	uint16_t data = 0;
-	byte_array[bytecount] = b;
-	bytecount++;
-	if (sync == 0) {
-		//sync_word = (prev_byte << 8) + b;
-#if 0
-		/* maybe create object to show this  data */
-		if(bytecount==1)
-		{
-			/* record losscounter into channel8 */
-			CaptureValueTemp[7]=b;
-			/* instant write */
-			CaptureValue[7]=b;
-		}
-#endif
-		/* Known sync bytes, 0x01, 0x02, 0x12 */
-		if (bytecount == 2) {
-			if (b == 0x01) {
-				datalength=0; // 10bit
-				//frames=1;
-				sync = 1;
-				bytecount = 2;
-			}
-			else if(b == 0x02) {
-				datalength=0; // 10bit
-				//frames=2;
-				sync = 1;
-				bytecount = 2;
-			}
-			else if(b == 0x12) {
-				datalength=1; // 11bit
-				//frames=2;
-				sync = 1;
-				bytecount = 2;
-			}
-			else
-			{
-				bytecount = 0;
-			}
-		}
-	} else {
-		if ((bytecount % 2) == 0) {
-			channel = (prev_byte << 8) + b;
-			frame = channel >> 15;
-			channeln = (channel >> (10+datalength)) & 0x0F;
-			data = channel & (0x03FF+(0x0400*datalength));
-			if(channeln==0 && data<10) // discard frame if throttle misbehaves
-			{
-				frame_error=1;
-			}
-			if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error)
-				CaptureValueTemp[channeln] = data;
-		}
-	}
-	if (bytecount == 16) {
-		//PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff
-		bytecount = 0;
-		sync = 0;
-		sync_of = 0;
-		if (!frame_error)
-		{
-			for(int i=0;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
-			{
-				CaptureValue[i] = CaptureValueTemp[i];
-			}
-		}
-		frame_error=0;
-	}
-	prev_byte = b;
-	return 0;
-}
-
 /**
  *@brief This function is called between frames and when a spektrum word hasnt been decoded for too long
  *@brief clears the channel values
  */
-static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) {
+static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id)
-	/* 125hz */
+{
-	supv_timer++;
+	struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)spektrum_id;
-	if(supv_timer > 5) {
+
+	bool valid = PIOS_SPEKTRUM_validate(spektrum_dev);
+	PIOS_Assert(valid);
+
+	/* 625hz */
+	spektrum_dev->supv_timer++;
+	if(spektrum_dev->supv_timer > 4) {
 		/* sync between frames */
-		sync = 0;
+		struct pios_spektrum_fsm * fsm = &(spektrum_dev->fsm);
-		bytecount = 0;
+
-		prev_byte = 0xFF;
+		fsm->sync = 0;
-		frame_error = 0;
+		fsm->bytecount = 0;
-		sync_of++;
+		fsm->prev_byte = 0xFF;
-		/* watchdog activated after 100ms silence */
+		fsm->frame_error = 0;
-		if (sync_of > 12) {
+		fsm->sync_of++;
+		/* watchdog activated after 200ms silence */
+		if (fsm->sync_of > 30) {
+
 			/* signal lost */
-			sync_of = 0;
+			fsm->sync_of = 0;
 			for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
-				CaptureValue[i] = 0;
+				fsm->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
-				CaptureValueTemp[i] = 0;
+				fsm->CaptureValueTemp[i] = PIOS_RCVR_TIMEOUT;
 			}
 		}
-		supv_timer = 0;
+		spektrum_dev->supv_timer = 0;
 	}
 }
 

flight/PiOS/STM32F10x/pios_spi.c

@@ -46,10 +46,10 @@ static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev)
 	return(true);
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_spi_dev * PIOS_SPI_alloc(void)
 {
-	return (malloc(sizeof(struct pios_spi_dev)));
+	return (pvPortMalloc(sizeof(struct pios_spi_dev)));
 }
 #else
 static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS];

flight/PiOS/STM32F10x/pios_tim.c

@@ -0,0 +1,427 @@
+#include "pios.h"
+
+#include "pios_tim.h"
+#include "pios_tim_priv.h"
+
+enum pios_tim_dev_magic {
+	PIOS_TIM_DEV_MAGIC = 0x87654098,
+};
+
+struct pios_tim_dev {
+	enum pios_tim_dev_magic     magic;
+
+	const struct pios_tim_channel * channels;
+	uint8_t num_channels;
+
+	const struct pios_tim_callbacks * callbacks;
+	uint32_t context;
+};
+
+#if 0
+static bool PIOS_TIM_validate(struct pios_tim_dev * tim_dev)
+{
+	return (tim_dev->magic == PIOS_TIM_DEV_MAGIC);
+}
+#endif
+
+#if defined(PIOS_INCLUDE_FREERTOS) && 0
+static struct pios_tim_dev * PIOS_TIM_alloc(void)
+{
+	struct pios_tim_dev * tim_dev;
+
+	tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev));
+	if (!tim_dev) return(NULL);
+
+	tim_dev->magic = PIOS_TIM_DEV_MAGIC;
+	return(tim_dev);
+}
+#else
+static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS];
+static uint8_t pios_tim_num_devs;
+static struct pios_tim_dev * PIOS_TIM_alloc(void)
+{
+	struct pios_tim_dev * tim_dev;
+
+	if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) {
+		return (NULL);
+	}
+
+	tim_dev = &pios_tim_devs[pios_tim_num_devs++];
+	tim_dev->magic = PIOS_TIM_DEV_MAGIC;
+
+	return (tim_dev);
+}
+#endif
+
+
+
+
+int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg)
+{
+	PIOS_DEBUG_Assert(cfg);
+
+	/* Enable appropriate clock to timer module */
+	switch((uint32_t) cfg->timer) {
+		case (uint32_t)TIM1:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
+			break;
+		case (uint32_t)TIM2:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
+			break;
+		case (uint32_t)TIM3:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+			break;
+		case (uint32_t)TIM4:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
+			break;
+#ifdef STM32F10X_HD
+		case (uint32_t)TIM5:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
+			break;
+		case (uint32_t)TIM6:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
+			break;
+		case (uint32_t)TIM7:
+			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE);
+			break;
+		case (uint32_t)TIM8:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
+			break;
+#endif
+	}
+
+	/* Configure the dividers for this timer */
+	TIM_TimeBaseInit(cfg->timer, cfg->time_base_init);
+
+	/* Configure internal timer clocks */
+	TIM_InternalClockConfig(cfg->timer);
+
+	/* Enable timers */
+	TIM_Cmd(cfg->timer, ENABLE);
+
+	/* Enable Interrupts */
+	NVIC_Init(&cfg->irq.init);
+
+	return 0;
+}
+
+int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context)
+{
+	PIOS_Assert(channels);
+	PIOS_Assert(num_channels);
+
+	struct pios_tim_dev * tim_dev;
+	tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc();
+	if (!tim_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	tim_dev->channels = channels;
+	tim_dev->num_channels = num_channels;
+	tim_dev->callbacks = callbacks;
+	tim_dev->context = context;
+
+	/* Configure the pins */
+	for (uint8_t i = 0; i < num_channels; i++) {
+		const struct pios_tim_channel * chan = &(channels[i]);
+
+		/* Enable the peripheral clock for the GPIO */
+		switch ((uint32_t)chan->pin.gpio) {
+		case (uint32_t) GPIOA:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
+			break;
+		case (uint32_t) GPIOB:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+			break;
+		case (uint32_t) GPIOC:
+			RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+			break;
+		default:
+			PIOS_Assert(0);
+			break;
+		}
+		GPIO_Init(chan->pin.gpio, &chan->pin.init);
+
+		if (chan->remap) {
+			GPIO_PinRemapConfig(chan->remap, ENABLE);
+		}
+	}
+
+	*tim_id = (uint32_t)tim_dev;
+
+	return(0);
+
+out_fail:
+	return(-1);
+}
+
+static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer)
+{
+	/* Iterate over all registered clients of the TIM layer to find channels on this timer */
+	for (uint8_t i = 0; i < pios_tim_num_devs; i++) {
+		const struct pios_tim_dev * tim_dev = &pios_tim_devs[i];
+
+		if (!tim_dev->channels || tim_dev->num_channels == 0) {
+			/* No channels to process on this client */
+			continue;
+		}
+
+		/* Check for an overflow event on this timer */
+		bool overflow_event;
+		uint16_t overflow_count;
+		if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) {
+			TIM_ClearITPendingBit(timer, TIM_IT_Update);
+			overflow_count = timer->ARR;
+			overflow_event = true;
+		} else {
+			overflow_count = 0;
+			overflow_event = false;
+		}
+
+		for (uint8_t j = 0; j < tim_dev->num_channels; j++) {
+			const struct pios_tim_channel * chan = &tim_dev->channels[j];
+
+			if (chan->timer != timer) {
+				/* channel is not on this timer */
+				continue;
+			}
+
+			/* Figure out which interrupt bit we should be looking at */
+			uint16_t timer_it;
+			switch (chan->timer_chan) {
+			case TIM_Channel_1:
+				timer_it = TIM_IT_CC1;
+				break;
+			case TIM_Channel_2:
+				timer_it = TIM_IT_CC2;
+				break;
+			case TIM_Channel_3:
+				timer_it = TIM_IT_CC3;
+				break;
+			case TIM_Channel_4:
+				timer_it = TIM_IT_CC4;
+				break;
+			default:
+				PIOS_Assert(0);
+				break;
+			}
+
+			bool edge_event;
+			uint16_t edge_count;
+			if (TIM_GetITStatus(chan->timer, timer_it) == SET) {
+				TIM_ClearITPendingBit(chan->timer, timer_it);
+
+				/* Read the current counter */
+				switch(chan->timer_chan) {
+				case TIM_Channel_1:
+					edge_count = TIM_GetCapture1(chan->timer);
+					break;
+				case TIM_Channel_2:
+					edge_count = TIM_GetCapture2(chan->timer);
+					break;
+				case TIM_Channel_3:
+					edge_count = TIM_GetCapture3(chan->timer);
+					break;
+				case TIM_Channel_4:
+					edge_count = TIM_GetCapture4(chan->timer);
+					break;
+				default:
+					PIOS_Assert(0);
+					break;
+				}
+				edge_event = true;
+			} else {
+				edge_event = false;
+				edge_count = 0;
+			}
+
+			if (!tim_dev->callbacks) {
+				/* No callbacks registered, we're done with this channel */
+				continue;
+			}
+
+			/* Generate the appropriate callbacks */
+			if (overflow_event & edge_event) {
+				/*
+				 * When both edge and overflow happen in the same interrupt, we
+				 * need a heuristic to determine the order of the edge and overflow
+				 * events so that the callbacks happen in the right order.  If we
+				 * get the order wrong, our pulse width calculations could be off by up
+				 * to ARR ticks.  That could be bad.
+				 *
+				 * Heuristic: If the edge_count is < 16 ticks above zero then we assume the
+				 *            edge happened just after the overflow.
+				 */
+
+				if (edge_count < 16) {
+					/* Call the overflow callback first */
+					if (tim_dev->callbacks->overflow) {
+						(*tim_dev->callbacks->overflow)((uint32_t)tim_dev,
+									tim_dev->context,
+									j,
+									overflow_count);
+					}
+					/* Call the edge callback second */
+					if (tim_dev->callbacks->edge) {
+						(*tim_dev->callbacks->edge)((uint32_t)tim_dev,
+									tim_dev->context,
+									j,
+									edge_count);
+					}
+				} else {
+					/* Call the edge callback first */
+					if (tim_dev->callbacks->edge) {
+						(*tim_dev->callbacks->edge)((uint32_t)tim_dev,
+									tim_dev->context,
+									j,
+									edge_count);
+					}
+					/* Call the overflow callback second */
+					if (tim_dev->callbacks->overflow) {
+						(*tim_dev->callbacks->overflow)((uint32_t)tim_dev,
+									tim_dev->context,
+									j,
+									overflow_count);
+					}
+				}
+			} else if (overflow_event && tim_dev->callbacks->overflow) {
+				(*tim_dev->callbacks->overflow)((uint32_t)tim_dev,
+								tim_dev->context,
+								j,
+								overflow_count);
+			} else if (edge_event && tim_dev->callbacks->edge) {
+				(*tim_dev->callbacks->edge)((uint32_t)tim_dev,
+							tim_dev->context,
+							j,
+							edge_count);
+			}
+		}
+	}
+}
+#if 0
+	uint16_t val = 0;
+	for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) {
+		struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
+		if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) {
+			
+			TIM_ClearITPendingBit(channel.timer, channel.ccr);
+			
+			switch(channel.channel) {
+				case TIM_Channel_1:
+					val = TIM_GetCapture1(channel.timer);
+					break;
+				case TIM_Channel_2:
+					val = TIM_GetCapture2(channel.timer);
+					break;
+				case TIM_Channel_3:
+					val = TIM_GetCapture3(channel.timer);
+					break;
+				case TIM_Channel_4:
+					val = TIM_GetCapture4(channel.timer);
+					break;					
+			}
+			
+			if (CaptureState[i] == 0) {
+				RiseValue[i] = val; 
+			} else {
+				FallValue[i] = val;
+			}
+			
+			// flip state machine and capture value here
+			/* Simple rise or fall state machine */
+			TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
+			if (CaptureState[i] == 0) {
+				/* Switch states */
+				CaptureState[i] = 1;
+				
+				/* Switch polarity of input capture */
+				TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
+				TIM_ICInitStructure.TIM_Channel = channel.channel;
+				TIM_ICInit(channel.timer, &TIM_ICInitStructure);				
+			} else {
+				/* Capture computation */
+				if (FallValue[i] > RiseValue[i]) {
+					CaptureValue[i] = (FallValue[i] - RiseValue[i]);
+				} else {
+					CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]);
+				}
+				
+				/* Switch states */
+				CaptureState[i] = 0;
+				
+				/* Increase supervisor counter */
+				CapCounter[i]++;
+				
+				/* Switch polarity of input capture */
+				TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
+				TIM_ICInitStructure.TIM_Channel = channel.channel;
+				TIM_ICInit(channel.timer, &TIM_ICInitStructure);
+			}
+		}		
+	}
+#endif
+
+/* Bind Interrupt Handlers
+ *
+ * Map all valid TIM IRQs to the common interrupt handler
+ * and give it enough context to properly demux the various timers
+ */
+void TIM1_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_UP_irq_handler")));
+static void PIOS_TIM_1_UP_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM1);
+}
+
+void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler")));
+static void PIOS_TIM_1_CC_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM1);
+}
+
+void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler")));
+static void PIOS_TIM_2_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM2);
+}
+
+void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler")));
+static void PIOS_TIM_3_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM3);
+}
+
+void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler")));
+static void PIOS_TIM_4_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM4);
+}
+
+void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler")));
+static void PIOS_TIM_5_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM5);
+}
+
+void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler")));
+static void PIOS_TIM_6_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM6);
+}
+
+void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler")));
+static void PIOS_TIM_7_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM7);
+}
+
+void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler")));
+static void PIOS_TIM_8_UP_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM8);
+}
+
+void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler")));
+static void PIOS_TIM_8_CC_irq_handler (void)
+{
+	PIOS_TIM_generic_irq_handler (TIM8);
+}
+

flight/PiOS/STM32F10x/pios_usart.c

@@ -70,12 +70,12 @@ static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev)
 	return (usart_dev->magic == PIOS_USART_DEV_MAGIC);
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_usart_dev * PIOS_USART_alloc(void)
 {
 	struct pios_usart_dev * usart_dev;
 
-	usart_dev = (struct pios_usart_dev *)malloc(sizeof(*usart_dev));
+	usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev));
 	if (!usart_dev) return(NULL);
 
 	usart_dev->magic = PIOS_USART_DEV_MAGIC;

flight/PiOS/STM32F10x/pios_usb_hid.c

@@ -75,12 +75,12 @@ static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev)
 	return (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC);
 }
 
-#if defined(PIOS_INCLUDE_FREERTOS) && 0
+#if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void)
 {
 	struct pios_usb_hid_dev * usb_hid_dev;
 
-	usb_hid_dev = (struct pios_usb_hid_dev *)malloc(sizeof(*usb_hid_dev));
+	usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev));
 	if (!usb_hid_dev) return(NULL);
 
 	usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC;

flight/PiOS/inc/pios_debug.h

@@ -33,7 +33,9 @@
 
 extern const char *PIOS_DEBUG_AssertMsg;
 
-void PIOS_DEBUG_Init(void);
+#include <pios_tim_priv.h>
+
+void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels);
 void PIOS_DEBUG_PinHigh(uint8_t pin);
 void PIOS_DEBUG_PinLow(uint8_t pin);
 void PIOS_DEBUG_PinValue8Bit(uint8_t value);

flight/PiOS/inc/pios_flashfs_objlist.h

@@ -32,6 +32,7 @@
 #include "uavobjectmanager.h"
 
 int32_t PIOS_FLASHFS_Init();
+int32_t PIOS_FLASHFS_Format();
 int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data);
 int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data);
-int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId);
+int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId);

flight/PiOS/inc/pios_gcsrcvr_priv.h

@@ -1,45 +1,47 @@
-/**
+/**
- ******************************************************************************
+ ******************************************************************************
- *
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @file       uavobjectsinit.c
+ * @{
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup   PIOS_GCSRCVR GCS Receiver Functions
- * @brief      Initialize all objects.
+ * @brief PIOS interface to read from GCS receiver port
- *             Automatically generated by the UAVObjectGenerator.
+ * @{
- *   
+ *
- * @note       This is an automatically generated file.
+ * @file       pios_gcsrcvr_priv.h
- *             DO NOT modify manually.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @see        The GNU Public License (GPL) Version 3
+ * @brief      GCS receiver private functions
- *
+ * @see        The GNU Public License (GPL) Version 3
- *****************************************************************************/
+ *
-/*
+ *****************************************************************************/
- * This program is free software; you can redistribute it and/or modify
+/*
- * it under the terms of the GNU General Public License as published by
+ * This program is free software; you can redistribute it and/or modify
- * the Free Software Foundation; either version 3 of the License, or
+ * it under the terms of the GNU General Public License as published by
- * (at your option) any later version.
+ * the Free Software Foundation; either version 3 of the License, or
- *
+ * (at your option) any later version.
- * This program is distributed in the hope that it will be useful, but
+ *
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * This program is distributed in the hope that it will be useful, but
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * for more details.
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- *
+ * for more details.
- * You should have received a copy of the GNU General Public License along
+ *
- * with this program; if not, write to the Free Software Foundation, Inc.,
+ * You should have received a copy of the GNU General Public License along
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * with this program; if not, write to the Free Software Foundation, Inc.,
- */
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
-
+ */
-#include "openpilot.h"
+
-
+#ifndef PIOS_GCSRCVR_PRIV_H
-/**
+#define PIOS_GCSRCVR_PRIV_H
- * Function used to initialize the first instance of each object.
+
- * This file is automatically updated by the UAVObjectGenerator.
+#include <pios.h>
- */
+
-extern initcall_t __uavobj_initcall_start[], __uavobj_initcall_end[];
+#include "gcsreceiver.h"
-
+
-void UAVObjectsInitializeAll()
+extern const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver;
-{
+
-	initcall_t *fn;
+extern void PIOS_GCSRCVR_Init(void);
-	int32_t ret;
+
-
+#endif /* PIOS_GCSRCVR_PRIV_H */
-	for (fn = __uavobj_initcall_start; fn < __uavobj_initcall_end; fn++)
+
-		ret = (*fn)();
+/**
-}
+ * @}
+ * @}
+ */

flight/PiOS/inc/pios_initcall.h

@@ -67,7 +67,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[];
 	static initmodule_t __initcall_##fn __attribute__((__used__)) \
 	__attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn };
 
-#define UAVOBJ_INITCALL(fn)		__define_initcall("uavobj",fn,1)
 #define MODULE_INITCALL(ifn, sfn)		__define_module_initcall("module", ifn, sfn)
 
 #define MODULE_INITIALISE_ALL  { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \

flight/PiOS/inc/pios_ppm_priv.h

@@ -35,24 +35,14 @@
 #include <pios_stm32.h>
 
 struct pios_ppm_cfg {
-	TIM_TimeBaseInitTypeDef tim_base_init;
 	TIM_ICInitTypeDef tim_ic_init;
-	GPIO_InitTypeDef gpio_init;
+	const struct pios_tim_channel * channels;
-	uint32_t remap;		/* GPIO_Remap_* */
+	uint8_t num_channels;
-	struct stm32_irq irq;
-	TIM_TypeDef * timer;
-	GPIO_TypeDef * port;
-	uint16_t ccr;
 };
 
-extern void PIOS_PPM_irq_handler();
-
-extern uint8_t pios_ppm_num_channels;
-extern const struct pios_ppm_cfg pios_ppm_cfg;
-
 extern const struct pios_rcvr_driver pios_ppm_rcvr_driver;
 
-extern void PIOS_PPM_Init(void);
+extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg);
 
 #endif /* PIOS_PPM_PRIV_H */
 

flight/PiOS/inc/pios_pwm_priv.h

@@ -34,32 +34,17 @@
 #include <pios.h>
 #include <pios_stm32.h>
 
-struct pios_pwm_channel {
+#include <pios_tim_priv.h>
-	TIM_TypeDef * timer;
-	GPIO_TypeDef * port;
-	uint16_t ccr;
-	uint8_t channel;
-	uint16_t pin;
-};
 
 struct pios_pwm_cfg {
-	TIM_TimeBaseInitTypeDef tim_base_init;
 	TIM_ICInitTypeDef tim_ic_init;
-	GPIO_InitTypeDef gpio_init;
+	const struct pios_tim_channel * channels;
-	uint32_t remap;		/* GPIO_Remap_* */
-	struct stm32_irq irq;
-	const struct pios_pwm_channel *const channels;
 	uint8_t num_channels;
 };
 
-extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer);
-
-extern uint8_t pios_pwm_num_channels;
-extern const struct pios_pwm_cfg pios_pwm_cfg;
-
 extern const struct pios_rcvr_driver pios_pwm_rcvr_driver;
 
-extern void PIOS_PWM_Init(void);
+extern int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg);
 
 #endif /* PIOS_PWM_PRIV_H */
 

flight/PiOS/inc/pios_rcvr.h

@@ -31,13 +31,6 @@
 #ifndef PIOS_RCVR_H
 #define PIOS_RCVR_H
 
-struct pios_rcvr_channel_map {
-	uint32_t id;
-	uint8_t  channel;
-};
-
-extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[];
-
 struct pios_rcvr_driver {
 	void    (*init)(uint32_t id);
 	int32_t (*read)(uint32_t id, uint8_t channel);
@@ -46,6 +39,16 @@ struct pios_rcvr_driver {
 /* Public Functions */
 extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel);
 
+/*! Define error codes for PIOS_RCVR_Get */
+enum PIOS_RCVR_errors {
+	/*! Indicates that a failsafe condition or missing receiver detected for that channel */
+	PIOS_RCVR_TIMEOUT = 0,
+	/*! Channel is invalid for this driver (usually out of range supported) */
+	PIOS_RCVR_INVALID = -1,
+	/*! Indicates that the driver for this channel has not been initialized */
+	PIOS_RCVR_NODRIVER = -2
+};
+
 #endif /* PIOS_RCVR_H */
 
 /**

flight/PiOS/inc/pios_sbus_priv.h

@@ -44,8 +44,8 @@
  *  1 byte  - 0x0f (start of frame byte)
  * 22 bytes - channel data (11 bit/channel, 16 channels, LSB first)
  *  1 byte  - bit flags:
- *                   0x01 - digital channel 1,
+ *                   0x01 - discrete channel 1,
- *                   0x02 - digital channel 2,
+ *                   0x02 - discrete channel 2,
  *                   0x04 - lost frame flag,
  *                   0x08 - failsafe flag,
  *                   0xf0 - reserved
@@ -54,16 +54,20 @@
 #define SBUS_FRAME_LENGTH		(1+22+1+1)
 #define SBUS_SOF_BYTE			0x0f
 #define SBUS_EOF_BYTE			0x00
-#define SBUS_FLAG_DG1			0x01
+#define SBUS_FLAG_DC1			0x01
-#define SBUS_FLAG_DG2			0x02
+#define SBUS_FLAG_DC2			0x02
 #define SBUS_FLAG_FL			0x04
 #define SBUS_FLAG_FS			0x08
 
 /*
- * S.Bus protocol provides up to 16 analog and 2 digital channels.
+ * S.Bus protocol provides 16 proportional and 2 discrete channels.
- * Only 8 channels are currently supported by the OpenPilot.
+ * Do not change unless driver code is updated accordingly.
  */
-#define	SBUS_NUMBER_OF_CHANNELS		8
+#define	SBUS_NUMBER_OF_CHANNELS		(16 + 2)
+
+/* Discrete channels represented as bits, provide values for them */
+#define	SBUS_VALUE_MIN			352
+#define	SBUS_VALUE_MAX			1696
 
 /*
  * S.Bus configuration programmable invertor

flight/PiOS/inc/pios_servo.h

@@ -31,7 +31,6 @@
 #define PIOS_SERVO_H
 
 /* Public Functions */
-extern void PIOS_Servo_Init(void);
 extern void PIOS_Servo_SetHz(uint16_t * update_rates, uint8_t channels);
 extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position);
 

flight/PiOS/inc/pios_servo_priv.h

@@ -33,25 +33,18 @@
 
 #include <pios.h>
 #include <pios_stm32.h>
-
+#include <pios_tim_priv.h>
-struct pios_servo_channel {
-	TIM_TypeDef * timer;
-	GPIO_TypeDef * port;
-	uint8_t channel;
-	uint16_t pin;
-};
 
 struct pios_servo_cfg {
 	TIM_TimeBaseInitTypeDef tim_base_init;
 	TIM_OCInitTypeDef tim_oc_init;
 	GPIO_InitTypeDef gpio_init;
 	uint32_t remap;
-	const struct pios_servo_channel *const channels;
+	const struct pios_tim_channel * channels;
 	uint8_t num_channels;
 };
 
-
+extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg);
-extern const struct pios_servo_cfg pios_servo_cfg;
 
 #endif /* PIOS_SERVO_PRIV_H */
 

flight/PiOS/inc/pios_spektrum_priv.h

@@ -42,9 +42,9 @@ struct pios_spektrum_cfg {
 
 extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver;
 
-extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind);
+extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind);
 
-#endif /* PIOS_PWM_PRIV_H */
+#endif /* PIOS_SPEKTRUM_PRIV_H */
 
 /**
  * @}

flight/PiOS/inc/pios_tim.h

@@ -0,0 +1,4 @@
+#ifndef PIOS_TIM_H
+#define PIOS_TIM_H
+
+#endif	/* PIOS_TIM_H */

flight/PiOS/inc/pios_tim_priv.h

@@ -0,0 +1,28 @@
+#ifndef PIOS_TIM_PRIV_H
+#define PIOS_TIM_PRIV_H
+
+#include <pios_stm32.h>
+
+struct pios_tim_clock_cfg {
+	TIM_TypeDef * timer;
+	const TIM_TimeBaseInitTypeDef * time_base_init;
+	struct stm32_irq irq;
+};
+
+struct pios_tim_channel {
+	TIM_TypeDef * timer;
+	uint8_t timer_chan;
+
+	struct stm32_gpio pin;
+	uint32_t remap;
+};
+
+struct pios_tim_callbacks {
+	void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
+	void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count);
+};
+
+extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg);
+extern int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context);
+
+#endif	/* PIOS_TIM_PRIV_H */

flight/PipXtreme/Makefile

@@ -394,7 +394,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino opfw
 elf: $(OUTDIR)/$(TARGET).elf

flight/Project/OpenOCD/stm32f1x.cfg

@@ -0,0 +1,75 @@
+# script for stm32
+
+if { [info exists CHIPNAME] } {
+   set  _CHIPNAME $CHIPNAME
+} else {
+   set  _CHIPNAME stm32
+}
+
+if { [info exists ENDIAN] } {
+   set  _ENDIAN $ENDIAN
+} else {
+   set  _ENDIAN little
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 16kB
+if { [info exists WORKAREASIZE] } {
+   set  _WORKAREASIZE $WORKAREASIZE
+} else {
+   set  _WORKAREASIZE 0x4000
+}
+
+# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
+adapter_khz 1000
+
+adapter_nsrst_delay 100
+jtag_ntrst_delay 100
+
+#jtag scan chain
+if { [info exists CPUTAPID ] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+  # See STM Document RM0008
+  # Section 26.6.3
+   set _CPUTAPID 0x3ba00477
+}
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+
+if { [info exists BSTAPID ] } {
+   # FIXME this never gets used to override defaults...
+   set _BSTAPID $BSTAPID
+} else {
+  # See STM Document RM0008
+  # Section 29.6.2
+  # Low density devices, Rev A
+  set _BSTAPID1 0x06412041
+  # Medium density devices, Rev A
+  set _BSTAPID2 0x06410041
+  # Medium density devices, Rev B and Rev Z
+  set _BSTAPID3 0x16410041
+  set _BSTAPID4 0x06420041
+  # High density devices, Rev A
+  set _BSTAPID5 0x06414041
+  # Connectivity line devices, Rev A and Rev Z
+  set _BSTAPID6 0x06418041
+  # XL line devices, Rev A
+  set _BSTAPID7 0x06430041
+}
+jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID1 \
+	-expected-id $_BSTAPID2 -expected-id $_BSTAPID3 \
+	-expected-id $_BSTAPID4 -expected-id $_BSTAPID5 \
+	-expected-id $_BSTAPID6 -expected-id $_BSTAPID7
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+# flash size will be probed
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f1x 0x08000000 0 0 0 $_TARGETNAME
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m3 reset_config sysresetreq

flight/Project/OpenOCD/stm32f2x.cfg

@@ -0,0 +1,61 @@
+# script for stm32f2xxx
+
+if { [info exists CHIPNAME] } {
+   set  _CHIPNAME $CHIPNAME
+} else {
+   set  _CHIPNAME stm32f2xxx
+}
+
+if { [info exists ENDIAN] } {
+   set  _ENDIAN $ENDIAN
+} else {
+   set  _ENDIAN little
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 64kB
+if { [info exists WORKAREASIZE] } {
+   set  _WORKAREASIZE $WORKAREASIZE
+} else {
+   set  _WORKAREASIZE 0x10000
+}
+
+# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
+#
+# Since we may be running of an RC oscilator, we crank down the speed a
+# bit more to be on the safe side. Perhaps superstition, but if are
+# running off a crystal, we can run closer to the limit. Note
+# that there can be a pretty wide band where things are more or less stable.
+jtag_khz 1000
+
+jtag_nsrst_delay 100
+jtag_ntrst_delay 100
+
+#jtag scan chain
+if { [info exists CPUTAPID ] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+  # See STM Document RM0033
+  # Section 32.6.3 - corresponds to Cortex-M3 r2p0
+   set _CPUTAPID 0x4ba00477
+}
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+
+if { [info exists BSTAPID ] } {
+   set _BSTAPID $BSTAPID
+} else {
+  # See STM Document RM0033
+  # Section 32.6.2
+  # 
+  set _BSTAPID 0x06411041
+}
+jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
+

flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj

@@ -85,11 +85,22 @@
 		6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
 		6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = "<group>"; };
 		655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
+		655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = camerastabsettings.xml; sourceTree = "<group>"; };
 		656268C612DC1923007B0A0F /* nedaccel.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = nedaccel.xml; sourceTree = "<group>"; };
 		6562BE1713CCAD0600C823E8 /* pios_rcvr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rcvr.c; sourceTree = "<group>"; };
 		65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
 		65632DF61251650300469B77 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = "<group>"; };
 		65632DF71251650300469B77 /* STM3210E_OP.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM3210E_OP.h; sourceTree = "<group>"; };
+		65643CAB1413322000A32F59 /* pios_rcvr_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr_priv.h; sourceTree = "<group>"; };
+		65643CAC1413322000A32F59 /* pios_rcvr.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr.h; sourceTree = "<group>"; };
+		65643CAD1413322000A32F59 /* pios_rtc_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc_priv.h; sourceTree = "<group>"; };
+		65643CAE1413322000A32F59 /* pios_sbus_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus_priv.h; sourceTree = "<group>"; };
+		65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = "<group>"; };
+		65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = "<group>"; };
+		65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = "<group>"; };
+		65643CBA141350C200A32F59 /* pios_sbus.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_sbus.c; sourceTree = "<group>"; };
+		65643CEC141429A100A32F59 /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = "<group>"; };
+		65643CEE141429AF00A32F59 /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = "<group>"; };
 		6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; };
 		6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; };
 		657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = "<group>"; };
@@ -2680,7 +2691,6 @@
 		65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = watchdogstatus.xml; sourceTree = "<group>"; };
 		65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjecttemplate.c; sourceTree = "<group>"; };
 		65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinittemplate.c; sourceTree = "<group>"; };
-		65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_cc.c; sourceTree = "<group>"; };
 		65C35EA112F0A834004811C2 /* uavobjectmanager.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectmanager.c; sourceTree = "<group>"; };
 		65C35EA312F0A834004811C2 /* eventdispatcher.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = eventdispatcher.h; sourceTree = "<group>"; };
 		65C35EA412F0A834004811C2 /* uavobjectmanager.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = uavobjectmanager.h; sourceTree = "<group>"; };
@@ -2690,9 +2700,13 @@
 		65C35EA812F0A834004811C2 /* eventdispatcher.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = eventdispatcher.c; sourceTree = "<group>"; };
 		65C35F6612F0DC2D004811C2 /* attitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = attitude.c; sourceTree = "<group>"; };
 		65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = "<group>"; };
+		65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = "<group>"; };
+		65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = "<group>"; };
 		65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = "<group>"; };
 		65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = "<group>"; };
+		65DEA79113F2143B00095B06 /* cameradesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = cameradesired.xml; sourceTree = "<group>"; };
 		65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = "<group>"; };
+		65E6D80713E3A4D0002A557A /* hwsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = hwsettings.xml; sourceTree = "<group>"; };
 		65E6DF7112E02E8E00058553 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = "<group>"; };
 		65E6DF7312E02E8E00058553 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = alarms.c; sourceTree = "<group>"; };
 		65E6DF7412E02E8E00058553 /* coptercontrol.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = coptercontrol.c; sourceTree = "<group>"; };
@@ -3208,6 +3222,7 @@
 				650D8E2812DFE16400D05CC9 /* Altitude */,
 				65C35F6512F0DC2D004811C2 /* Attitude */,
 				650D8E2E12DFE16400D05CC9 /* Battery */,
+				65C9903B13A871B90082BD60 /* CameraStab */,
 				650D8E3212DFE16400D05CC9 /* Example */,
 				650D8E3B12DFE16400D05CC9 /* FirmwareIAP */,
 				650D8E3F12DFE16400D05CC9 /* FlightPlan */,
@@ -3517,7 +3532,6 @@
 			children = (
 				65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */,
 				65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */,
-				65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */,
 				65C35EA112F0A834004811C2 /* uavobjectmanager.c */,
 				65C35EA212F0A834004811C2 /* inc */,
 				65C35EA812F0A834004811C2 /* eventdispatcher.c */,
@@ -7422,6 +7436,7 @@
 		65C35E4F12EFB2F3004811C2 /* uavobjectdefinition */ = {
 			isa = PBXGroup;
 			children = (
+				65E6D80713E3A4D0002A557A /* hwsettings.xml */,
 				65E8C788139AA2A800E1F979 /* accessorydesired.xml */,
 				65C35E5012EFB2F3004811C2 /* actuatorcommand.xml */,
 				65C35E5112EFB2F3004811C2 /* actuatordesired.xml */,
@@ -7434,6 +7449,7 @@
 				65E410AE12F65AEA00725888 /* attitudesettings.xml */,
 				65C35E5912EFB2F3004811C2 /* baroaltitude.xml */,
 				65C35E5A12EFB2F3004811C2 /* batterysettings.xml */,
+				655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */,
 				652C8568132B632A00BFCC70 /* firmwareiapobj.xml */,
 				65C35E5C12EFB2F3004811C2 /* flightbatterystate.xml */,
 				65C35E5D12EFB2F3004811C2 /* flightplancontrol.xml */,
@@ -7468,6 +7484,7 @@
 				65C35E7712EFB2F3004811C2 /* velocityactual.xml */,
 				65C35E7812EFB2F3004811C2 /* velocitydesired.xml */,
 				65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */,
+				65DEA79113F2143B00095B06 /* cameradesired.xml */,
 			);
 			path = uavobjectdefinition;
 			sourceTree = "<group>";
@@ -7501,6 +7518,23 @@
 			path = inc;
 			sourceTree = "<group>";
 		};
+		65C9903B13A871B90082BD60 /* CameraStab */ = {
+			isa = PBXGroup;
+			children = (
+				65C9903C13A871B90082BD60 /* camerastab.c */,
+				65C9903D13A871B90082BD60 /* inc */,
+			);
+			path = CameraStab;
+			sourceTree = "<group>";
+		};
+		65C9903D13A871B90082BD60 /* inc */ = {
+			isa = PBXGroup;
+			children = (
+				65C9903E13A871B90082BD60 /* camerastab.h */,
+			);
+			path = inc;
+			sourceTree = "<group>";
+		};
 		65E6DF7012E02E8E00058553 /* CopterControl */ = {
 			isa = PBXGroup;
 			children = (
@@ -7700,11 +7734,17 @@
 				65E8F04811EFF25C00BBF654 /* pios_ppm.h */,
 				65E8F04911EFF25C00BBF654 /* pios_pwm.h */,
 				657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */,
+				65643CAC1413322000A32F59 /* pios_rcvr.h */,
+				65643CAB1413322000A32F59 /* pios_rcvr_priv.h */,
 				6589A9E2131DF1C7006BD67C /* pios_rtc.h */,
+				65643CAD1413322000A32F59 /* pios_rtc_priv.h */,
+				65643CAE1413322000A32F59 /* pios_sbus_priv.h */,
+				65643CAF1413322000A32F59 /* pios_sbus.h */,
 				65E8F04A11EFF25C00BBF654 /* pios_sdcard.h */,
 				65E8F04B11EFF25C00BBF654 /* pios_servo.h */,
 				65FBE14412E7C98100176B5A /* pios_servo_priv.h */,
 				65E8F04C11EFF25C00BBF654 /* pios_spektrum.h */,
+				65643CB01413322000A32F59 /* pios_spektrum_priv.h */,
 				65E8F04D11EFF25C00BBF654 /* pios_spi.h */,
 				65E8F04E11EFF25C00BBF654 /* pios_spi_priv.h */,
 				65E8F04F11EFF25C00BBF654 /* pios_stm32.h */,
@@ -7728,6 +7768,24 @@
 		65E8F05811EFF25C00BBF654 /* STM32F10x */ = {
 			isa = PBXGroup;
 			children = (
+				65D1FBD813F51865006374A6 /* pios_bmp085.c */,
+				6560A39D13EE277E00105DA5 /* pios_iap.c */,
+				6560A39E13EE277E00105DA5 /* pios_sbus.c */,
+				6560A38E13EE270C00105DA5 /* link_STM3210E_INS_BL_sections.ld */,
+				6560A38F13EE270C00105DA5 /* link_STM3210E_INS_memory.ld */,
+				6560A39013EE270C00105DA5 /* link_STM3210E_INS_sections.ld */,
+				6560A39113EE270C00105DA5 /* link_STM3210E_OP_BL_sections.ld */,
+				6560A39213EE270C00105DA5 /* link_STM3210E_OP_memory.ld */,
+				6560A39313EE270C00105DA5 /* link_STM3210E_OP_sections.ld */,
+				6560A39413EE270C00105DA5 /* link_STM32103CB_AHRS_BL_sections.ld */,
+				6560A39513EE270C00105DA5 /* link_STM32103CB_AHRS_memory.ld */,
+				6560A39613EE270C00105DA5 /* link_STM32103CB_AHRS_sections.ld */,
+				6560A39713EE270C00105DA5 /* link_STM32103CB_CC_Rev1_BL_sections.ld */,
+				6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */,
+				6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */,
+				6560A39813EE270C00105DA5 /* link_STM32103CB_PIPXTREME_BL_sections.ld */,
+				6560A39913EE270C00105DA5 /* link_STM32103CB_PIPXTREME_memory.ld */,
+				6560A39A13EE270C00105DA5 /* link_STM32103CB_PIPXTREME_sections.ld */,
 				65E8F05911EFF25C00BBF654 /* Libraries */,
 				65E8F0D811EFF25C00BBF654 /* link_stm32f10x_HD.ld */,
 				65E8F0DB11EFF25C00BBF654 /* link_stm32f10x_MD.ld */,
@@ -7742,10 +7800,12 @@
 				65E8F0E411EFF25C00BBF654 /* pios_ppm.c */,
 				65E8F0E511EFF25C00BBF654 /* pios_pwm.c */,
 				6589A9DB131DEE76006BD67C /* pios_rtc.c */,
+				65643CBA141350C200A32F59 /* pios_sbus.c */,
 				65E8F0E611EFF25C00BBF654 /* pios_servo.c */,
 				65E8F0E711EFF25C00BBF654 /* pios_spektrum.c */,
 				65E8F0E811EFF25C00BBF654 /* pios_spi.c */,
 				65E8F0E911EFF25C00BBF654 /* pios_sys.c */,
+				65643CB91413456D00A32F59 /* pios_tim.c */,
 				65E8F0EA11EFF25C00BBF654 /* pios_usart.c */,
 				65E8F0ED11EFF25C00BBF654 /* pios_usb_hid.c */,
 				651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */,
@@ -7763,8 +7823,6 @@
 		65E8F05911EFF25C00BBF654 /* Libraries */ = {
 			isa = PBXGroup;
 			children = (
-				6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */,
-				6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */,
 				65E8F05A11EFF25C00BBF654 /* CMSIS */,
 				65E8F06B11EFF25C00BBF654 /* dosfs */,
 				65E8F07111EFF25C00BBF654 /* FreeRTOS */,

flight/Project/versionblob.py

@@ -1,99 +0,0 @@
-#!/usr/bin/env python
-
-#  Generate a version blob for
-# the OpenPilot firmware.
-#
-# By E. Lafargue (c) 2011 E. Lafargue & the OpenPilot team
-#  Licence: GPLv3
-#
-###
-# Usage:
-#   versionblob.py --append --firmware=<Firmware file name> --boardid=<Board code>
-#
-#    append: if present, then append blob to firmware file directly, otherwise create "blob.bin"
-#    firmware: the filename of the firmware binary
-#    boardid: as a string, the board code, for example "0401" for CC board version 1.
-#           should match the codes in firmware description files.
-# 
-# We have 100 bytes for the whole description.
-#
-# Only the first 40 are visible on the FirmwareIAP uavobject, the remaining
-# 60 are ok to use for packaging and will be saved in the flash
-#
-# Structure is:
-#  4 bytes: header: "OpFw"
-#  4 bytes: GIT commit tag (short version of SHA1)
-#  4 bytes: Unix timestamp of compile time
-#  2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files.
-#  26 bytes: commit tag if it is there, otherwise branch name. Zero-padded
-#   ---- 40 bytes limit ---
-#  20 bytes: SHA1 sum of the firmware.
-#  40 bytes: free for now.
-
-import binascii
-import os
-from time import time
-import argparse
-
-# Do the argument parsing:
-parser = argparse.ArgumentParser(description='Generate firmware desciption blob')
-parser.add_argument('--append', action='store_true')
-parser.add_argument('--firmware', help='name of firmware binary file' , required=True)
-parser.add_argument('--boardid', help='ID of board model, for example 0401 for CopterControl', required=True)
-args = parser.parse_args()
-print args
-
-if args.append == True:
-	print 'Appending description blob directly to ' + args.firmware
-	filename = args.firmware
-	file = open(filename,"ab")
-else:
-	filename = 'blob.bin'
-	file = open(filename,"wb")
-
-
-# Write the magic value:
-file.write("OpFw")
-# Get the short commit tag of the current git repository.
-# Strip it to 8 characters for a 4 byte (int32) value.
-# We have no full guarantee of unicity, but it is good enough
-# with the rest of the information in the structure.
-hs= os.popen('git rev-parse --short=8 HEAD').read().strip()
-print "Version: " + hs
-hb=binascii.a2b_hex(hs)
-file.write(hb)
-# Then the Unix time into a 32 bit integer:
-print "Date: "  + hex(int(time())).lstrip('0x')
-hb = binascii.a2b_hex(hex(int(time())).lstrip('0x'))
-file.write(hb)
-
-# Then write board type and board revision
-hb = binascii.a2b_hex(args.boardid)
-file.write(hb)
-
-# Last: a user-friendly description if it exists in GIT, otherwise
-# just "unreleased"
-hs = os.popen('git describe --exact-match').read()
-if len(hs) == 0 :
-	print "Unreleased: get branch name instead"
-	hs = os.popen('git branch --contains HEAD').read()
-
-file.write(hs[0:26])
-file.write("\0"*(26-len(hs)))
-
-## Now we are at the 40 bytes mark.
-
-## Add the 20 byte SHA1 hash of the firmware:
-import hashlib
-sha1 = hashlib.sha1()
-with open('build/coptercontrol/CopterControl.bin','rb') as f: 
-    for chunk in iter(lambda: f.read(8192), ''): 
-         sha1.update(chunk)
-file.write(sha1.digest())
-
-# Pad will null bytes:
-file.write('\0'*40)
-
-
-file.close()
-

flight/UAVObjects/inc/uavobjectsinittemplate.h

@@ -29,4 +29,6 @@
 
 void UAVObjectsInitializeAll();
 
+#define UAVOBJECTS_LARGEST $(SIZECALCULATION)
+
 #endif // UAVOBJECTSINIT_H

flight/UAVObjects/uavobjectsinittemplate.c

@@ -36,5 +36,7 @@ $(OBJINC)
  */
 void UAVObjectsInitializeAll()
 {
+return;
+// This function is no longer used anyway
 $(OBJINIT)
 }

flight/UAVObjects/uavobjecttemplate.c

@@ -40,15 +40,19 @@
 #include "$(NAMELC).h"
 
 // Private variables
-static UAVObjHandle handle;
+static UAVObjHandle handle = NULL;
 
 /**
  * Initialize object.
  * \return 0 Success
- * \return -1 Failure
+ * \return -1 Failure to initialize or -2 for already initialized
  */
 int32_t $(NAME)Initialize(void)
 {
+	// Don't set the handle to null if already registered
+	if(UAVObjGetByID($(NAMEUC)_OBJID) != NULL)
+		return -2;
+	
 	// Register object with the object manager
 	handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0,
 			$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
@@ -64,8 +68,6 @@ int32_t $(NAME)Initialize(void)
 	}
 }
 
-UAVOBJ_INITCALL($(NAME)Initialize);
-
 /**
  * Initialize object fields and metadata with the default values.
  * If a default value is not specified the object fields

flight/UAVTalk/inc/uavtalk.h

@@ -29,10 +29,6 @@
 #ifndef UAVTALK_H
 #define UAVTALK_H
 
-// Public constants
-#define UAVTALK_WAITFOREVER -1
-#define UAVTALK_NOWAIT 0
-
 // Public types
 typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length);
 
@@ -47,14 +43,17 @@ typedef struct {
     uint32_t rxErrors;
 } UAVTalkStats;
 
+typedef void* UAVTalkConnection;
+
 // Public functions
-int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream);
+UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize);
-int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream);
+int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
-int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
+UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
-int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
+int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
-int32_t UAVTalkProcessInputStream(uint8_t rxbyte);
+int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
-void UAVTalkGetStats(UAVTalkStats* stats);
+int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
-void UAVTalkResetStats();
+void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
+void UAVTalkResetStats(UAVTalkConnection connection);
 
 #endif // UAVTALK_H
 /**

flight/UAVTalk/inc/uavtalk_priv.h

@@ -0,0 +1,111 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @{
+ * @addtogroup OpenPilotLibraries OpenPilot System Libraries
+ * @{
+ * @file       uavtalk.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Private include file of the UAVTalk library
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef UAVTALK_PRIV_H
+#define UAVTALK_PRIV_H
+
+#include "uavobjectsinit.h"
+
+// Private types and constants
+typedef struct {
+	uint8_t  sync;
+	uint8_t	 type;
+	uint16_t size;
+	uint32_t objId;
+} uavtalk_min_header;
+#define UAVTALK_MIN_HEADER_LENGTH       sizeof(uavtalk_min_header)
+
+typedef struct {
+	uint8_t  sync;
+	uint8_t	 type;
+	uint16_t size;
+	uint32_t objId;
+	uint16_t instId;
+} uavtalk_max_header;
+#define UAVTALK_MAX_HEADER_LENGTH       sizeof(uavtalk_max_header)
+
+typedef uint8_t uavtalk_checksum;
+#define UAVTALK_CHECKSUM_LENGTH	        sizeof(uavtalk_checksum)
+#define UAVTALK_MAX_PAYLOAD_LENGTH      (UAVOBJECTS_LARGEST + 1)
+#define UAVTALK_MIN_PACKET_LENGTH	UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
+#define UAVTALK_MAX_PACKET_LENGTH       UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
+
+typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState;
+
+typedef struct {
+    UAVObjHandle obj;
+    uint8_t type;
+    uint16_t packet_size;
+    uint32_t objId;
+    uint16_t instId;
+    uint32_t length;
+    uint8_t cs;
+    int32_t rxCount;
+    UAVTalkRxState state;
+    uint16_t rxPacketLength;
+} UAVTalkInputProcessor;
+
+typedef struct {
+    uint8_t canari;
+    UAVTalkOutputStream outStream;
+    xSemaphoreHandle lock;
+    xSemaphoreHandle transLock;
+    xSemaphoreHandle respSema;
+    UAVObjHandle respObj;
+    uint16_t respInstId;
+    UAVTalkStats stats;
+    UAVTalkInputProcessor iproc;
+    uint8_t *rxBuffer;
+    uint32_t txSize;
+    uint8_t *txBuffer;
+} UAVTalkConnectionData;
+
+#define UAVTALK_CANARI         0xCA
+#define UAVTALK_WAITFOREVER     -1
+#define UAVTALK_NOWAIT          0
+#define UAVTALK_SYNC_VAL       0x3C
+#define UAVTALK_TYPE_MASK      0xF8
+#define UAVTALK_TYPE_VER       0x20
+#define UAVTALK_TYPE_OBJ       (UAVTALK_TYPE_VER | 0x00)
+#define UAVTALK_TYPE_OBJ_REQ   (UAVTALK_TYPE_VER | 0x01)
+#define UAVTALK_TYPE_OBJ_ACK   (UAVTALK_TYPE_VER | 0x02)
+#define UAVTALK_TYPE_ACK       (UAVTALK_TYPE_VER | 0x03)
+#define UAVTALK_TYPE_NACK      (UAVTALK_TYPE_VER | 0x04)
+
+//macros
+#define CHECKCONHANDLE(handle,variable,failcommand) \
+	variable = (UAVTalkConnectionData*) handle; \
+	if (variable == NULL || variable->canari != UAVTALK_CANARI) { \
+		failcommand; \
+	}
+
+#endif // UAVTALK__PRIV_H
+/**
+ * @}
+ * @}
+ */