Merge branch 'next'

.gitignore

@@ -1,3 +1,5 @@
+# Exclude temporary and system files
+.DS_Store
 
 # /flight/
 /flight/*.pnproj

HISTORY.txt

@@ -1,5 +1,60 @@
 Short summary of changes.  For a complete list see the git log.
 
+2011-12-10
+Merged a change that sorts the UAVO fields based on size.  Because this changes
+all of the objects, erase all existing flash files based on this.
+
+2011-11-04
+New Spektrum/JR satellite receiver driver implementation.
+It now provides explicit selection of DSM2 (and DSMJ), DSMX (10bit) and
+DSMX (11bit) serial protocol variations to better serve different frame
+and resolution modes. The protocol name used now is DSM instead of
+previously used Spektrum to make it less ambiguous when used with JR
+2.4GHz radios.
+
+2011-10-20
+Inputs can be remapped to outputs to allow up to 10 channels of control.  The
+receiver inputs remap as follows:
+Receiver 3 because output channel 7
+Receiver 4 because output channel 8
+Receiver 5 because output channel 9
+Receiver 6 because output channel 10
+
+2011-10-11
+Fix for the Mac telemetry rates and specifically how long enumeration took.
+
+2011-10-08
+Make the flash chip need to be have bad magic for a full second before erasing
+settings.  Should avoid random lost settings.
+
+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).
@@ -28,3 +83,4 @@ selected from ManualControlSettings.InputMode and the aircraft must be rebooted
 after changing this.  Also for CopterControl the HwSettings object must
 indicate which modules are connected to which ports.  PPM currently not
 working.
+

MILESTONES.txt

@@ -137,11 +137,31 @@ 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
+D: August 2011
 V: http://www.youtube.com/watch?v=CugI0oBSQn8
 
+M: First CopterControl flip on a Flybarless Heli
+C: Anders Johansson (dezent)
+D: November 2011
+V: http://www.youtube.com/watch?v=Xfas2TUhOPw
+
+M: First OpenPilot over 1km FixedWing navigation flight
+C: Eric Price (Corvus Corax)
+D: December 2011
+V: http://www.youtube.com/watch?v=nWNWuUiUTNg
+
 
 M: First Altitude Hold using Sonar
 C: 
@@ -158,11 +178,6 @@ C:
 D: 
 V: 
 
-M: First CopterControl flip on a Flybarless Heli
-C: 
-D: 
-V: 
-
 
 An incomplete list of some future Milestones is below:
 
@@ -174,8 +189,6 @@ An incomplete list of some future Milestones is below:
 * First fixed wing navigation flight 
 * First Multirotor navigation flight
 * First Helicopter navigation flight
-* First over 1km navigation flight
 * First over 5km navigation flight
 * First "Follow Me" navigation flight
 * First Channel Crossing with OpenPilot
-

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/AHRS/Makefile

@@ -354,7 +354,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 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
@@ -411,7 +413,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

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
@@ -417,7 +419,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/OpenPilot/inc/pios_config.h

@@ -43,23 +43,12 @@
 #define PIOS_INCLUDE_OPAHRS
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_GPIO
-#define PIOS_NO_GPS
 //#define DEBUG_SSP
 
 /* Defaults for Logging */
 #define LOG_FILENAME 			"PIOS.LOG"
 #define STARTUP_LOG_ENABLED		1
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
-/* Servos */
-#define SERVOS_POSITION_MIN		800
-#define SERVOS_POSITION_MAX		2200
-
 #endif /* PIOS_CONFIG_H */
 /**
  * @}

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
@@ -412,7 +414,7 @@ $(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
 $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
 
 # Add jtag targets (program and wipe)
-$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE)))
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BL_BANK_BASE),$(BL_BANK_SIZE),$(OPENOCD_CONFIG)))
 
 .PHONY: elf lss sym hex bin bino
 elf: $(OUTDIR)/$(TARGET).elf

flight/Bootloaders/PipXtreme/inc/pios_config.h

@@ -40,23 +40,12 @@
 #define PIOS_INCLUDE_USB_HID
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_GPIO
-#define PIOS_NO_GPS
 //#define DEBUG_SSP
 
 /* Defaults for Logging */
 #define LOG_FILENAME 			"PIOS.LOG"
 #define STARTUP_LOG_ENABLED		1
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
-/* Servos */
-#define SERVOS_POSITION_MIN		800
-#define SERVOS_POSITION_MAX		2200
-
 #endif /* PIOS_CONFIG_H */
 /**
  * @}

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
@@ -45,7 +48,7 @@ ENABLE_DEBUG_PINS ?= NO
 # Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
 ENABLE_AUX_UART ?= NO
 
-USE_GPS ?= NO
+USE_GPS ?= YES
 
 USE_I2C ?= NO
 
@@ -62,7 +65,14 @@ endif
 FLASH_TOOL = OPENOCD
 
 # List of modules to include
-MODULES = Telemetry Attitude Stabilization Actuator ManualControl FirmwareIAP
+OPTMODULES = CameraStab
+ifeq ($(USE_GPS), YES)
+OPTMODULES += GPS
+endif
+
+MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP
+# Telemetry must be last to grab the optional modules (why?)
+MODULES += Telemetry
 
 # Paths
 OPSYSTEM = ./System
@@ -115,6 +125,7 @@ OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
 
 ifndef TESTAPP
 ## MODULES
+SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 ## OPENPILOT CORE:
 SRC += ${OPMODULEDIR}/System/systemmod.c
@@ -125,7 +136,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 +163,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)/gpsposition.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)
@@ -182,13 +193,14 @@ SRC += $(PIOSSTM32F10X)/pios_i2c.c
 SRC += $(PIOSSTM32F10X)/pios_spi.c
 SRC += $(PIOSSTM32F10X)/pios_ppm.c
 SRC += $(PIOSSTM32F10X)/pios_pwm.c
-SRC += $(PIOSSTM32F10X)/pios_spektrum.c
+SRC += $(PIOSSTM32F10X)/pios_dsm.c
 SRC += $(PIOSSTM32F10X)/pios_sbus.c
 SRC += $(PIOSSTM32F10X)/pios_debug.c
 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 +219,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
@@ -325,7 +338,7 @@ EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
 EXTRAINCDIRS  += $(AHRSBOOTLOADERINC)
 EXTRAINCDIRS  += $(PYMITEINC)
 
-EXTRAINCDIRS += ${foreach MOD, ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
+EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc
 
 
 # List any extra directories to look for library files here.
@@ -417,6 +430,10 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -DDEBUG
 endif
 
+ifeq ($(DIAGNOSTICS),YES)
+CFLAGS = -DDIAGNOSTICS
+endif
+
 CFLAGS += -g$(DEBUGF)
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
@@ -432,7 +449,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
@@ -542,7 +561,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

@@ -45,6 +45,8 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
  */
 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();

flight/CopterControl/System/coptercontrol.c

@@ -35,6 +35,7 @@
 /* OpenPilot Includes */
 #include "openpilot.h"
 #include "uavobjectsinit.h"
+#include "hwsettings.h"
 #include "systemmod.h"
 
 /* Task Priorities */
@@ -69,6 +70,12 @@ int main()
 	 * */
 	PIOS_Board_Init();
 
+#ifdef ERASE_FLASH
+	PIOS_Flash_W25X_EraseChip();
+	PIOS_LED_Off(LED1);
+	while (1) ;
+#endif
+
 	/* Initialize modules */
 	MODULE_INITIALISE_ALL
 
@@ -81,11 +88,11 @@ int main()
 	/* 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); \
+	PIOS_LED_Off(LED1);
-	for(;;) { \
+	while (1) {
-		PIOS_LED_Toggle(LED1); \
+		PIOS_LED_Toggle(LED1);
-		PIOS_DELAY_WaitmS(100); \
+		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

@@ -46,14 +46,16 @@
 #define PIOS_INCLUDE_RCVR
 
 /* Supported receiver interfaces */
-#define PIOS_INCLUDE_SPEKTRUM
+#define PIOS_INCLUDE_DSM
 #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
-//#define PIOS_INCLUDE_GPS
+#define PIOS_INCLUDE_GPS
+#define PIOS_GPS_MINIMAL
 
 #define PIOS_INCLUDE_SERVO
 #define PIOS_INCLUDE_SPI
@@ -79,12 +81,6 @@
 #define LOG_FILENAME 			"PIOS.LOG"
 #define STARTUP_LOG_ENABLED		1
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
 /* Alarm Thresholds */
 #define HEAP_LIMIT_WARNING             220
 #define HEAP_LIMIT_CRITICAL             40
@@ -106,6 +102,9 @@
 // This can't be too high to stop eventdispatcher thread overflowing
 #define PIOS_EVENTDISAPTCHER_QUEUE      10
 
+/* PIOS Initcall infrastructure */
+#define PIOS_INCLUDE_INITCALL
+
 #endif /* PIOS_CONFIG_H */
 /**
  * @}

flight/CopterControl/System/inc/pios_config_posix.h

@@ -44,11 +44,4 @@
 #define LOG_FILENAME 			"PIOS.LOG"
 #define STARTUP_LOG_ENABLED		1
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
-
 #endif /* PIOS_CONFIG_POSIX_H */

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,347 @@ 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,
+			},
+		},
+	},
+};
+
+
+static const struct pios_tim_channel pios_tim_servoport_rcvrport_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,
+			},
+		},
+	},
+
+	// Receiver port pins
+	// S3-S6 inputs are used as outputs in this case
+	{
+		.timer = TIM3,
+		.timer_chan = TIM_Channel_3,
+		.pin = {
+			.gpio = GPIOB,
+			.init = {
+				.GPIO_Pin   = GPIO_Pin_0,
+				.GPIO_Mode  = GPIO_Mode_AF_PP,
+				.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_AF_PP,
+				.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_AF_PP,
+				.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_AF_PP,
+				.GPIO_Speed = GPIO_Speed_2MHz,
+			},
+		},
+	},
+};
 #if defined(PIOS_INCLUDE_USART)
 
 #include "pios_usart_priv.h"
@@ -353,13 +695,13 @@ static const struct pios_usart_cfg pios_usart_gps_flexi_cfg = {
 };
 #endif	/* PIOS_INCLUDE_GPS */
 
-#if defined(PIOS_INCLUDE_SPEKTRUM)
+#if defined(PIOS_INCLUDE_DSM)
 /*
- * SPEKTRUM USART
+ * Spektrum/JR DSM USART
  */
-#include <pios_spektrum_priv.h>
+#include <pios_dsm_priv.h>
 
-static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = {
+static const struct pios_usart_cfg pios_usart_dsm_main_cfg = {
 	.regs = USART1,
 	.init = {
 		.USART_BaudRate            = 115200,
@@ -395,7 +737,7 @@ static const struct pios_usart_cfg pios_usart_spektrum_main_cfg = {
 	},
 };
 
-static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
+static const struct pios_dsm_cfg pios_dsm_main_cfg = {
 	.bind = {
 		.gpio = GPIOA,
 		.init = {
@@ -404,10 +746,9 @@ static const struct pios_spektrum_cfg pios_spektrum_main_cfg = {
 			.GPIO_Mode  = GPIO_Mode_Out_PP,
 		},
 	},
-	.remap = 0,
 };
 
-static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
+static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = {
 	.regs = USART3,
 	.init = {
 		.USART_BaudRate            = 115200,
@@ -443,7 +784,7 @@ static const struct pios_usart_cfg pios_usart_spektrum_flexi_cfg = {
 	},
 };
 
-static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = {
+static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
 	.bind = {
 		.gpio = GPIOB,
 		.init = {
@@ -452,14 +793,13 @@ static const struct pios_spektrum_cfg pios_spektrum_flexi_cfg = {
 			.GPIO_Mode  = GPIO_Mode_Out_PP,
 		},
 	},
-	.remap = 0,
 };
 
-#endif	/* PIOS_INCLUDE_SPEKTRUM */
+#endif	/* PIOS_INCLUDE_DSM */
 
 #if defined(PIOS_INCLUDE_SBUS)
 /*
- * SBUS USART
+ * S.Bus USART
  */
 #include <pios_sbus_priv.h>
 
@@ -525,7 +865,7 @@ static const struct pios_sbus_cfg pios_sbus_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
+#define PIOS_COM_GPS_RX_BUF_LEN 32
 
 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192
 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192
@@ -564,53 +904,8 @@ 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,
@@ -621,18 +916,25 @@ 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 = GPIO_PartialRemap_TIM3,
+const struct pios_servo_cfg pios_servo_rcvr_cfg = {
-	.channels = pios_servo_channels,
+	.tim_oc_init = {
-	.num_channels = NELEMENTS(pios_servo_channels),
+		.TIM_OCMode = TIM_OCMode_PWM1,
+		.TIM_OutputState = TIM_OutputState_Enable,
+		.TIM_OutputNState = TIM_OutputNState_Disable,
+		.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,
+	},
+	.channels = pios_tim_servoport_rcvrport_pins,
+	.num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins),
 };
 
-#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
@@ -640,45 +942,18 @@ const struct pios_servo_cfg pios_servo_cfg = {
 #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 +962,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,
 	},
-	.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 +1049,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, DSMMAINPORT, DSMFLEXIPORT, 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,7 +1108,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 */
@@ -920,7 +1122,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);
 
@@ -956,9 +1166,16 @@ void PIOS_Board_Init(void) {
 			}
 
 			uint32_t pios_sbus_id;
-			if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
+			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,20 +1197,48 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_GPS */
 		break;
-	case HWSETTINGS_CC_MAINPORT_SPEKTRUM:
+	case HWSETTINGS_CC_MAINPORT_DSM2:
-#if defined(PIOS_INCLUDE_SPEKTRUM)
+	case HWSETTINGS_CC_MAINPORT_DSMX10BIT:
+	case HWSETTINGS_CC_MAINPORT_DSMX11BIT:
+#if defined(PIOS_INCLUDE_DSM)
 		{
-			uint32_t pios_usart_spektrum_id;
+			enum pios_dsm_proto proto;
-			if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) {
+			switch (hwsettings_cc_mainport) {
+			case HWSETTINGS_CC_MAINPORT_DSM2:
+				proto = PIOS_DSM_PROTO_DSM2;
+				break;
+			case HWSETTINGS_CC_MAINPORT_DSMX10BIT:
+				proto = PIOS_DSM_PROTO_DSMX10BIT;
+				break;
+			case HWSETTINGS_CC_MAINPORT_DSMX11BIT:
+				proto = PIOS_DSM_PROTO_DSMX11BIT;
+				break;
+			default:
+				PIOS_Assert(0);
+				break;
+			}
+
+			uint32_t pios_usart_dsm_id;
+			if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) {
 				PIOS_Assert(0);
 			}
 
-			uint32_t pios_spektrum_id;
+			uint32_t pios_dsm_id;
-			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
+			if (PIOS_DSM_Init(&pios_dsm_id,
+					  &pios_dsm_main_cfg,
+					  &pios_usart_com_driver,
+					  pios_usart_dsm_id,
+					  proto, 0)) {
 				PIOS_Assert(0);
 			}
+
+			uint32_t pios_dsm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
 		}
-#endif	/* PIOS_INCLUDE_SPEKTRUM */
+#endif	/* PIOS_INCLUDE_DSM */
 		break;
 	case HWSETTINGS_CC_MAINPORT_COMAUX:
 		break;
@@ -1042,20 +1287,48 @@ void PIOS_Board_Init(void) {
 		}
 #endif	/* PIOS_INCLUDE_GPS */
 		break;
-	case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM:
+	case HWSETTINGS_CC_FLEXIPORT_DSM2:
-#if defined(PIOS_INCLUDE_SPEKTRUM)
+	case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT:
+	case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT:
+#if defined(PIOS_INCLUDE_DSM)
 		{
-			uint32_t pios_usart_spektrum_id;
+			enum pios_dsm_proto proto;
-			if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_flexi_cfg)) {
+			switch (hwsettings_cc_flexiport) {
+			case HWSETTINGS_CC_FLEXIPORT_DSM2:
+				proto = PIOS_DSM_PROTO_DSM2;
+				break;
+			case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT:
+				proto = PIOS_DSM_PROTO_DSMX10BIT;
+				break;
+			case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT:
+				proto = PIOS_DSM_PROTO_DSMX11BIT;
+				break;
+			default:
+				PIOS_Assert(0);
+				break;
+			}
+
+			uint32_t pios_usart_dsm_id;
+			if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) {
 				PIOS_Assert(0);
 			}
 
-			uint32_t pios_spektrum_id;
+			uint32_t pios_dsm_id;
-			if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) {
+			if (PIOS_DSM_Init(&pios_dsm_id,
+					  &pios_dsm_flexi_cfg,
+					  &pios_usart_com_driver,
+					  pios_usart_dsm_id,
+					  proto, hwsettings_DSMxBind)) {
 				PIOS_Assert(0);
 			}
+
+			uint32_t pios_dsm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id;
 		}
-#endif	/* PIOS_INCLUDE_SPEKTRUM */
+#endif	/* PIOS_INCLUDE_DSM */
 		break;
 	case HWSETTINGS_CC_FLEXIPORT_COMAUX:
 		break;
@@ -1070,83 +1343,72 @@ 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:
+	case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
 #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
+	switch (hwsettings_rcvrport) {
+		case HWSETTINGS_CC_RCVRPORT_DISABLED:
+		case HWSETTINGS_CC_RCVRPORT_PWM:
+		case HWSETTINGS_CC_RCVRPORT_PPM:
+			PIOS_Servo_Init(&pios_servo_cfg);
+			break;
+		case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS:
+		case HWSETTINGS_CC_RCVRPORT_OUTPUTS:
+			PIOS_Servo_Init(&pios_servo_rcvr_cfg);
+			break;
+	}
+#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();
@@ -1168,7 +1430,9 @@ void PIOS_Board_Init(void) {
 #endif	/* PIOS_INCLUDE_USB_HID */
 
 	PIOS_IAP_Init();
+#ifndef ERASE_FLASH
 	PIOS_WDG_Init();
+#endif
 }
 
 /**

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/INS/inc/pios_config.h

@@ -54,13 +54,7 @@
 
 #define PIOS_INCLUDE_BMA180
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
 #endif /* PIOS_CONFIG_H */
-
 /**
  * @}
  * @}

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
@@ -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,6 +109,14 @@ 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);
 
@@ -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()) //Nothing can fly with less than two mixers.
 		{
 			setFailsafe(); // So that channels like PWM buzzer keep working
 			continue;
@@ -208,22 +219,29 @@ static void actuatorTask(void* parameters)
 		bool positiveThrottle = desired.Throttle >= 0.00;
 		bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
 
-		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
+		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM);
+		
 		//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,7 +250,7 @@ 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
 					curve2 = 0;
 				break;
@@ -247,7 +265,12 @@ static void actuatorTask(void* parameters)
 				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) {
@@ -273,20 +296,44 @@ static void actuatorTask(void* parameters)
 			// the correct behavior is since it seems domain specific.  I don't love
 			// this code
 			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;
@@ -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));

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

@@ -54,6 +54,7 @@
 #include "attitudeactual.h"
 #include "attitudesettings.h"
 #include "flightstatus.h"
+#include "manualcontrolcommand.h"
 #include "CoordinateConversions.h"
 #include "pios_flash_w25x.h"
 
@@ -91,6 +92,16 @@ static int8_t rotate = 0;
 static bool zero_during_arming = false;
 static bool bias_correct_gyro = true;
 
+// For running trim flights
+static volatile bool trim_requested = false;
+static volatile int32_t trim_accels[3];
+static volatile int32_t trim_samples;
+int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
+
+#define GRAV         9.81f
+#define ACCEL_SCALE  (GRAV * 0.004f)
+/* 0.004f is gravity / LSB */
+
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
@@ -112,6 +123,10 @@ int32_t AttitudeStart(void)
  */
 int32_t AttitudeInitialize(void)
 {
+	AttitudeActualInitialize();
+	AttitudeRawInitialize();
+	AttitudeSettingsInitialize();
+	
 	// Initialize quaternion
 	AttitudeActualData attitude;
 	AttitudeActualGet(&attitude);
@@ -134,6 +149,8 @@ int32_t AttitudeInitialize(void)
 		for(uint8_t j = 0; j < 3; j++)
 			R[i][j] = 0;
 	
+	trim_requested = false;
+	
 	// Create queue for passing gyro data, allow 2 back samples in case
 	gyro_queue = xQueueCreate(1, sizeof(float) * 4);
 	if(gyro_queue == NULL)
@@ -273,10 +290,28 @@ static int8_t updateSensors(AttitudeRawData * attitudeRaw)
 		attitudeRaw->accels[2] = accel[2];
 	}
 	
+	if (trim_requested) {
+		if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) {
+			trim_requested = false;
+		} else {
+			uint8_t armed;
+			float throttle;
+			FlightStatusArmedGet(&armed);
+			ManualControlCommandThrottleGet(&throttle);  // Until flight status indicates airborne
+			if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
+				trim_samples++;
+				// Store the digitally scaled version since that is what we use for bias
+				trim_accels[0] += attitudeRaw->accels[ATTITUDERAW_ACCELS_X];
+				trim_accels[1] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Y];
+				trim_accels[2] += attitudeRaw->accels[ATTITUDERAW_ACCELS_Z];	
+			}
+		}
+	}
+	
 	// Scale accels and correct bias
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * 0.004f * 9.81f;
+	attitudeRaw->accels[ATTITUDERAW_ACCELS_X] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_X] - accelbias[0]) * ACCEL_SCALE;
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * 0.004f * 9.81f;
+	attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Y] - accelbias[1]) * ACCEL_SCALE;
-	attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * 0.004f * 9.81f;
+	attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] = (attitudeRaw->accels[ATTITUDERAW_ACCELS_Z] - accelbias[2]) * ACCEL_SCALE;
 	
 	if(bias_correct_gyro) {
 		// Applying integral component here so it can be seen on the gyros and correct bias
@@ -328,7 +363,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
 		gyro_correct_int[0] += accel_err[0] * accelKi;
 		gyro_correct_int[1] += accel_err[1] * accelKi;
 		
-		//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
+		//gyro_correct_int[2] += accel_err[2] * accelKi;
 		
 		// Correct rates based on error, integral component dealt with in updateSensors
 		gyro[0] += accel_err[0] * accelKp / dT;
@@ -424,8 +459,25 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
 		Quaternion2R(rotationQuat, R);
 		rotate = 1;
 	}
+	
+	if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) {
+		trim_accels[0] = 0;
+		trim_accels[1] = 0;
+		trim_accels[2] = 0;
+		trim_samples = 0;
+		trim_requested = true;
+	} else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
+		trim_requested = false;
+		attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_X] = trim_accels[0] / trim_samples;
+		attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Y] = trim_accels[1] / trim_samples;
+		// Z should average -grav
+		attitudeSettings.AccelBias[ATTITUDESETTINGS_ACCELBIAS_Z] = trim_accels[2] / trim_samples + GRAV / ACCEL_SCALE;
+		attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
+		AttitudeSettingsSet(&attitudeSettings);
+	} else
+		trim_requested = false;
 }
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

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,169 @@
+/**
+ ******************************************************************************
+ * @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"
+#include "hwsettings.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;
+
+	bool cameraStabEnabled;
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+
+	HwSettingsInitialize();
+	HwSettingsOptionalModulesGet(optionalModules);
+
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTAB] == HWSETTINGS_OPTIONALMODULES_ENABLED)
+		cameraStabEnabled = true;
+	else
+		cameraStabEnabled = false;
+
+	if (cameraStabEnabled) {
+
+		AttitudeActualInitialize();
+
+		ev.obj = AttitudeActualHandle();
+		ev.instId = 0;
+		ev.event = 0;
+
+		CameraStabSettingsInitialize();
+		CameraDesiredInitialize();
+
+		EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
+
+		return 0;
+	}
+
+	return -1;
+}
+
+/* stub: module has no module thread */
+int32_t CameraStabStart(void)
+{
+	return 0;
+}
+
+MODULE_INITCALL(CameraStabInitialize, CameraStabStart)
+
+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

@@ -2,13 +2,13 @@
  ******************************************************************************
  * @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

@@ -75,6 +75,10 @@ int32_t FlightPlanInitialize()
 {
 	taskHandle = NULL;
 	
+	FlightPlanStatusInitialize();
+	FlightPlanControlInitialize();
+	FlightPlanSettingsInitialize();
+	
 	// Listen for object updates
 	FlightPlanControlConnectCallback(&objectUpdatedCb);
 

flight/Modules/GPS/GPS.c

@@ -35,24 +35,22 @@
 
 #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"
+#include "hwsettings.h"
+
 
 // ****************
 // Private functions
 
 static void gpsTask(void *parameters);
+static void updateSettings();
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 static void setHomeLocation(GPSPositionData * gpsData);
@@ -62,25 +60,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)
@@ -89,12 +78,11 @@ static float GravityAccel(float latitude, float longitude, float altitude);
 // Private variables
 
 static uint32_t gpsPort;
+static bool gpsEnabled = false;
 
 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;
@@ -111,12 +99,19 @@ static uint32_t numParsingErrors;
 
 int32_t GPSStart(void)
 {
-	// Start gps task
+	if (gpsEnabled) {
-	xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
+		if (gpsPort) {
-	TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
+			// Start gps task
+			xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
+			TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
+			return 0;
+		}
 
-	return 0;
+		AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
+	}
+	return -1;
 }
+
 /**
  * Initialise the gps module
  * \return -1 if initialisation failed
@@ -124,11 +119,39 @@ int32_t GPSStart(void)
  */
 int32_t GPSInitialize(void)
 {
-	// TODO: Get gps settings object
 	gpsPort = PIOS_COM_GPS;
 
-	return 0;
+	HwSettingsInitialize();
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+
+	HwSettingsOptionalModulesGet(optionalModules);
+
+	if (optionalModules[HWSETTINGS_OPTIONALMODULES_GPS] == HWSETTINGS_OPTIONALMODULES_ENABLED)
+		gpsEnabled = true;
+	else
+		gpsEnabled = false;
+
+	if (gpsPort && gpsEnabled) {
+		GPSPositionInitialize();
+#if !defined(PIOS_GPS_MINIMAL)
+		GPSTimeInitialize();
+		GPSSatellitesInitialize();
+#endif
+#ifdef PIOS_GPS_SETS_HOMELOCATION
+		HomeLocationInitialize();
+#endif
+		HwSettingsInitialize();
+		updateSettings();
+
+		gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
+		PIOS_Assert(gps_rx_buffer);
+
+		return 0;
+	}
+
+	return -1;
 }
+
 MODULE_INITCALL(GPSInitialize, GPSStart)
 
 // ****************
@@ -142,44 +165,10 @@ 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;
@@ -191,108 +180,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 (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0)
+			if (rx_count >= NMEA_MAX_PACKET_LENGTH)
-				{
+			{
-					numUpdates++;
+				// 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;
 				}
 			}
-
+		}
-		#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 +273,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 +298,6 @@ static void gpsTask(void *parameters)
 				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
 		}
 
-		// Block task until next update
-		vTaskDelay(xDelay);
 	}
 }
 
@@ -417,7 +359,47 @@ static void setHomeLocation(GPSPositionData * gpsData)
 }
 #endif
 
-// ****************
+/**
+ * Update the GPS settings, called on startup.
+ * FIXME: This should be in the GPSSettings object. But objects have
+ * too much overhead yet. Also the GPS has no any specific settings
+ * like protocol, etc. Thus the HwSettings object which contains the
+ * GPS port speed is used for now.
+ */
+static void updateSettings()
+{
+	if (gpsPort) {
+
+		// Retrieve settings
+		uint8_t speed;
+		HwSettingsGPSSpeedGet(&speed);
+
+		// Set port speed
+		switch (speed) {
+		case HWSETTINGS_GPSSPEED_2400:
+			PIOS_COM_ChangeBaud(gpsPort, 2400);
+			break;
+		case HWSETTINGS_GPSSPEED_4800:
+			PIOS_COM_ChangeBaud(gpsPort, 4800);
+			break;
+		case HWSETTINGS_GPSSPEED_9600:
+			PIOS_COM_ChangeBaud(gpsPort, 9600);
+			break;
+		case HWSETTINGS_GPSSPEED_19200:
+			PIOS_COM_ChangeBaud(gpsPort, 19200);
+			break;
+		case HWSETTINGS_GPSSPEED_38400:
+			PIOS_COM_ChangeBaud(gpsPort, 38400);
+			break;
+		case HWSETTINGS_GPSSPEED_57600:
+			PIOS_COM_ChangeBaud(gpsPort, 57600);
+			break;
+		case HWSETTINGS_GPSSPEED_115200:
+			PIOS_COM_ChangeBaud(gpsPort, 115200);
+			break;
+		}
+	}
+}
 
 /** 
   * @}

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,49 @@ 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);
+#if !defined(PIOS_GPS_MINIMAL)
-		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
+#endif //PIOS_GPS_MINIMAL
 
-	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,
+#if !defined(PIOS_GPS_MINIMAL)
-		},
+	{
-		{
+		.prefix = "GPZDA",
-			.prefix = "GPZDA",
+		.handler = nmeaProcessGPZDA,
-			.handler = nmeaProcessGPZDA,
+		.cnt = 0,
-			.cnt = 0,
+	},
-		},
+	{
-		{
+		.prefix = "GPGSV",
-			.prefix = "GPGSV",
+		.handler = nmeaProcessGPGSV,
-			.handler = nmeaProcessGPGSV,
+		.cnt = 0,
-			.cnt = 0,
+	},
-		},
+#endif //PIOS_GPS_MINIMAL
-	#endif
-        {
-         .prefix = "PGTOP",
-         .handler = nmeaProcessPGTOP,
-         .cnt = 0,
-         },
 };
 
 static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
@@ -229,7 +219,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 +276,6 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool
 
 	return true;
 }
-#endif	// ENABLE_GPS_NMEA
 
 
 /**
@@ -376,7 +364,6 @@ bool NMEA_update_position(char *nmea_sentence)
 	return true;
 }
 
-#ifdef ENABLE_GPS_NMEA
 
 /**
  * Parse an NMEA GPGGA sentence and update the given UAVObject
@@ -445,6 +432,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 
 	*gpsDataUpdated = true;
 
+#if !defined(PIOS_GPS_MINIMAL)
 	GPSTimeData gpst;
 	GPSTimeGet(&gpst);
 
@@ -453,6 +441,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	gpst.Second = (int)hms % 100;
 	gpst.Minute = (((int)hms - gpst.Second) / 100) % 100;
 	gpst.Hour = (int)hms / 10000;
+#endif //PIOS_GPS_MINIMAL
 
 	// get latitude [DDMM.mmmmm] [N|S]
 	if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) {
@@ -470,6 +459,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	// get True course
 	GpsData->Heading = NMEA_real_to_float(param[8]);
 
+#if !defined(PIOS_GPS_MINIMAL)
 	// get Date of fix
 	// TODO: Should really not use a float here to be safe
 	float date = NMEA_real_to_float(param[9]);
@@ -478,6 +468,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	gpst.Day = (int)(date / 10000);
 	gpst.Year += 2000;
 	GPSTimeSet(&gpst);
+#endif //PIOS_GPS_MINIMAL
 
 	return true;
 }
@@ -505,6 +496,7 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 	return true;
 }
 
+#if !defined(PIOS_GPS_MINIMAL)
 /**
  * Parse an NMEA GPZDA sentence and update the @ref GPSTime object
  * \param[in] A pointer to a GPSPosition UAVObject to be updated (unused).
@@ -627,6 +619,7 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
 
 	return true;
 }
+#endif //PIOS_GPS_MINIMAL
 
 /**
  * Parse an NMEA GPGSA sentence and update the given UAVObject
@@ -675,83 +668,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,48 +177,90 @@ static void manualControlTask(void *parameters)
 		// Read settings
 		ManualControlSettingsGet(&settings);
 
-		if (ManualControlCommandReadOnly(&cmd)) {
+		/* 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()) {
 			FlightTelemetryStatsData flightTelemStats;
 			FlightTelemetryStatsGet(&flightTelemStats);
 			if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
 				/* trying to fly via GCS and lost connection.  fall back to transmitter */
 				UAVObjMetadata metadata;
-				UAVObjGetMetadata(&cmd, &metadata);
+				ManualControlCommandGetMetadata(&metadata);
 				metadata.access = ACCESS_READWRITE;
-				UAVObjSetMetadata(&cmd, &metadata);
+				ManualControlCommandSetMetadata(&metadata);
 			}
 		}
 
-		if (!ManualControlCommandReadOnly(&cmd)) {
+		if (!ManualControlCommandReadOnly()) {
+
+			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_DSMMAINPORT:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
+				break;
+			case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
+				group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
+				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((uint8_t*)&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,10 +43,11 @@
 #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"
 
 
 // Private constants
@@ -73,16 +74,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 +93,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 +110,17 @@ int32_t SystemModStart(void)
 int32_t SystemModInitialize(void)
 {
 
+	// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
+	SystemSettingsInitialize();
+	SystemStatsInitialize();
+	FlightStatusInitialize();
+	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
@@ -369,25 +389,24 @@ static void updateSystemAlarms()
 	EventStats evStats;
 	SystemStatsGet(&stats);
 
-	// Check heap
+	// Check heap, IRQ stack and malloc failures
-	if (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) {
+	if ( mallocFailed
-		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
+	     || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)
-	} else if (stats.HeapRemaining < HEAP_LIMIT_WARNING) {
-		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
-	} else {
-		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
-	}
-
 #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
-	// Check IRQ stack
+	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)
-	if (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) {
+#endif
+	    ) {
 		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
-	} else if (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) {
+	} else if (
+		(stats.HeapRemaining < HEAP_LIMIT_WARNING)
+#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
+	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)
+#endif
+	    ) {
 		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
 	} else {
 		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
 	}
-#endif
 
 	// Check CPU load
 	if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
@@ -399,7 +418,7 @@ static void updateSystemAlarms()
 	}
 
 	// Check for stack overflow
-	if (stackOverflow == 1) {
+	if (stackOverflow) {
 		AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
 	} else {
 		AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
@@ -443,9 +462,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

@@ -34,7 +34,7 @@
 #include "telemetry.h"
 #include "flighttelemetrystats.h"
 #include "gcstelemetrystats.h"
-#include "telemetrysettings.h"
+#include "hwsettings.h"
 
 // Private constants
 #define MAX_QUEUE_SIZE   TELEM_QUEUE_SIZE
@@ -65,8 +65,8 @@ static xTaskHandle telemetryTxTaskHandle;
 static xTaskHandle telemetryRxTaskHandle;
 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,6 +88,11 @@ 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);
     
 	// Start telemetry tasks
 	xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
@@ -110,7 +115,8 @@ int32_t TelemetryStart(void)
  */
 int32_t TelemetryInitialize(void)
 {
-	UAVObjEvent ev;
+	FlightTelemetryStatsInitialize();
+	GCSTelemetryStatsInitialize();
 
 	// Initialize vars
 	timeOfLastObjectUpdate = 0;
@@ -121,25 +127,21 @@ int32_t TelemetryInitialize(void)
 	priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 #endif
 
-	// Get telemetry settings object
+	// Update telemetry settings
+	telemetryPort = PIOS_COM_TELEM_RF;
+	HwSettingsInitialize();
 	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;
+	UAVObjEvent ev;
 	memset(&ev, 0, sizeof(UAVObjEvent));
 	EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
 
-	// Listen to objects of interest
-	GCSTelemetryStatsConnectQueue(priorityQueue);
-	TelemetrySettingsConnectQueue(priorityQueue);
-
 	return 0;
 }
 
@@ -221,8 +223,6 @@ static void processObjEvent(UAVObjEvent * ev)
 		updateTelemetryStats();
 	} else if (ev->obj == GCSTelemetryStatsHandle()) {
 		gcsTelemetryStatsUpdated();
-	} else if (ev->obj == TelemetrySettingsHandle()) {
-		updateSettings();
 	} else {
 		// Only process event if connected to GCS or if object FlightTelemetryStats is updated
 		FlightTelemetryStatsGet(&flightStats);
@@ -235,7 +235,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 +246,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 +326,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 +426,8 @@ static void updateTelemetryStats()
 	uint32_t timeNow;
 
 	// Get stats
-	UAVTalkGetStats(&utalkStats);
+	UAVTalkGetStats(uavTalkCon, &utalkStats);
-	UAVTalkResetStats();
+	UAVTalkResetStats(uavTalkCon);
 
 	// Get object data
 	FlightTelemetryStatsGet(&flightStats);
@@ -504,27 +504,45 @@ static void updateTelemetryStats()
 }
 
 /**
- * Update the telemetry settings, called on startup and
+ * Update the telemetry settings, called on startup.
- * each time the settings object is updated
+ * FIXME: This should be in the TelemetrySettings object. But objects
+ * have too much overhead yet. Also the telemetry has no any specific
+ * settings, etc. Thus the HwSettings object which contains the
+ * telemetry port speed is used for now.
  */
 static void updateSettings()
 {
-    // Set port
+	if (telemetryPort) {
-    telemetryPort = PIOS_COM_TELEM_RF;
 
-    // Retrieve settings
+		// Retrieve settings
-    TelemetrySettingsGet(&settings);
+		uint8_t speed;
+		HwSettingsTelemetrySpeedGet(&speed);
 
-    if (telemetryPort) {
+		// Set port speed
-	// Set port speed
+		switch (speed) {
-	if (settings.Speed == TELEMETRYSETTINGS_SPEED_2400) PIOS_COM_ChangeBaud(telemetryPort, 2400);
+		case HWSETTINGS_TELEMETRYSPEED_2400:
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_4800) PIOS_COM_ChangeBaud(telemetryPort, 4800);
+			PIOS_COM_ChangeBaud(telemetryPort, 2400);
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) PIOS_COM_ChangeBaud(telemetryPort, 9600);
+			break;
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_19200) PIOS_COM_ChangeBaud(telemetryPort, 19200);
+		case HWSETTINGS_TELEMETRYSPEED_4800:
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400) PIOS_COM_ChangeBaud(telemetryPort, 38400);
+			PIOS_COM_ChangeBaud(telemetryPort, 4800);
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) PIOS_COM_ChangeBaud(telemetryPort, 57600);
+			break;
-	else if (settings.Speed == TELEMETRYSETTINGS_SPEED_115200) PIOS_COM_ChangeBaud(telemetryPort, 115200);
+		case HWSETTINGS_TELEMETRYSPEED_9600:
-    }
+			PIOS_COM_ChangeBaud(telemetryPort, 9600);
+			break;
+		case HWSETTINGS_TELEMETRYSPEED_19200:
+			PIOS_COM_ChangeBaud(telemetryPort, 19200);
+			break;
+		case HWSETTINGS_TELEMETRYSPEED_38400:
+			PIOS_COM_ChangeBaud(telemetryPort, 38400);
+			break;
+		case HWSETTINGS_TELEMETRYSPEED_57600:
+			PIOS_COM_ChangeBaud(telemetryPort, 57600);
+			break;
+		case HWSETTINGS_TELEMETRYSPEED_115200:
+			PIOS_COM_ChangeBaud(telemetryPort, 115200);
+			break;
+		}
+	}
 }
 
 /**

flight/OpenPilot/Makefile

@@ -37,6 +37,9 @@ OUTDIR := $(TOP)/build/$(TARGET)
 # Set to YES to compile for debugging
 DEBUG ?= YES
 
+# Include objects that are just nice information to show
+DIAGNOSTICS ?= YES
+
 # Set to YES to use the Servo output pins for debugging via scope or logic analyser
 ENABLE_DEBUG_PINS ?= NO
 
@@ -55,13 +58,16 @@ endif
 
 FLASH_TOOL = OPENOCD
 
+
 # List of modules to include
-MODULES = Actuator Telemetry GPS ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
+OPTMODULES = CameraStab GPS
+MODULES = Actuator ManualControl Altitude AHRSComms Stabilization Guidance FirmwareIAP
 PYMODULES = FlightPlan
 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
 #MODULES = Telemetry
 #MODULES += Osd/OsdEtStd
+MODULES += Telemetry
 
 # Paths
 OPSYSTEM = ./System
@@ -125,6 +131,7 @@ PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 SRC += $(PYSRC)
 
 ## MODULES
+SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 ## OPENPILOT CORE:
 SRC += ${OPMODULEDIR}/System/systemmod.c
@@ -135,7 +142,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
@@ -163,8 +169,9 @@ SRC += $(PIOSSTM32F10X)/pios_i2c.c
 SRC += $(PIOSSTM32F10X)/pios_spi.c
 SRC += $(PIOSSTM32F10X)/pios_ppm.c
 SRC += $(PIOSSTM32F10X)/pios_pwm.c
-SRC += $(PIOSSTM32F10X)/pios_spektrum.c
+SRC += $(PIOSSTM32F10X)/pios_dsm.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
@@ -307,7 +314,7 @@ EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
 EXTRAINCDIRS  += $(AHRSBOOTLOADERINC)
 EXTRAINCDIRS  += $(PYMITEINC)
 
-EXTRAINCDIRS += ${foreach MOD, ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
+EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
 
 
 # List any extra directories to look for library files here.
@@ -385,9 +392,14 @@ CSTANDARD = -std=gnu99
 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
 
 ifeq ($(DEBUG),YES)
-CFLAGS = -g$(DEBUGF) -DDEBUG
+CFLAGS = -DDEBUG
 endif
 
+ifeq ($(DIAGNOSTICS),YES)
+CFLAGS = -DDIAGNOSTICS
+endif
+
+CFLAGS += -g$(DEBUGF)
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
 CFLAGS += $(CDEFS)
@@ -469,9 +481,6 @@ endif
 endif
 
 # Generate intermediate code
-gencode: ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h 
-
-$(PYSRC): gencode
 
 # 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) 
@@ -512,7 +521,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
@@ -579,4 +588,4 @@ else
 endif
 
 # Listing of phony targets.
-.PHONY : all build clean clean_list gencode install
+.PHONY : all build clean clean_list install

flight/OpenPilot/Makefile.posix

@@ -27,6 +27,9 @@
 # Set to YES to compile for debugging
 DEBUG ?= YES
 
+# Include objects that are just nice information to show
+DIAGNOSTICS ?= YES
+
 # Set to YES to use the Servo output pins for debugging via scope or logic analyser
 ENABLE_DEBUG_PINS ?= NO
 
@@ -53,10 +56,12 @@ FLASH_TOOL = OPENOCD
 USE_THUMB_MODE = YES
 
 # List of modules to include
-MODULES =  Telemetry Actuator Stabilization Guidance ManualControl FlightPlan GPS
+OPTMODULES = CameraStab GPS
+MODULES =  Telemetry Actuator Stabilization Guidance ManualControl 
 #MODULES =  Telemetry ManualControl Actuator Attitude Stabilization
 #MODULES = Telemetry Example
 #MODULES = Telemetry MK/MKSerial
+PYMODULES = FlightPlan
 
 #MODULES += Osd/OsdEtStd
 
@@ -121,6 +126,7 @@ UAVOBJPYTHONSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/python
 # use file-extension c for "c-only"-files
 
 MODNAMES = $(notdir ${MODULES})
+MODNAMES += $(notdir ${OPTMODULES})
 
 ifndef TESTAPP
 
@@ -129,10 +135,13 @@ SRC += $(OUTDIR)/pmlib_img.c
 SRC += $(OUTDIR)/pmlib_nat.c
 SRC += $(OUTDIR)/pmlibusr_img.c
 SRC += $(OUTDIR)/pmlibusr_nat.c
-SRC += $(wildcard ${PYMITEVM}/*.c)
+PYSRC += $(wildcard ${PYMITEVM}/*.c)
-SRC += $(wildcard ${PYMITEPLAT}/*.c)
+PYSRC += $(wildcard ${PYMITEPLAT}/*.c)
+PYSRC += ${foreach MOD, ${PYMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
+SRC += $(PYSRC)
 
 ## MODULES
+SRC += ${foreach MOD, ${OPTMODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
 SRC += ${OUTDIR}/InitMods.c
 ## OPENPILOT CORE:
@@ -228,7 +237,7 @@ EXTRAINCDIRS  += $(APPLIBDIR)
 EXTRAINCDIRS  += $(RTOSSRCDIR)/portable/GCC/Posix
 EXTRAINCDIRS  += $(PYMITEINC)
 
-EXTRAINCDIRS += ${foreach MOD, ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
+EXTRAINCDIRS += ${foreach MOD, ${PYMODULES} ${OPTMODULES} ${MODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc
 
 
 # List any extra directories to look for library files here.
@@ -306,6 +315,10 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -g$(DEBUGF) -DDEBUG
 endif
 
+ifeq ($(DIAGNOSTICS),YES)
+CFLAGS = -DDIAGNOSTICS
+endif
+
 CFLAGS += $(CFLAGS_UAVOBJECTS)
 CFLAGS += -DARCH_POSIX
 CFLAGS += -O$(OPT)
@@ -436,9 +449,6 @@ else
 	quote =
 endif
 
-# Generate intermediate code
-gencode: ${OUTDIR}/InitMods.c ${OUTDIR}/pmlib_img.c ${OUTDIR}/pmlib_nat.c ${OUTDIR}/pmlibusr_img.c ${OUTDIR}/pmlibusr_nat.c ${OUTDIR}/pmfeatures.h 
-
 # Generate code for module initialization
 ${OUTDIR}/InitMods.c: Makefile.posix
 	@echo ${MSG_MODINIT}
@@ -453,9 +463,9 @@ ${OUTDIR}/InitMods.c: Makefile.posix
 	@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
 
 # 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) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.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}
+	@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) $(wildcard $(UAVOBJPYTHONSYNTHDIR)/*.py)
+	@$(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
 	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -u -o $(OUTDIR)/pmlibusr_img.c --native-file=$(OUTDIR)/pmlibusr_nat.c $(FLIGHTPLANS)/test.py
 
@@ -651,5 +661,5 @@ endif
 
 # Listing of phony targets.
 .PHONY : all begin finish end sizebefore sizeafter gccversion \
-build elf hex bin lss sym clean clean_list program gencode
+build elf hex bin lss sym clean clean_list program
 

flight/OpenPilot/System/alarms.c

@@ -45,10 +45,8 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
  */
 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;
 }
 

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

@@ -44,10 +44,10 @@
 
 #define PIOS_INCLUDE_RCVR
 
-#define PIOS_INCLUDE_SPEKTRUM
+#define PIOS_INCLUDE_DSM
 //#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
@@ -77,12 +78,6 @@
 /* Enable a priority queue in telemetry */
 #define PIOS_TELEM_PRIORITY_QUEUE
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
 /* Alarm Thresholds */
 #define HEAP_LIMIT_WARNING		4000
 #define HEAP_LIMIT_CRITICAL		1000
@@ -97,6 +92,8 @@
 /* GPS options */
 #define PIOS_GPS_SETS_HOMELOCATION
 
+/* PIOS Initcall infrastructure */
+#define PIOS_INCLUDE_INITCALL
 
 #endif /* PIOS_CONFIG_H */
 /**

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
@@ -49,18 +50,28 @@
 #define LOG_FILENAME 			"PIOS.LOG"
 #define STARTUP_LOG_ENABLED		1
 
-/* COM Module */
-#define GPS_BAUDRATE			19200
-#define TELEM_BAUDRATE			19200
-#define AUXUART_ENABLED			0
-#define AUXUART_BAUDRATE		19200
-
 #define TELEM_QUEUE_SIZE                20
 #define PIOS_TELEM_STACK_SIZE           2048
 
+/* Alarm Thresholds */
+#define HEAP_LIMIT_WARNING		4000
+#define HEAP_LIMIT_CRITICAL		1000
+#define IRQSTACK_LIMIT_WARNING		150
+#define IRQSTACK_LIMIT_CRITICAL		80
+#define CPULOAD_LIMIT_WARNING		80
+#define CPULOAD_LIMIT_CRITICAL		95
+
 /* Stabilization options */
 #define PIOS_QUATERNION_STABILIZATION
 
+/* Alarm Thresholds */
+#define HEAP_LIMIT_WARNING		4000
+#define HEAP_LIMIT_CRITICAL		1000
+#define IRQSTACK_LIMIT_WARNING		150
+#define IRQSTACK_LIMIT_CRITICAL		80
+#define CPULOAD_LIMIT_WARNING		80
+#define CPULOAD_LIMIT_CRITICAL		95
+
 /* GPS options */
 #define PIOS_GPS_SETS_HOMELOCATION
 

flight/OpenPilot/System/openpilot.c

@@ -143,11 +143,11 @@ static void TaskTesting(void *pvParameters)
 		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
 		*/
 
-#if defined(PIOS_INCLUDE_SPEKTRUM)
+#if defined(PIOS_INCLUDE_DSM)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SPEKTRUM_Get(0), PIOS_SPEKTRUM_Get(1), PIOS_SPEKTRUM_Get(2), PIOS_SPEKTRUM_Get(3), PIOS_SPEKTRUM_Get(4), PIOS_SPEKTRUM_Get(5), PIOS_SPEKTRUM_Get(6), PIOS_SPEKTRUM_Get(7));
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_DSM_Get(0), PIOS_DSM_Get(1), PIOS_DSM_Get(2), PIOS_DSM_Get(3), PIOS_DSM_Get(4), PIOS_DSM_Get(5), PIOS_DSM_Get(6), PIOS_DSM_Get(7));
 #endif
 #if defined(PIOS_INCLUDE_SBUS)
-		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBUS_Get(0), PIOS_SBUS_Get(1), PIOS_SBUS_Get(2), PIOS_SBUS_Get(3), PIOS_SBUS_Get(4), PIOS_SBUS_Get(5), PIOS_SBUS_Get(6), PIOS_SBUS_Get(7));
+		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SBus_Get(0), PIOS_SBus_Get(1), PIOS_SBus_Get(2), PIOS_SBus_Get(3), PIOS_SBus_Get(4), PIOS_SBus_Get(5), PIOS_SBus_Get(6), PIOS_SBus_Get(7));
 #endif
 #if defined(PIOS_INCLUDE_PWM)
 		PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));

flight/OpenPilot/System/pios_board.c

@@ -30,7 +30,8 @@
 #include <pios.h>
 #include <openpilot.h>
 #include <uavobjectsinit.h>
-#include "manualcontrolsettings.h"
+#include <hwsettings.h>
+#include <manualcontrolsettings.h>
 
 //#define I2C_DEBUG_PIN			0
 //#define USART_GPS_DEBUG_PIN		1
@@ -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"
@@ -454,13 +538,13 @@ void PIOS_RTC_IRQ_Handler (void)
 
 #endif
 
-#if defined(PIOS_INCLUDE_SPEKTRUM)
+#if defined(PIOS_INCLUDE_DSM)
 /*
- * SPEKTRUM USART
+ * Spektrum/JR DSM USART
  */
-#include <pios_spektrum_priv.h>
+#include <pios_dsm_priv.h>
 
-static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
+static const struct pios_usart_cfg pios_usart_dsm_cfg = {
   .regs = USART1,
   .init = {
     .USART_BaudRate            = 115200,
@@ -496,8 +580,7 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = {
   },
 };
 
-#include <pios_spektrum_priv.h>
+static const struct pios_dsm_cfg pios_dsm_cfg = {
-static const struct pios_spektrum_cfg pios_spektrum_cfg = {
 	.bind = {
 		.gpio = GPIOA,
 		.init = {
@@ -506,10 +589,9 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = {
 			.GPIO_Mode  = GPIO_Mode_Out_PP,
 		},
 	},
-	.remap = 0,
 };
 
-#endif	/* PIOS_COM_SPEKTRUM */
+#endif	/* PIOS_COM_DSM */
 
 #if defined(PIOS_INCLUDE_SBUS)
 #error PIOS_INCLUDE_SBUS not implemented
@@ -535,65 +617,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 +727,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 +855,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 +863,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 +1033,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, DSMMAINPORT, DSMFLEXIPORT, 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)
@@ -989,7 +1062,7 @@ uint32_t pios_com_telem_rf_id;
 uint32_t pios_com_telem_usb_id;
 uint32_t pios_com_gps_id;
 uint32_t pios_com_aux_id;
-uint32_t pios_com_spektrum_id;
+uint32_t pios_com_dsm_id;
 
 #include "ahrs_spi_comm.h"
 
@@ -1003,8 +1076,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 +1097,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 +1111,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 +1130,150 @@ 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) {
+	case HWSETTINGS_OP_RCVRPORT_DISABLED:
+		break;
+	case HWSETTINGS_OP_RCVRPORT_DEBUG:
+		/* Not supported yet */
+		break;
+	case HWSETTINGS_OP_RCVRPORT_DSM2:
+	case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
+	case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
+#if defined(PIOS_INCLUDE_DSM)
+		{
+			enum pios_dsm_proto proto;
+			switch (hwsettings_rcvrport) {
+			case HWSETTINGS_OP_RCVRPORT_DSM2:
+				proto = PIOS_DSM_PROTO_DSM2;
+				break;
+			case HWSETTINGS_OP_RCVRPORT_DSMX10BIT:
+				proto = PIOS_DSM_PROTO_DSMX10BIT;
+				break;
+			case HWSETTINGS_OP_RCVRPORT_DSMX11BIT:
+				proto = PIOS_DSM_PROTO_DSMX11BIT;
+				break;
+			default:
+				PIOS_Assert(0);
+				break;
+			}
+
+			uint32_t pios_usart_dsm_id;
+			if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_cfg)) {
+				PIOS_Assert(0);
+			}
+
+			uint32_t pios_dsm_id;
+			if (PIOS_DSM_Init(&pios_dsm_id,
+					  &pios_dsm_cfg,
+					  &pios_usart_com_driver,
+					  pios_usart_dsm_id,
+					  proto, 0)) {
+				PIOS_Assert(0);
+			}
+
+			uint32_t pios_dsm_rcvr_id;
+			if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
+				PIOS_Assert(0);
+			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id;
+		}
+#endif
+		break;
+	case HWSETTINGS_OP_RCVRPORT_PWM:
 #if defined(PIOS_INCLUDE_PWM)
-#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS)
+		{
-#error More receiver inputs than available devices
+			uint32_t pios_pwm_id;
-#endif
+			PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg);
-			PIOS_PWM_Init();
+
 			uint32_t pios_pwm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) {
+			if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
-			  PIOS_Assert(0);
+				PIOS_Assert(0);
 			}
-			for (uint8_t i = 0;
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
-			     i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map);
+		}
-			     i++) {
+#endif	/* PIOS_INCLUDE_PWM */
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id;
+		break;
-				pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i;
+	case HWSETTINGS_OP_RCVRPORT_PPM:
-				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
+			uint32_t pios_ppm_id;
-#endif
+			PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg);
-			PIOS_PPM_Init();
+
 			uint32_t pios_ppm_rcvr_id;
-			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) {
+			if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
-			  PIOS_Assert(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++;
 			}
+			pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
+		}
 #endif	/* PIOS_INCLUDE_PPM */
-			break;
+		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
-#endif
-			{
-				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++;
-				}
-			}
-#endif
-			break;
-		case MANUALCONTROLSETTINGS_INPUTMODE_SBUS:
-#if defined(PIOS_INCLUDE_SBUS)
-#error SBUS NOT ON OP YET
-#endif  /* PIOS_INCLUDE_SBUS */
-			break;
 	}
 
 #if defined(PIOS_INCLUDE_USB_HID)

flight/OpenPilot/System/pios_board_posix.c

@@ -29,10 +29,24 @@
 #include <openpilot.h>
 #include <uavobjectsinit.h>
 
+#include "hwsettings.h"
+#include "attituderaw.h"
+#include "attitudeactual.h"
+#include "positionactual.h"
+#include "velocityactual.h"
+#include "manualcontrolsettings.h"
+
+#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, DSMMAINPORT, DSMFLEXIPORT, SBUS
+ * NOTE: No slot in this map for NONE.
+ */
+uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
+
+#endif /* PIOS_INCLUDE_RCVR */
+
 
 void Stack_Change() {
 }
@@ -66,6 +80,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 +169,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 +183,12 @@ void PIOS_Board_Init(void) {
 #endif	/* PIOS_INCLUDE_GPS */
 #endif
 
+	// Initialize these here as posix has no AHRSComms
+	AttitudeRawInitialize();
+	AttitudeActualInitialize();
+	VelocityActualInitialize();
+	PositionActualInitialize();
+	HwSettingsInitialize();
 
 }
 

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

@@ -64,11 +64,14 @@ UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
 UAVOBJSRCFILENAMES += systemstats
 UAVOBJSRCFILENAMES += taskinfo
-UAVOBJSRCFILENAMES += telemetrysettings
 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/inc/pios_rcvr.h

@@ -31,21 +31,24 @@
 #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);
+	void    (*init)(uint32_t id);
-  int32_t (*read)(uint32_t id, uint8_t channel);
+	int32_t (*read)(uint32_t id, uint8_t channel);
 };
 
 /* 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.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_rcvr.c

@@ -21,32 +21,39 @@ static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev)
 }
 
 #if defined(PIOS_INCLUDE_FREERTOS) && 0
-static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
+//static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
-{
+//{
-  struct pios_rcvr_dev * rcvr_dev;
+//  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);
+//  if (!rcvr_dev) return (NULL);
-
+//
-  rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
+//  rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
-  return(rcvr_dev);
+//  return(rcvr_dev);
-}
+//}
 #else
 static struct pios_rcvr_dev pios_rcvr_devs[PIOS_RCVR_MAX_DEVS];
 static uint8_t pios_rcvr_num_devs;
-static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
+static uint32_t PIOS_RCVR_alloc(void)
 {
   struct pios_rcvr_dev * rcvr_dev;
 
   if (pios_rcvr_num_devs >= PIOS_RCVR_MAX_DEVS) {
-    return (NULL);
+    return (PIOS_RCVR_MAX_DEVS+1);
   }
 
   rcvr_dev = &pios_rcvr_devs[pios_rcvr_num_devs++];
   rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC;
 
-  return (rcvr_dev);
+  return (pios_rcvr_num_devs);
 }
+static struct pios_rcvr_dev * PIOS_RCVR_find_dev(uint32_t rcvr_dev_id)
+{
+	if (!rcvr_dev_id) return NULL;
+	if (rcvr_dev_id>pios_rcvr_num_devs+1)	return NULL;
+	return &pios_rcvr_devs[rcvr_dev_id-1];
+}
+
 #endif
 
 /**
@@ -56,35 +63,53 @@ static struct pios_rcvr_dev * PIOS_RCVR_alloc(void)
   * \param[in] id
   * \return < 0 if initialisation failed
   */
-int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, const uint32_t lower_id)
+int32_t PIOS_RCVR_Init(uint32_t * rcvr_id, const struct pios_rcvr_driver * driver, uint32_t lower_id)
 {
   PIOS_DEBUG_Assert(rcvr_id);
   PIOS_DEBUG_Assert(driver);
 
+  uint32_t rcvr_dev_id;
   struct pios_rcvr_dev * rcvr_dev;
 
-  rcvr_dev = (struct pios_rcvr_dev *) PIOS_RCVR_alloc();
+  rcvr_dev_id = PIOS_RCVR_alloc();
+  rcvr_dev = PIOS_RCVR_find_dev(rcvr_dev_id);
   if (!rcvr_dev) goto out_fail;
 
   rcvr_dev->driver   = driver;
   rcvr_dev->lower_id = lower_id;
 
-
+  *rcvr_id = rcvr_dev_id;
-  *rcvr_id = pios_rcvr_num_devs - 1;
-
   return(0);
 
 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)
 {
-  struct pios_rcvr_dev * rcvr_dev = &pios_rcvr_devs[rcvr_id];
+	// 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 = PIOS_RCVR_find_dev(rcvr_id);
 
   if (!PIOS_RCVR_validate(rcvr_dev)) {
     /* Undefined RCVR port for this board (see pios_board.c) */
-    PIOS_DEBUG_Assert(0);
+    PIOS_Assert(0);
   }
 
   PIOS_DEBUG_Assert(rcvr_dev->driver->read);

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;
 }
 
@@ -191,7 +194,7 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
 			length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
 			rem = length;
 			while (rem>0) {
-				len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0,
+				len = sendto(udp_dev->socket, udp_dev->tx_buffer+length-rem, rem, 0,
 						 (struct sockaddr *) &udp_dev->client,
 						 sizeof(udp_dev->client));
 				if (len<=0) {

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,23 +206,32 @@ 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
+// Receiver DSM input
 //-------------------------
-#define PIOS_SPEKTRUM_NUM_INPUTS                12
+#define PIOS_DSM_MAX_DEVS			2
+#define PIOS_DSM_NUM_INPUTS			12
+
+//-------------------------
+// Receiver S.Bus input
+//-------------------------
+#define PIOS_SBUS_MAX_DEVS			1
+#define PIOS_SBUS_NUM_INPUTS			(16+2)
 
 //-------------------------
 // Servo outputs
@@ -230,6 +239,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,17 +185,26 @@ 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
+// Receiver DSM input
 //-------------------------
-#define PIOS_SPEKTRUM_NUM_INPUTS                12
+#define PIOS_DSM_MAX_DEVS			1
+#define PIOS_DSM_NUM_INPUTS			12
+
+//-------------------------
+// Receiver S.Bus input
+//-------------------------
+#define PIOS_SBUS_MAX_DEVS			0
+#define PIOS_SBUS_NUM_INPUTS			(16+2)
 
 //-------------------------
 // Servo outputs
@@ -203,6 +212,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

@@ -56,12 +56,12 @@ struct fileHeader {
 } __attribute__((packed));
 
 
-#define OBJECT_TABLE_MAGIC 0x85FB3C35
+#define OBJECT_TABLE_MAGIC 0x85FB3D35
-#define OBJ_MAGIC          0x3015AE71
+#define OBJ_MAGIC          0x3015A371
 #define OBJECT_TABLE_START 0x00000010
 #define OBJECT_TABLE_END   0x00001000
 #define SECTOR_SIZE        0x00001000
-#define MAX_BADMAGIC       4
+#define MAX_BADMAGIC       1000
 
 /**
  * @brief Initialize the flash object setting FS
@@ -72,7 +72,7 @@ int32_t PIOS_FLASHFS_Init()
 
 	// Check for valid object table or create one
 	uint32_t object_table_magic;
-	uint8_t magic_fail_count = 0;
+	uint32_t magic_fail_count = 0;
 	bool magic_good = false;
 
 	while(!magic_good) {
@@ -85,7 +85,7 @@ int32_t PIOS_FLASHFS_Init()
 				magic_fail_count = 0;
 				magic_good = true;
 			} else {
-				PIOS_DELAY_WaituS(100);
+				PIOS_DELAY_WaituS(1000);
 			}
 
 		}
@@ -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_hcsr04.c

@@ -32,8 +32,8 @@
 #include "pios.h"
 
 #if defined(PIOS_INCLUDE_HCSR04)
-#if !(defined(PIOS_INCLUDE_SPEKTRUM) || defined(PIOS_INCLUDE_SBUS))
+#if !(defined(PIOS_INCLUDE_DSM) || defined(PIOS_INCLUDE_SBUS))
-#error Only supported with Spektrum or S.Bus interface!
+#error Only supported with Spektrum/JR DSM or S.Bus interface!
 #endif
 
 /* Local Variables */

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 = 0x1E6;
 /* 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

@@ -207,16 +207,6 @@ SECTIONS
     } > 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");
@@ -182,17 +182,6 @@ SECTIONS
         . = 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_dsm.c

@@ -0,0 +1,407 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DSM Spektrum/JR DSMx satellite receiver functions
+ * @brief Code to bind and read Spektrum/JR DSMx satellite receiver serial stream
+ * @{
+ *
+ * @file       pios_dsm.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      Code bind and read Spektrum/JR DSMx satellite receiver serial 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
+ */
+
+/* Project Includes */
+#include "pios.h"
+#include "pios_dsm_priv.h"
+
+#if defined(PIOS_INCLUDE_DSM)
+
+/* Forward Declarations */
+static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel);
+static uint16_t PIOS_DSM_RxInCallback(uint32_t context,
+				      uint8_t *buf,
+				      uint16_t buf_len,
+				      uint16_t *headroom,
+				      bool *need_yield);
+static void PIOS_DSM_Supervisor(uint32_t dsm_id);
+
+/* Local Variables */
+const struct pios_rcvr_driver pios_dsm_rcvr_driver = {
+	.read = PIOS_DSM_Get,
+};
+
+enum pios_dsm_dev_magic {
+	PIOS_DSM_DEV_MAGIC = 0x44534d78,
+};
+
+struct pios_dsm_state {
+	uint16_t channel_data[PIOS_DSM_NUM_INPUTS];
+	uint8_t received_data[DSM_FRAME_LENGTH];
+	uint8_t receive_timer;
+	uint8_t failsafe_timer;
+	uint8_t frame_found;
+	uint8_t byte_count;
+#ifdef DSM_LOST_FRAME_COUNTER
+	uint8_t	frames_lost_last;
+	uint16_t frames_lost;
+#endif
+};
+
+struct pios_dsm_dev {
+	enum pios_dsm_dev_magic magic;
+	const struct pios_dsm_cfg *cfg;
+	enum pios_dsm_proto proto;
+	struct pios_dsm_state state;
+};
+
+/* Allocate DSM device descriptor */
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_dsm_dev *PIOS_DSM_Alloc(void)
+{
+	struct pios_dsm_dev *dsm_dev;
+
+	dsm_dev = (struct pios_dsm_dev *)pvPortMalloc(sizeof(*dsm_dev));
+	if (!dsm_dev)
+		return NULL;
+
+	dsm_dev->magic = PIOS_DSM_DEV_MAGIC;
+	return dsm_dev;
+}
+#else
+static struct pios_dsm_dev pios_dsm_devs[PIOS_DSM_MAX_DEVS];
+static uint8_t pios_dsm_num_devs;
+static struct pios_dsm_dev *PIOS_DSM_Alloc(void)
+{
+	struct pios_dsm_dev *dsm_dev;
+
+	if (pios_dsm_num_devs >= PIOS_DSM_MAX_DEVS)
+		return NULL;
+
+	dsm_dev = &pios_dsm_devs[pios_dsm_num_devs++];
+	dsm_dev->magic = PIOS_DSM_DEV_MAGIC;
+
+	return dsm_dev;
+}
+#endif
+
+/* Validate DSM device descriptor */
+static bool PIOS_DSM_Validate(struct pios_dsm_dev *dsm_dev)
+{
+	return (dsm_dev->magic == PIOS_DSM_DEV_MAGIC);
+}
+
+/* Try to bind DSMx satellite using specified number of pulses */
+static void PIOS_DSM_Bind(struct pios_dsm_dev *dsm_dev, uint8_t bind)
+{
+	const struct pios_dsm_cfg *cfg = dsm_dev->cfg;
+
+	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 */
+	if (bind > 10)
+		bind = 10;
+
+	GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
+
+	/* RX line, set high */
+	GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
+
+	/* on CC works up to 140ms, guess bind window is around 20-140ms after power up */
+	PIOS_DELAY_WaitmS(60);
+
+	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);
+		/* RX line, drive high for 120us */
+		GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
+		PIOS_DELAY_WaituS(120);
+	}
+	/* RX line, set input and wait for data */
+	GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure);
+}
+
+/* Reset channels in case of lost signal or explicit failsafe receiver flag */
+static void PIOS_DSM_ResetChannels(struct pios_dsm_dev *dsm_dev)
+{
+	struct pios_dsm_state *state = &(dsm_dev->state);
+	for (int i = 0; i < PIOS_DSM_NUM_INPUTS; i++) {
+		state->channel_data[i] = PIOS_RCVR_TIMEOUT;
+	}
+}
+
+/* Reset DSM receiver state */
+static void PIOS_DSM_ResetState(struct pios_dsm_dev *dsm_dev)
+{
+	struct pios_dsm_state *state = &(dsm_dev->state);
+	state->receive_timer = 0;
+	state->failsafe_timer = 0;
+	state->frame_found = 0;
+#ifdef DSM_LOST_FRAME_COUNTER
+	state->frames_lost_last = 0;
+	state->frames_lost = 0;
+#endif
+	PIOS_DSM_ResetChannels(dsm_dev);
+}
+
+/**
+ * Check and unroll complete frame data.
+ * \output 0 frame data accepted
+ * \output -1 frame error found
+ */
+static int PIOS_DSM_UnrollChannels(struct pios_dsm_dev *dsm_dev)
+{
+	struct pios_dsm_state *state = &(dsm_dev->state);
+	uint8_t resolution;
+
+#ifdef DSM_LOST_FRAME_COUNTER
+	/* increment the lost frame counter */
+	uint8_t frames_lost = state->received_data[0];
+	state->frames_lost += (frames_lost - state->frames_lost_last);
+	state->frames_lost_last = frames_lost;
+#endif
+
+	/* check the frame type assuming master satellite stream */
+	uint8_t type = state->received_data[1];
+	switch (type) {
+	case 0x01:
+	case 0x02:
+	case 0x12:
+		/* DSM2, DSMJ stream */
+		if (dsm_dev->proto == PIOS_DSM_PROTO_DSM2) {
+			/* DSM2/DSMJ resolution is known from the header */
+			resolution = (type & DSM_DSM2_RES_MASK) ? 11 : 10;
+		} else {
+			/* DSMX resolution should explicitly be selected */
+			goto stream_error;
+		}
+		break;
+	case 0xA2:
+	case 0xB2:
+		/* DSMX stream */
+		if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX10BIT) {
+			resolution = 10;
+		} else if (dsm_dev->proto == PIOS_DSM_PROTO_DSMX11BIT) {
+			resolution = 11;
+		} else {
+			/* DSMX resolution should explicitly be selected */
+			goto stream_error;
+		}
+		break;
+	default:
+		/* unknown yet data stream */
+		goto stream_error;
+	}
+
+	/* unroll channels */
+	uint8_t *s = &(state->received_data[2]);
+	uint16_t mask = (resolution == 10) ? 0x03ff : 0x07ff;
+
+	for (int i = 0; i < DSM_CHANNELS_PER_FRAME; i++) {
+		uint16_t word = ((uint16_t)s[0] << 8) | s[1];
+		s += 2;
+
+		/* skip empty channel slot */
+		if (word == 0xffff)
+			continue;
+
+		/* minimal data validation */
+		if ((i > 0) && (word & DSM_2ND_FRAME_MASK)) {
+			/* invalid frame data, ignore rest of the frame */
+			goto stream_error;
+		}
+
+		/* extract and save the channel value */
+		uint8_t channel_num = (word >> resolution) & 0x0f;
+		if (channel_num < PIOS_DSM_NUM_INPUTS)
+			state->channel_data[channel_num] = (word & mask);
+	}
+
+#ifdef DSM_LOST_FRAME_COUNTER
+	/* put lost frames counter into the last channel for debugging */
+	state->channel_data[PIOS_DSM_NUM_INPUTS-1] = state->frames_lost;
+#endif
+
+	/* all channels processed */
+	return 0;
+
+stream_error:
+	/* either DSM2 selected with DSMX stream found, or vice-versa */
+	return -1;
+}
+
+/* Update decoder state processing input byte from the DSMx stream */
+static void PIOS_DSM_UpdateState(struct pios_dsm_dev *dsm_dev, uint8_t byte)
+{
+	struct pios_dsm_state *state = &(dsm_dev->state);
+	if (state->frame_found) {
+		/* receiving the data frame */
+		if (state->byte_count < DSM_FRAME_LENGTH) {
+			/* store next byte */
+			state->received_data[state->byte_count++] = byte;
+			if (state->byte_count == DSM_FRAME_LENGTH) {
+				/* full frame received - process and wait for new one */
+				if (!PIOS_DSM_UnrollChannels(dsm_dev))
+					/* data looking good */
+					state->failsafe_timer = 0;
+
+				/* prepare for the next frame */
+				state->frame_found = 0;
+			}
+		}
+	}
+}
+
+/* Initialise DSM receiver interface */
+int32_t PIOS_DSM_Init(uint32_t *dsm_id,
+		      const struct pios_dsm_cfg *cfg,
+		      const struct pios_com_driver *driver,
+		      uint32_t lower_id,
+		      enum pios_dsm_proto proto,
+		      uint8_t bind)
+{
+	PIOS_DEBUG_Assert(dsm_id);
+	PIOS_DEBUG_Assert(cfg);
+	PIOS_DEBUG_Assert(driver);
+
+	struct pios_dsm_dev *dsm_dev;
+
+	dsm_dev = (struct pios_dsm_dev *)PIOS_DSM_Alloc();
+	if (!dsm_dev)
+		return -1;
+
+	/* Bind the configuration to the device instance */
+	dsm_dev->cfg = cfg;
+	dsm_dev->proto = proto;
+
+	/* Bind the receiver if requested */
+	if (bind)
+		PIOS_DSM_Bind(dsm_dev, bind);
+
+	PIOS_DSM_ResetState(dsm_dev);
+
+	*dsm_id = (uint32_t)dsm_dev;
+
+	/* Set comm driver callback */
+	(driver->bind_rx_cb)(lower_id, PIOS_DSM_RxInCallback, *dsm_id);
+
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_DSM_Supervisor, *dsm_id)) {
+		PIOS_DEBUG_Assert(0);
+	}
+
+	return 0;
+}
+
+/* Comm byte received callback */
+static uint16_t PIOS_DSM_RxInCallback(uint32_t context,
+				      uint8_t *buf,
+				      uint16_t buf_len,
+				      uint16_t *headroom,
+				      bool *need_yield)
+{
+	struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)context;
+
+	bool valid = PIOS_DSM_Validate(dsm_dev);
+	PIOS_Assert(valid);
+
+	/* process byte(s) and clear receive timer */
+	for (uint8_t i = 0; i < buf_len; i++) {
+		PIOS_DSM_UpdateState(dsm_dev, buf[i]);
+		dsm_dev->state.receive_timer = 0;
+	}
+
+	/* Always signal that we can accept another byte */
+	if (headroom)
+		*headroom = DSM_FRAME_LENGTH;
+
+	/* We never need a yield */
+	*need_yield = false;
+
+	/* Always indicate that all bytes were consumed */
+	return buf_len;
+}
+
+/**
+ * Get the value of an input channel
+ * \param[in] channel Number of the channel desired (zero based)
+ * \output PIOS_RCVR_INVALID channel not available
+ * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
+ * \output >0 channel value
+ */
+static int32_t PIOS_DSM_Get(uint32_t rcvr_id, uint8_t channel)
+{
+	struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)rcvr_id;
+
+	if (!PIOS_DSM_Validate(dsm_dev))
+		return PIOS_RCVR_INVALID;
+
+	/* return error if channel is not available */
+	if (channel >= PIOS_DSM_NUM_INPUTS)
+		return PIOS_RCVR_INVALID;
+
+	/* may also be PIOS_RCVR_TIMEOUT set by other function */
+	return dsm_dev->state.channel_data[channel];
+}
+
+/**
+ * Input data supervisor is called periodically and provides
+ * two functions: frame syncing and failsafe triggering.
+ *
+ * DSM frames come at 11ms or 22ms rate at 115200bps.
+ * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives
+ * 8ms pause between frames which is good for both DSM frame rates.
+ *
+ * Data receive function must clear the receive_timer to confirm new
+ * data reception. If no new data received in 100ms, we must call the
+ * failsafe function which clears all channels.
+ */
+static void PIOS_DSM_Supervisor(uint32_t dsm_id)
+{
+	struct pios_dsm_dev *dsm_dev = (struct pios_dsm_dev *)dsm_id;
+
+	bool valid = PIOS_DSM_Validate(dsm_dev);
+	PIOS_Assert(valid);
+
+	struct pios_dsm_state *state = &(dsm_dev->state);
+
+	/* waiting for new frame if no bytes were received in 8ms */
+	if (++state->receive_timer > 4) {
+		state->frame_found = 1;
+		state->byte_count = 0;
+		state->receive_timer = 0;
+	}
+
+	/* activate failsafe if no frames have arrived in 102.4ms */
+	if (++state->failsafe_timer > 64) {
+		PIOS_DSM_ResetChannels(dsm_dev);
+		state->failsafe_timer = 0;
+	}
+}
+
+#endif	/* PIOS_INCLUDE_DSM */
+
+/** 
+ * @}
+ * @}
+ */

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);
+	PIOS_DEBUG_Assert(cfg);
+
+	struct pios_pwm_dev * pwm_dev;
+
+	pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc();
+	if (!pwm_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	pwm_dev->cfg = cfg;
+
+	for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
 		/* Flush counter variables */
-		CaptureState[i] = 0;
+		pwm_dev->CaptureState[i] = 0;
-		RiseValue[i] = 0;
+		pwm_dev->RiseValue[i] = 0;
-		FallValue[i] = 0;
+		pwm_dev->FallValue[i] = 0;
-		CaptureValue[i] = 0;
+		pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
+	}
 
-		NVIC_InitTypeDef NVIC_InitStructure = pios_pwm_cfg.irq.init;
+	uint32_t tim_id;
-		GPIO_InitTypeDef GPIO_InitStructure = pios_pwm_cfg.gpio_init;
+	if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) {
-		TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_pwm_cfg.tim_base_init;
+		return -1;
-		TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init;
+	}
 
-		struct pios_pwm_channel channel = pios_pwm_cfg.channels[i];
+	/* Configure the channels to be in capture/compare mode */
-		
+	for (uint8_t i = 0; i < cfg->num_channels; i++) {
-		/* Enable appropriate clock to timer module */
+		const struct pios_tim_channel * chan = &cfg->channels[i];
-		switch((int32_t) channel.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
-		}
-		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 ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) {
 
-			TIM_ClearITPendingBit(channel.timer, channel.ccr);
+	if (!PIOS_PWM_validate(pwm_dev)) {
-			
+		/* Invalid device specified */
-			switch(channel.channel) {
+		return;
-				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

@@ -2,13 +2,13 @@
  ******************************************************************************
  * @addtogroup PIOS PIOS Core hardware abstraction layer
  * @{
- * @addtogroup PIOS_SBUS Futaba S.Bus receiver functions
+ * @addtogroup PIOS_SBus Futaba S.Bus receiver functions
- * @brief Code to read Futaba S.Bus input
+ * @brief Code to read Futaba S.Bus receiver serial stream
  * @{
  *
  * @file       pios_sbus.c
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * @brief      USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent)
+ * @brief      Code to read Futaba S.Bus receiver serial stream
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -34,52 +34,168 @@
 
 #if defined(PIOS_INCLUDE_SBUS)
 
-/* Global Variables */
+/* Forward Declarations */
+static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel);
+static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
+				       uint8_t *buf,
+				       uint16_t buf_len,
+				       uint16_t *headroom,
+				       bool *need_yield);
+static void PIOS_SBus_Supervisor(uint32_t sbus_id);
 
-/* Provide a RCVR driver */
-static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel);
-
-const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
-	.read = PIOS_SBUS_Get,
-};
 
 /* Local Variables */
-static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS];
+const struct pios_rcvr_driver pios_sbus_rcvr_driver = {
-static uint8_t received_data[SBUS_FRAME_LENGTH - 2];
+	.read = PIOS_SBus_Get,
-static uint8_t receive_timer;
+};
-static uint8_t failsafe_timer;
-static uint8_t frame_found;
 
-static void PIOS_SBUS_Supervisor(uint32_t sbus_id);
+enum pios_sbus_dev_magic {
+	PIOS_SBUS_DEV_MAGIC = 0x53427573,
+};
 
-/**
+struct pios_sbus_state {
- * reset_channels() function clears all channel data in case of
+	uint16_t channel_data[PIOS_SBUS_NUM_INPUTS];
- * lost signal or explicit failsafe flag from the S.Bus data stream
+	uint8_t received_data[SBUS_FRAME_LENGTH - 2];
- */
+	uint8_t receive_timer;
-static void reset_channels(void)
+	uint8_t failsafe_timer;
+	uint8_t frame_found;
+	uint8_t byte_count;
+};
+
+struct pios_sbus_dev {
+	enum pios_sbus_dev_magic magic;
+	const struct pios_sbus_cfg *cfg;
+	struct pios_sbus_state state;
+};
+
+/* Allocate S.Bus device descriptor */
+#if defined(PIOS_INCLUDE_FREERTOS)
+static struct pios_sbus_dev *PIOS_SBus_Alloc(void)
 {
-	for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) {
+	struct pios_sbus_dev *sbus_dev;
-		channel_data[i] = 0;
+
+	sbus_dev = (struct pios_sbus_dev *)pvPortMalloc(sizeof(*sbus_dev));
+	if (!sbus_dev) return(NULL);
+
+	sbus_dev->magic = PIOS_SBUS_DEV_MAGIC;
+	return(sbus_dev);
+}
+#else
+static struct pios_sbus_dev pios_sbus_devs[PIOS_SBUS_MAX_DEVS];
+static uint8_t pios_sbus_num_devs;
+static struct pios_sbus_dev *PIOS_SBus_Alloc(void)
+{
+	struct pios_sbus_dev *sbus_dev;
+
+	if (pios_sbus_num_devs >= PIOS_SBUS_MAX_DEVS) {
+		return (NULL);
+	}
+
+	sbus_dev = &pios_sbus_devs[pios_sbus_num_devs++];
+	sbus_dev->magic = PIOS_SBUS_DEV_MAGIC;
+
+	return (sbus_dev);
+}
+#endif
+
+/* Validate S.Bus device descriptor */
+static bool PIOS_SBus_Validate(struct pios_sbus_dev *sbus_dev)
+{
+	return (sbus_dev->magic == PIOS_SBUS_DEV_MAGIC);
+}
+
+/* Reset channels in case of lost signal or explicit failsafe receiver flag */
+static void PIOS_SBus_ResetChannels(struct pios_sbus_state *state)
+{
+	for (int i = 0; i < PIOS_SBUS_NUM_INPUTS; i++) {
+		state->channel_data[i] = PIOS_RCVR_TIMEOUT;
 	}
 }
 
-/**
+/* Reset S.Bus receiver state */
- * unroll_channels() function computes channel_data[] from received_data[]
+static void PIOS_SBus_ResetState(struct pios_sbus_state *state)
- * For efficiency it unrolls first 8 channels without loops. If other
- * 8 channels are needed they can be unrolled using the same code
- * 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;
+	state->receive_timer = 0;
-	uint16_t *d = channel_data;
+	state->failsafe_timer = 0;
+	state->frame_found = 0;
+	PIOS_SBus_ResetChannels(state);
+}
 
-#if (SBUS_NUMBER_OF_CHANNELS != 8)
+/* Initialise S.Bus receiver interface */
-#error Current S.Bus code unrolls only first 8 channels
+int32_t PIOS_SBus_Init(uint32_t *sbus_id,
-#endif
+		       const struct pios_sbus_cfg *cfg,
+		       const struct pios_com_driver *driver,
+		       uint32_t lower_id)
+{
+	PIOS_DEBUG_Assert(sbus_id);
+	PIOS_DEBUG_Assert(cfg);
+	PIOS_DEBUG_Assert(driver);
 
-#define F(v,s) ((v) >> s) & 0x7ff
+	struct pios_sbus_dev *sbus_dev;
+
+	sbus_dev = (struct pios_sbus_dev *)PIOS_SBus_Alloc();
+	if (!sbus_dev) goto out_fail;
+
+	/* Bind the configuration to the device instance */
+	sbus_dev->cfg = cfg;
+
+	PIOS_SBus_ResetState(&(sbus_dev->state));
+
+	*sbus_id = (uint32_t)sbus_dev;
+
+	/* Enable inverter clock and enable the inverter */
+	(*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE);
+	GPIO_Init(cfg->inv.gpio, &cfg->inv.init);
+	GPIO_WriteBit(cfg->inv.gpio, cfg->inv.init.GPIO_Pin, cfg->gpio_inv_enable);
+
+	/* Set comm driver callback */
+	(driver->bind_rx_cb)(lower_id, PIOS_SBus_RxInCallback, *sbus_id);
+
+	if (!PIOS_RTC_RegisterTickCallback(PIOS_SBus_Supervisor, *sbus_id)) {
+		PIOS_DEBUG_Assert(0);
+	}
+
+	return 0;
+
+out_fail:
+	return -1;
+}
+
+/**
+ * Get the value of an input channel
+ * \param[in] channel Number of the channel desired (zero based)
+ * \output PIOS_RCVR_INVALID channel not available
+ * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
+ * \output >0 channel value
+ */
+static int32_t PIOS_SBus_Get(uint32_t rcvr_id, uint8_t channel)
+{
+	struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)rcvr_id;
+
+	if (!PIOS_SBus_Validate(sbus_dev))
+		return PIOS_RCVR_INVALID;
+
+	/* return error if channel is not available */
+	if (channel >= PIOS_SBUS_NUM_INPUTS) {
+		return PIOS_RCVR_INVALID;
+	}
+
+	return sbus_dev->state.channel_data[channel];
+}
+
+/**
+ * Compute channel_data[] from received_data[].
+ * For efficiency it unrolls first 8 channels without loops and does the
+ * same for other 8 channels. Also 2 discrete channels will be set.
+ */
+static void PIOS_SBus_UnrollChannels(struct pios_sbus_state *state)
+{
+	uint8_t *s = state->received_data;
+	uint16_t *d = state->channel_data;
+
+#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,134 +204,135 @@ 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;
 }
 
-/**
+/* Update decoder state processing input byte from the S.Bus stream */
- * process_byte() function processes incoming byte from S.Bus stream
+static void PIOS_SBus_UpdateState(struct pios_sbus_state *state, uint8_t b)
- */
-static void process_byte(uint8_t b)
 {
-	static uint8_t byte_count;
+	/* should not process any data until new frame is found */
+	if (!state->frame_found)
+		return;
 
-	if (frame_found == 0) {
+	if (state->byte_count == 0) {
-		/* no frame found yet, waiting for start byte */
+		if (b != SBUS_SOF_BYTE) {
-		if (b == SBUS_SOF_BYTE) {
+			/* discard the whole frame if the 1st byte is not correct */
-			byte_count = 0;
+			state->frame_found = 0;
-			frame_found = 1;
-		}
-	} else {
-		/* do not store start and end of frame bytes */
-		if (byte_count < SBUS_FRAME_LENGTH - 2) {
-			/* store next byte */
-			received_data[byte_count++] = b;
 		} else {
-			if (b == SBUS_EOF_BYTE) {
+			/* do not store the SOF byte */
-				/* full frame received */
+			state->byte_count++;
-				uint8_t flags = received_data[SBUS_FRAME_LENGTH - 3];
-				if (flags & SBUS_FLAG_FL) {
-					/* frame lost, do not update */
-				} else if (flags & SBUS_FLAG_FS) {
-					/* failsafe flag active */
-					reset_channels();
-				} else {
-					/* data looking good */
-					unroll_channels();
-					failsafe_timer = 0;
-				}
-			} else {
-				/* discard whole frame */
-			}
-
-			/* prepare for the next frame */
-			frame_found = 0;
 		}
+		return;
+	}
+
+	/* do not store last frame byte as well */
+	if (state->byte_count < SBUS_FRAME_LENGTH - 1) {
+		/* store next byte */
+		state->received_data[state->byte_count - 1] = b;
+		state->byte_count++;
+	} else {
+		if (b == SBUS_EOF_BYTE) {
+			/* full frame received */
+			uint8_t flags = state->received_data[SBUS_FRAME_LENGTH - 3];
+			if (flags & SBUS_FLAG_FL) {
+				/* frame lost, do not update */
+			} else if (flags & SBUS_FLAG_FS) {
+				/* failsafe flag active */
+				PIOS_SBus_ResetChannels(state);
+			} else {
+				/* data looking good */
+				PIOS_SBus_UnrollChannels(state);
+				state->failsafe_timer = 0;
+			}
+		} else {
+			/* discard whole frame */
+		}
+
+		/* prepare for the next frame */
+		state->frame_found = 0;
 	}
 }
 
-static uint16_t PIOS_SBUS_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
+/* Comm byte received callback */
+static uint16_t PIOS_SBus_RxInCallback(uint32_t context,
+				       uint8_t *buf,
+				       uint16_t buf_len,
+				       uint16_t *headroom,
+				       bool *need_yield)
 {
+	struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)context;
+
+	bool valid = PIOS_SBus_Validate(sbus_dev);
+	PIOS_Assert(valid);
+
+	struct pios_sbus_state *state = &(sbus_dev->state);
+
 	/* process byte(s) and clear receive timer */
 	for (uint8_t i = 0; i < buf_len; i++) {
-		process_byte(buf[i]);
+		PIOS_SBus_UpdateState(state, buf[i]);
-		receive_timer = 0;
+		state->receive_timer = 0;
 	}
 
 	/* Always signal that we can accept another byte */
-	if (headroom) {
+	if (headroom)
 		*headroom = SBUS_FRAME_LENGTH;
-	}
 
 	/* We never need a yield */
 	*need_yield = false;
 
 	/* Always indicate that all bytes were consumed */
-	return (buf_len);
+	return buf_len;
-}
-
-/**
- * Initialise S.Bus receiver interface
- */
-int32_t PIOS_SBUS_Init(uint32_t * sbus_id, const struct pios_sbus_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id)
-{
-	/* Enable inverter clock and enable the inverter */
-	(*cfg->gpio_clk_func)(cfg->gpio_clk_periph, ENABLE);
-	GPIO_Init(cfg->inv.gpio, &cfg->inv.init);
-	GPIO_WriteBit(cfg->inv.gpio,
-		      cfg->inv.init.GPIO_Pin,
-		      cfg->gpio_inv_enable);
-
-	(driver->bind_rx_cb)(lower_id, PIOS_SBUS_RxInCallback, 0);
-
-	if (!PIOS_RTC_RegisterTickCallback(PIOS_SBUS_Supervisor, 0)) {
-		PIOS_Assert(0);
-	}
-
-	return (0);
-}
-
-/**
- * Get the value of an input channel
- * \param[in] channel Number of the channel desired (zero based)
- * \output -1 channel not available
- * \output >0 channel value
- */
-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 channel_data[channel];
 }
 
 /**
  * Input data supervisor is called periodically and provides
  * two functions: frame syncing and failsafe triggering.
  *
- * S.Bus frames come at 7ms (HS) or 14ms (FS) rate at 100000bps. RTC
+ * S.Bus frames come at 7ms (HS) or 14ms (FS) rate at 100000bps.
- * timer is running at 625Hz (1.6ms). So with divider 2 it gives
+ * RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
- * 3.2ms pause between frames which is good for both S.Bus data rates.
+ * 3.2ms pause between frames which is good for both S.Bus frame rates.
  *
  * Data receive function must clear the receive_timer to confirm new
  * data reception. If no new data received in 100ms, we must call the
  * failsafe function which clears all channels.
  */
-static void PIOS_SBUS_Supervisor(uint32_t sbus_id)
+static void PIOS_SBus_Supervisor(uint32_t sbus_id)
 {
+	struct pios_sbus_dev *sbus_dev = (struct pios_sbus_dev *)sbus_id;
+
+	bool valid = PIOS_SBus_Validate(sbus_dev);
+	PIOS_Assert(valid);
+
+	struct pios_sbus_state *state = &(sbus_dev->state);
+
 	/* waiting for new frame if no bytes were received in 3.2ms */
-	if (++receive_timer > 2) {
+	if (++state->receive_timer > 2) {
-		receive_timer = 0;
+		state->frame_found = 1;
-		frame_found = 0;
+		state->byte_count = 0;
+		state->receive_timer = 0;
 	}
 
 	/* activate failsafe if no frames have arrived in 102.4ms */
-	if (++failsafe_timer > 64) {
+	if (++state->failsafe_timer > 64) {
-		reset_channels();
+		PIOS_SBus_ResetChannels(state);
-		failsafe_timer = 0;
+		state->failsafe_timer = 0;
 	}
 }
 
-#endif
+#endif	/* PIOS_INCLUDE_SBUS */
 
 /** 
  * @}

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

@@ -1,264 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup PIOS PIOS Core hardware abstraction layer
- * @{
- * @addtogroup PIOS_SPEKTRUM Spektrum receiver functions
- * @brief Code to read Spektrum input
- * @{
- *
- * @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
- *
- *****************************************************************************/
-/* 
- * 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"
-#include "pios_spektrum_priv.h"
-
-#if defined(PIOS_INCLUDE_SPEKTRUM)
-
-/**
- * @Note Framesyncing:
- * The code resets the watchdog timer whenever a single byte is received, so what watchdog code
- * is never called if regularly getting bytes.
- * RTC timer is running @625Hz, supervisor timer has divider 5 so frame sync comes every 1/125Hz=8ms.
- * Good for both 11ms and 22ms framecycles
- */
-
-/* Global Variables */
-
-/* Provide a RCVR driver */
-static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel);
-
-const struct pios_rcvr_driver pios_spektrum_rcvr_driver = {
-	.read = PIOS_SPEKTRUM_Get,
-};
-
-/* Local Variables */
-static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS];
-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;
-
-static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id);
-static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg);
-static int32_t PIOS_SPEKTRUM_Decode(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)
-{
-	/* process byte(s) and clear receive timer */
-	for (uint8_t i = 0; i < buf_len; i++) {
-		PIOS_SPEKTRUM_Decode(buf[i]);
-		supv_timer = 0;
-	}
-
-	/* Always signal that we can accept another byte */
-	if (headroom) {
-		*headroom = 1;
-	}
-
-	/* We never need a yield */
-	*need_yield = false;
-
-	/* Always indicate that all bytes were consumed */
-	return (buf_len);
-}
-
-/**
-* 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)
-{
-	// TODO: need setting flag for bind on next powerup
-	if (bind) {
-		PIOS_SPEKTRUM_Bind(cfg);
-	}
-
-	(driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0);
-
-	if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) {
-		PIOS_DEBUG_Assert(0);
-	}
-
-	return (0);
-}
-
-/**
-* Get the value of an input channel
-* \param[in] Channel Number of the channel desired
-* \output -1 Channel not available
-* \output >0 Channel value
-*/
-static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel)
-{
-	/* Return error if channel not available */
-	if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) {
-		return -1;
-	}
-	return CaptureValue[channel];
-}
-
-/**
-* Spektrum bind function
-* \output true Successful bind
-* \output false Bind failed
-*/
-static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg)
-{
-#define BIND_PULSES 5
-
-	GPIO_Init(cfg->bind.gpio, &cfg->bind.init);
-	/* RX line, set high */
-	GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
-
-	/* 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++) {
-		/* RX line, drive low for 120us */
-		GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
-		PIOS_DELAY_WaituS(120);
-		/* RX line, drive high for 120us */
-		GPIO_SetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin);
-		PIOS_DELAY_WaituS(120);
-	}
-	/* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */
-
-	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) {
-	/* 125hz */
-	supv_timer++;
-	if(supv_timer > 5) {
-		/* sync between frames */
-		sync = 0;
-		bytecount = 0;
-		prev_byte = 0xFF;
-		frame_error = 0;
-		sync_of++;
-		/* watchdog activated after 100ms silence */
-		if (sync_of > 12) {
-			/* signal lost */
-			sync_of = 0;
-			for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) {
-				CaptureValue[i] = 0;
-				CaptureValueTemp[i] = 0;
-			}
-		}
-		supv_timer = 0;
-	}
-}
-
-#endif
-
-/** 
-  * @}
-  * @}
-  */

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_dsm.h

@@ -2,13 +2,12 @@
  ******************************************************************************
  * @addtogroup PIOS PIOS Core hardware abstraction layer
  * @{
- * @addtogroup   PIOS_SPEKTRUM Spektrum receiver functions
+ * @addtogroup PIOS_DSM Spektrum/JR DSMx satellite receiver functions
  * @{
  *
- * @file       pios_spektrum.h  
+ * @file       pios_dsm.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
- * 	       Parts by Thorsten Klose (tk@midibox.org)
+ * @brief      Spektrum/JR DSMx satellite receiver functions header.
- * @brief      Spektrum satellite functions header.
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -28,16 +27,16 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef PIOS_SPEKTRUM_H
+#ifndef PIOS_DSM_H
-#define PIOS_SPEKTRUM_H
+#define PIOS_DSM_H
 
 /* Global Types */
 
 /* Public Functions */
 
-#endif /* PIOS_SPEKTRUM_H */
+#endif /* PIOS_DSM_H */
 
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

flight/PiOS/inc/pios_dsm_priv.h

@@ -0,0 +1,138 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_DSM Spektrum/JR DSMx satellite receiver functions
+ * @brief PIOS interface to bind and read Spektrum/JR DSMx satellite receiver
+ * @{
+ *
+ * @file       pios_dsm_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      Spektrum/JR DSMx satellite receiver private structures.
+ * @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 PIOS_DSM_PRIV_H
+#define PIOS_DSM_PRIV_H
+
+#include <pios.h>
+#include <pios_stm32.h>
+#include <pios_usart_priv.h>
+
+/*
+ * Currently known DSMx (DSM2, DSMJ, DSMX) satellite serial port settings:
+ *  115200bps serial stream, 8 bits, no parity, 1 stop bit
+ *  size of each frame: 16 bytes
+ *  data resolution: 10 or 11 bits
+ *  number of frames: 1 or 2
+ *  frame period: 11ms or 22ms
+ *
+ * Currently known DSMx frame structure:
+ *  2 bytes - depend on protocol version:
+ *    for DSM2/DSMJ:
+ *      1 byte - lost frame counter (8 bit)
+ *      1 byte - data format (for master receiver bound with 3 or 5 pulses),
+ *      	 or unknown (for slave receiver bound with 4 or 6 pulses,
+ *               some sources call it also the lost frame counter)
+ *    for DSMX:
+ *      1 byte - unknown data (does not look like lost frame counter)
+ *      1 byte - unknown data, has been seen only 0xB2 so far
+
+ * 14 bytes - up to 7 channels (16 bit word per channel) with encoded channel
+ * 	      number, channel value, the "2nd frame in a sequence" flag.
+ * 	      Unused channels have FF FF instead of data bytes.
+ *
+ * Data format identification:
+ *   - for DSM2/DSMJ: [0 0 0 R 0 0 N1 N0]
+ *     where
+ *   	  R is data resolution (0 - 10 bits, 1 - 11 bits),
+ *   	  N1..N0 is the number of frames required to receive all channel
+ *        data (01 or 10 are known to the moment, which means 1 or 2 frames).
+ *        Three values for the transmitter information byte have been seen
+ *        thus far: 0x01, 0x02, 0x12.
+ *   - for DSMX this byte contains just 0xB2 or 0xA2 value for any resolution.
+ *     It is not known at the moment how to find the exact resolution from the
+ *     DSMX data stream. The frame number (1 or 2) and 10/11 bit resolution were
+ *     found in different data streams. So it is safer at the moment to ask user
+ *     explicitly choose the resolution.
+ *     Also some weird throttle channel (0) behavior was found in some streams
+ *     from DX8 transmitter (all zeroes). Thus DSMX needs special processing.
+ *
+ * Channel data are:
+ * - for 10 bit: [F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0]
+ * - for 11 bit: [F C3 C2 C1 C0 D10 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0]
+ *   where
+ *   	F is normally 0 but set to 1 for the first channel of the 2nd frame,
+ *      C3 to C0 is the channel number, 4 bit, zero-based, in any order,
+ *      Dx..D0 - channel data (10 or 11 bits)
+ *
+ * DSM2 protocol bug: in some cases in 2-frame format some bytes of the
+ * frame can contain invalid values from the previous frame. They usually
+ * are the last 5 bytes and can be equal to FF or other data from last
+ * frame. There is no explicit workaround currently known.
+ *
+ * Binding: the number of pulses within bind window after power up defines
+ * if this receiver is a master (provides receiver capabilities info to
+ * the transmitter to choose data format) or slave (does not respond to
+ * the transmitter which falls back to the old DSM mode in that case).
+ * Currently known are 3(4) pulses for low resolution (10 bit) DSM2 mode,
+ * 5(6) pulses for high resolution (11 bit) DSM2 mode, and also 7(8) and
+ * 9(10) pulses for DSMX modes. Thus only 3, 5, 7 or 9 pulses should be
+ * used for stand-alone satellite receiver to be bound correctly as the
+ * master.
+ */
+
+#define DSM_CHANNELS_PER_FRAME		7
+#define DSM_FRAME_LENGTH		(1+1+DSM_CHANNELS_PER_FRAME*2)
+#define DSM_DSM2_RES_MASK		0x0010
+#define DSM_2ND_FRAME_MASK		0x8000
+
+/*
+ * Include lost frame counter and provide it as a last channel value
+ * for debugging. Currently is not used by the receiver layer.
+ */
+//#define DSM_LOST_FRAME_COUNTER
+
+/* DSM protocol variations */
+enum pios_dsm_proto {
+	PIOS_DSM_PROTO_DSM2,
+	PIOS_DSM_PROTO_DSMX10BIT,
+	PIOS_DSM_PROTO_DSMX11BIT,
+};
+
+/* DSM receiver instance configuration */
+struct pios_dsm_cfg {
+	struct stm32_gpio bind;
+};
+
+extern const struct pios_rcvr_driver pios_dsm_rcvr_driver;
+
+extern int32_t PIOS_DSM_Init(uint32_t *dsm_id,
+			     const struct pios_dsm_cfg *cfg,
+			     const struct pios_com_driver *driver,
+			     uint32_t lower_id,
+			     enum pios_dsm_proto proto,
+			     uint8_t bind);
+
+#endif /* PIOS_DSM_PRIV_H */
+
+/**
+ * @}
+ * @}
+ */

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);

flight/PiOS/inc/pios_gcsrcvr_priv.h

@@ -2,13 +2,13 @@
  ******************************************************************************
  * @addtogroup PIOS PIOS Core hardware abstraction layer
  * @{
- * @addtogroup   PIOS_SPEKTRUM SPEKTRUM Functions
+ * @addtogroup   PIOS_GCSRCVR GCS Receiver Functions
- * @brief PIOS interface to read and write from spektrum port
+ * @brief PIOS interface to read from GCS receiver port
  * @{
  *
- * @file       pios_spektrum_priv.h
+ * @file       pios_gcsrcvr_priv.h
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Servo private structures.
+ * @brief      GCS receiver private functions
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -28,23 +28,18 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef PIOS_SPEKTRUM_PRIV_H
+#ifndef PIOS_GCSRCVR_PRIV_H
-#define PIOS_SPEKTRUM_PRIV_H
+#define PIOS_GCSRCVR_PRIV_H
 
 #include <pios.h>
-#include <pios_stm32.h>
-#include <pios_usart_priv.h>
 
-struct pios_spektrum_cfg {
+#include "gcsreceiver.h"
-	struct stm32_gpio bind;
-	uint32_t remap;		/* GPIO_Remap_* */
-};
 
-extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver;
+extern const struct pios_rcvr_driver pios_gcsrcvr_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 void PIOS_GCSRCVR_Init(void);
 
-#endif /* PIOS_PWM_PRIV_H */
+#endif /* PIOS_GCSRCVR_PRIV_H */
 
 /**
  * @}

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 */