Merge branch 'camera_stabilization' into next

Conflicts: flight/OpenPilot/System/pios_board_posix.c
2025-02-20 10:54:14 +01:00 · 2011-08-19 01:48:24 -05:00 · 2011-08-19 01:48:24 -05:00 · f85ae84ec3
commit f85ae84ec3
parent 25b54a4323 86b652bbab
52 changed files with 1362 additions and 325 deletions
--- a/HISTORY.txt
+++ b/HISTORY.txt
@ -1,5 +1,18 @@
 Short summary of changes.  For a complete list see the git log.

+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).
--- a/flight/CopterControl/Makefile
+++ b/flight/CopterControl/Makefile
@ -37,6 +37,9 @@ OUTDIR := $(TOP)/build/$(TARGET)
 # Set to YES to compile for debugging
 DEBUG ?= NO

+# Include objects that are just nice information to show
+DIAGNOSTICS ?= NO
+
 # Set to YES to build a FW version that will erase all flash memory
 ERASE_FLASH ?= NO
 # Set to YES to use the Servo output pins for debugging via scope or logic analyser
@ -62,7 +65,9 @@ endif
 FLASH_TOOL = OPENOCD

 # List of modules to include
-MODULES = Telemetry Attitude Stabilization Actuator ManualControl FirmwareIAP
+MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab
+# Telemetry must be last to grab the optional modules
+MODULES += Telemetry

 # Paths
 OPSYSTEM = ./System
@ -125,7 +130,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
 SRC += $(OPSYSTEM)/pios_usb_hid_desc.c
 else
 ## TESTCODE
@ -153,21 +157,21 @@ SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
 SRC += $(OPUAVSYNTHDIR)/attituderaw.c
 SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
 SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
-SRC += $(OPUAVSYNTHDIR)/taskinfo.c
 SRC += $(OPUAVSYNTHDIR)/i2cstats.c
 SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
 SRC += $(OPUAVSYNTHDIR)/telemetrysettings.c
-SRC += $(OPUAVSYNTHDIR)/ratedesired.c
 SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
 SRC += $(OPUAVSYNTHDIR)/mixersettings.c
-SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
 SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
 SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
+SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
+SRC += $(OPUAVSYNTHDIR)/cameradesired.c
 SRC += $(OPUAVSYNTHDIR)/hwsettings.c
-#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
-#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
-# Cant use until i can automatically generate list of UAVObjects
-#SRC += ${OUTDIR}/InitObjects.c
+
+SRC += $(OPUAVSYNTHDIR)/taskinfo.c
+SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
+SRC += $(OPUAVSYNTHDIR)/ratedesired.c
+
 endif

 ## PIOS Hardware (STM32F10x)
@ -417,6 +421,10 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -DDEBUG
 endif

+ifeq ($(DIAGNOSTICS),YES)
+CFLAGS = -DDIAGNOSTICS
+endif
+
 CFLAGS += -g$(DEBUGF)
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
@ -425,7 +433,7 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.

 #CFLAGS += -fno-cprop-registers -fno-defer-pop -fno-guess-branch-probability -fno-section-anchors
 #CFLAGS += -fno-if-conversion -fno-if-conversion2 -fno-ipa-pure-const -fno-ipa-reference -fno-merge-constants
-#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename 
+#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename
 #CFLAGS += -fno-tree-dce -fno-tree-dominator-opts -fno-tree-dse -fno-tree-fre -fno-tree-sink -fno-tree-sra
 #CFLAGS += -fno-tree-ter
 #CFLAGS += -g$(DEBUGF) -DDEBUG
@ -504,7 +512,7 @@ endif
 endif

 # Generate code for PyMite
-#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) 
+#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
 #	@echo $(MSG_PYMITEINIT) $(call toprel, $@)
 #	@$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
 #	@$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h
--- a/flight/CopterControl/System/alarms.c
+++ b/flight/CopterControl/System/alarms.c
@ -41,10 +41,12 @@ static xSemaphoreHandle lock;
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);

 /**
- * Initialize the alarms library
+ * Initialize the alarms library
 */
 int32_t AlarmsInitialize(void)
 {
+    SystemAlarmsInitialize();
+    
 	lock = xSemaphoreCreateRecursiveMutex();
 	//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
 	//AlarmsClearAll();
@ -56,7 +58,7 @@ int32_t AlarmsInitialize(void)
 * Set an alarm
 * @param alarm The system alarm to be modified
 * @param severity The alarm severity
- * @return 0 if success, -1 if an error
+ * @return 0 if success, -1 if an error
 */
 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
 {
@ -151,7 +153,7 @@ void AlarmsClearAll()

 /**
 * Check if there are any alarms with the given or higher severity
- * @return 0 if no alarms are found, 1 if at least one alarm is found
+ * @return 0 if no alarms are found, 1 if at least one alarm is found
 */
 int32_t AlarmsHasWarnings()
 {
@ -208,5 +210,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 /**
 * @}
 * @}
- */
+ */

--- a/flight/CopterControl/System/coptercontrol.c
+++ b/flight/CopterControl/System/coptercontrol.c
@ -1,97 +1,107 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotSystem OpenPilot System
- * @brief These files are the core system files of OpenPilot.
- * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
- * in the main() function of openpilot.c
- * @{
- * @addtogroup OpenPilotCore OpenPilot Core
- * @brief This is where the OP firmware starts. Those files also define the compile-time
- * options of the firmware.
- * @{
- * @file       openpilot.c 
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Sets up and runs main OpenPilot tasks.
- * @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
- */
-
-
-/* OpenPilot Includes */
-#include "openpilot.h"
-#include "uavobjectsinit.h"
-#include "systemmod.h"
-
-/* Task Priorities */
-#define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
-
-/* Global Variables */
-
-/* Prototype of PIOS_Board_Init() function */
-extern void PIOS_Board_Init(void);
-extern void Stack_Change(void);
-
-/**
-* OpenPilot Main function:
-*
-* Initialize PiOS<BR>
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
-* If something goes wrong, blink LED1 and LED2 every 100ms
-*
-*/
-int main()
-{
-	/* NOTE: Do NOT modify the following start-up sequence */
-	/* Any new initialization functions should be added in OpenPilotInit() */
-
-	/* Brings up System using CMSIS functions, enables the LEDs. */
-	PIOS_SYS_Init();
-
-	/* Architecture dependant Hardware and
-	 * core subsystem initialisation
-	 * (see pios_board.c for your arch)
-	 * */
-	PIOS_Board_Init();
-
-	/* Initialize modules */
-	MODULE_INITIALISE_ALL
-
-	/* swap the stack to use the IRQ stack */
-	Stack_Change();
-
-	/* Start the FreeRTOS scheduler which should never returns.*/
-	vTaskStartScheduler();
-
-	/* If all is well we will never reach here as the scheduler will now be running. */
-
-	/* Do some indication to user that something bad just happened */
-	PIOS_LED_Off(LED1); \
-	for(;;) { \
-		PIOS_LED_Toggle(LED1); \
-		PIOS_DELAY_WaitmS(100); \
-	};
-
-    return 0;
-}
-
-/**
- * @}
- * @}
- */
-
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotSystem OpenPilot System
+ * @brief These files are the core system files of OpenPilot.
+ * They are the ground layer just above PiOS. In practice, OpenPilot actually starts
+ * in the main() function of openpilot.c
+ * @{
+ * @addtogroup OpenPilotCore OpenPilot Core
+ * @brief This is where the OP firmware starts. Those files also define the compile-time
+ * options of the firmware.
+ * @{
+ * @file       openpilot.c 
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Sets up and runs main OpenPilot tasks.
+ * @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
+ */
+
+
+/* OpenPilot Includes */
+#include "openpilot.h"
+#include "uavobjectsinit.h"
+#include "hwsettings.h"
+#include "camerastab.h"
+#include "systemmod.h"
+
+/* Task Priorities */
+#define PRIORITY_TASK_HOOKS             (tskIDLE_PRIORITY + 3)
+
+/* Global Variables */
+
+/* Prototype of PIOS_Board_Init() function */
+extern void PIOS_Board_Init(void);
+extern void Stack_Change(void);
+
+/**
+* OpenPilot Main function:
+*
+* Initialize PiOS<BR>
+* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
+* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
+* If something goes wrong, blink LED1 and LED2 every 100ms
+*
+*/
+int main()
+{
+	/* NOTE: Do NOT modify the following start-up sequence */
+	/* Any new initialization functions should be added in OpenPilotInit() */
+
+	/* Brings up System using CMSIS functions, enables the LEDs. */
+	PIOS_SYS_Init();
+
+	/* Architecture dependant Hardware and
+	 * core subsystem initialisation
+	 * (see pios_board.c for your arch)
+	 * */
+	PIOS_Board_Init();
+
+	/* Initialize modules */
+	MODULE_INITIALISE_ALL
+
+	/* Optional module initialization.  This code might want to go somewhere else as
+	 * it grows */
+	uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
+	HwSettingsOptionalModulesGet(optionalModules);
+	if(optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTABILIZATION] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+		CameraStabInitialize();
+	}
+
+	/* swap the stack to use the IRQ stack */
+	Stack_Change();
+
+	/* Start the FreeRTOS scheduler which should never returns.*/
+	vTaskStartScheduler();
+
+	/* If all is well we will never reach here as the scheduler will now be running. */
+
+	/* Do some indication to user that something bad just happened */
+	PIOS_LED_Off(LED1); \
+	for(;;) { \
+		PIOS_LED_Toggle(LED1); \
+		PIOS_DELAY_WaitmS(100); \
+	};
+
+    return 0;
+}
+
+/**
+ * @}
+ * @}
+ */
+
--- a/flight/CopterControl/System/inc/FreeRTOSConfig.h
+++ b/flight/CopterControl/System/inc/FreeRTOSConfig.h
@ -39,7 +39,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 +75,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 +86,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


 /**
--- a/flight/CopterControl/System/pios_board.c
+++ b/flight/CopterControl/System/pios_board.c
@ -907,8 +907,10 @@ void PIOS_Board_Init(void) {
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
-	UAVObjectsInitializeAll();

+	HwSettingsInitialize();
+	ManualControlSettingsInitialize();
+	
 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
 	PIOS_RTC_Init(&pios_rtc_main_cfg);
--- a/flight/CopterControl/System/pios_board_posix.c
+++ b/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();
--- a/flight/CopterControl/System/taskmonitor.c
+++ b/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
 }
--- a/flight/Modules/AHRSComms/ahrs_comms.c
+++ b/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
@ -72,6 +74,10 @@ static void ahrscommsTask(void *parameters);
 int32_t AHRSCommsStart(void)
 {
 	// Start main task
+	AhrsStatusInitialize();
+	AHRSCalibrationInitialize();
+	AttitudeRawInitialize();
+	
 	xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
 	TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle);
 	PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS);
--- a/flight/Modules/Actuator/actuator.c
+++ b/flight/Modules/Actuator/actuator.c
@ -41,6 +41,7 @@
 #include "flightstatus.h"
 #include "mixersettings.h"
 #include "mixerstatus.h"
+#include "cameradesired.h"


 // Private constants
@ -108,9 +109,17 @@ int32_t ActuatorInitialize()
 	// Create object queue
 	queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));

+	ActuatorSettingsInitialize();
+	ActuatorDesiredInitialize();
+	MixerSettingsInitialize();
+	ActuatorCommandInitialize();
+#if defined(DIAGNOSTICS)
+	MixerStatusInitialize();
+#endif
+
 	// Listen for ExampleObject1 updates
 	ActuatorDesiredConnectQueue(queue);
-	
+
 	// If settings change, update the output rate
 	ActuatorSettingsConnectCallback(actuator_update_rate);

@ -160,7 +169,7 @@ static void actuatorTask(void* parameters)
 	// Main task loop
 	lastSysTime = xTaskGetTickCount();
 	while (1)
-	{		
+	{
 		PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);

 		// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
@ -177,11 +186,13 @@ static void actuatorTask(void* parameters)
 		lastSysTime = thisSysTime;

 		FlightStatusGet(&flightStatus);
-		MixerStatusGet(&mixerStatus);
 		MixerSettingsGet (&mixerSettings);
 		ActuatorDesiredGet(&desired);
 		ActuatorCommandGet(&command);

+#if defined(DIAGNOSTICS)
+		MixerStatusGet(&mixerStatus);
+#endif
 		ActuatorSettingsMotorsSpinWhileArmedGet(&MotorsSpinWhileArmed);
 		ActuatorSettingsChannelMaxGet(ChannelMax);
 		ActuatorSettingsChannelMinGet(ChannelMin);
@ -196,7 +207,7 @@ static void actuatorTask(void* parameters)
 				nMixers ++;
 			}
 		}
-		if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers. 
+		if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
 		{
 			setFailsafe(); // So that channels like PWM buzzer keep working
 			continue;
@ -207,7 +218,7 @@ static void actuatorTask(void* parameters)
 		bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
 		bool positiveThrottle = desired.Throttle >= 0.00;
 		bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
-		
+
 		float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
 		//The source for the secondary curve is selectable
 		float curve2 = 0;
@ -233,11 +244,11 @@ static void actuatorTask(void* parameters)
 			case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
 				if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0)
 					curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2);
-				else 
-					curve2 = 0;				
+				else
+					curve2 = 0;
 				break;
 		}
-		
+
 		for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
 		{
 			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
@ -246,53 +257,82 @@ static void actuatorTask(void* parameters)
 				command.Channel[ct] = 0;
 				continue;
 			}
-			
-			status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
-			
+
+			if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
+				status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
+			else
+				status[ct] = -1;
+
+
+
 			// Motors have additional protection for when to be on
-			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {					
+			if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {

 				// If not armed or motors aren't meant to spin all the time
 				if( !armed ||
 				   (!spinWhileArmed && !positiveThrottle))
 				{
 					filterAccumulator[ct] = 0;
-					lastResult[ct] = 0;					
+					lastResult[ct] = 0;
 					status[ct] = -1;  //force min throttle
-				} 
-				// If armed meant to keep spinning, 
+				}
+				// If armed meant to keep spinning,
 				else if ((spinWhileArmed && !positiveThrottle) ||
 					 (status[ct] < 0) )
-					status[ct] = 0;					
+					status[ct] = 0;
 			}
-			
+
 			// If an accessory channel is selected for direct bypass mode
 			// In this configuration the accessory channel is scaled and mapped
 			// directly to output.  Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
-			// these also will not be updated in failsafe mode.  I'm not sure what 
+			// these also will not be updated in failsafe mode.  I'm not sure what
 			// the correct behavior is since it seems domain specific.  I don't love
 			// this code
-			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && 
-			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY2))
+			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
+			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5))
 			{
 				if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0)
 					status[ct] = accessory.AccessoryVal;
 				else
 					status[ct] = -1;
 			}
-			
+			if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) &&
+			   (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW))
+			{
+				CameraDesiredData cameraDesired;
+				if( CameraDesiredGet(&cameraDesired) == 0 ) {
+					switch(mixers[ct].type) {
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAROLL:
+							status[ct] = cameraDesired.Roll;
+							break;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH:
+							status[ct] = cameraDesired.Pitch;
+							break;
+						case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
+							status[ct] = cameraDesired.Yaw;
+							break;
+						default:
+							break;
+					}
+				}
+				else
+					status[ct] = -1;
+			}
+
 			command.Channel[ct] = scaleChannel(status[ct],
 							   ChannelMax[ct],
 							   ChannelMin[ct],
 							   ChannelNeutral[ct]);
 		}
+#if defined(DIAGNOSTICS)
 		MixerStatusSet(&mixerStatus);
+#endif

 		// Store update time
 		command.UpdateTime = 1000*dT;
 		if(1000*dT > command.MaxUpdateTime)
 			command.MaxUpdateTime = 1000*dT;
-		
+
 		// Update output object
 		ActuatorCommandSet(&command);
 		// Update in case read only (eg. during servo configuration)
@ -300,7 +340,7 @@ static void actuatorTask(void* parameters)

 		// Update servo outputs
 		bool success = true;
-		
+
 		for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 		{
 			success &= set_channel(n, command.Channel[n]);
@ -309,7 +349,7 @@ static void actuatorTask(void* parameters)
 		if(!success) {
 			command.NumFailedUpdates++;
 			ActuatorCommandSet(&command);
-			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); 
+			AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
 		}

 	}
@ -463,7 +503,7 @@ static void setFailsafe()
 	// Reset ActuatorCommand to safe values
 	for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
 	{
-		
+
 		if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
 		{
 			Channel[n] = ChannelMin[n];
@ -510,10 +550,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 }
 #else
 static bool set_channel(uint8_t mixer_channel, uint16_t value) {
-	
+
 	ActuatorSettingsData settings;
 	ActuatorSettingsGet(&settings);
-	
+
 	switch(settings.ChannelType[mixer_channel]) {
 		case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: {
 			// This is for buzzers that take a PWM input
@ -565,7 +605,7 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 			PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value);
 			return true;
 #if defined(PIOS_INCLUDE_I2C_ESC)
-		case ACTUATORSETTINGS_CHANNELTYPE_MK: 
+		case ACTUATORSETTINGS_CHANNELTYPE_MK:
 			return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);
 		case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
 			return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value);
@ -573,10 +613,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
 #endif
 		default:
 			return false;
-	}			
-	
+	}
+
 	return false;
-	
+
 }
 #endif

--- a/flight/Modules/Altitude/altitude.c
+++ b/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);
--- a/flight/Modules/Attitude/attitude.c
+++ b/flight/Modules/Attitude/attitude.c
@ -112,6 +112,10 @@ int32_t AttitudeStart(void)
 */
 int32_t AttitudeInitialize(void)
 {
+	AttitudeActualInitialize();
+	AttitudeRawInitialize();
+	AttitudeSettingsInitialize();
+
 	// Initialize quaternion
 	AttitudeActualData attitude;
 	AttitudeActualGet(&attitude);
--- a/flight/Modules/Battery/battery.c
+++ b/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));
--- a/flight/Modules/CameraStab/camerastab.c
+++ b/flight/Modules/CameraStab/camerastab.c
@ -0,0 +1,141 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup CameraStab Camera Stabilization Module
+ * @brief Camera stabilization module
+ * Updates accessory outputs with values appropriate for camera stabilization
+ * @{
+ *
+ * @file       camerastab.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Stabilize camera against the roll pitch and yaw of aircraft
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Output object: Accessory
+ *
+ * This module will periodically calculate the output values for stabilizing the camera
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+
+#include "accessorydesired.h"
+#include "attitudeactual.h"
+#include "camerastabsettings.h"
+#include "cameradesired.h"
+
+//
+// Configuration
+//
+#define SAMPLE_PERIOD_MS		10
+
+// Private types
+
+// Private variables
+
+// Private functions
+static void attitudeUpdated(UAVObjEvent* ev);
+static float bound(float val);
+
+/**
+ * Initialise the module, called on startup
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t CameraStabInitialize(void)
+{
+	static UAVObjEvent ev;
+	ev.obj = AttitudeActualHandle();
+	ev.instId = 0;
+	ev.event = 0;
+
+	CameraStabSettingsInitialize();
+	CameraDesiredInitialize();
+
+	EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
+
+	return 0;
+}
+
+static void attitudeUpdated(UAVObjEvent* ev)
+{
+	if (ev->obj != AttitudeActualHandle())
+		return;
+
+	float attitude;
+	float output;
+	AccessoryDesiredData accessory;
+
+	CameraStabSettingsData cameraStab;
+	CameraStabSettingsGet(&cameraStab);
+
+	// Read any input channels
+	float inputs[3] = {0,0,0};
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL];
+	}
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH];
+	}
+	if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) {
+		if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
+			inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW];
+	}
+
+	// Set output channels
+	AttitudeActualRollGet(&attitude);
+	output = bound((attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]);
+	CameraDesiredRollSet(&output);
+
+	AttitudeActualPitchGet(&attitude);
+	output = bound((attitude + inputs[1])  / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]);
+	CameraDesiredPitchSet(&output);
+
+	AttitudeActualYawGet(&attitude);
+	output = bound((attitude + inputs[2])  / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]);
+	CameraDesiredYawSet(&output);
+
+}
+
+float bound(float val)
+{
+	return (val > 1) ? 1 : 
+		(val < -1) ? -1 :
+		val;
+}
+/**
+  * @}
+  */
+
+/**
+ * @}
+ */
--- a/flight/Modules/CameraStab/inc/camerastab.h
+++ b/flight/Modules/CameraStab/inc/camerastab.h
@ -1,45 +1,42 @@
-/**
- ******************************************************************************
- *
- * @file       uavobjectsinit.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Initialize all objects.
- *             Automatically generated by the UAVObjectGenerator.
- *   
- * @note       This is an automatically generated file.
- *             DO NOT modify manually.
- * @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"
-
-/**
- * Function used to initialize the first instance of each object.
- * This file is automatically updated by the UAVObjectGenerator.
- */
-extern initcall_t __uavobj_initcall_start[], __uavobj_initcall_end[];
-
-void UAVObjectsInitializeAll()
-{
-	initcall_t *fn;
-	int32_t ret;
-
-	for (fn = __uavobj_initcall_start; fn < __uavobj_initcall_end; fn++)
-		ret = (*fn)();
-}
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{ 
+ * @addtogroup BatteryModule Battery Module
+ * @{ 
+ *
+ * @file       battery.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Module to read the battery Voltage and Current periodically and set alarms appropriately.
+ *
+ * @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 BATTERY_H
+#define BATTERY_H
+
+#include "openpilot.h"
+
+int32_t CameraStabInitialize(void);
+
+#endif // BATTERY_H
+
+/**
+  * @}
+  * @}
+  */
--- a/flight/Modules/FirmwareIAP/firmwareiap.c
+++ b/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;
--- a/flight/Modules/GPS/GPS.c
+++ b/flight/Modules/GPS/GPS.c
@ -124,6 +124,10 @@ int32_t GPSStart(void)
 */
 int32_t GPSInitialize(void)
 {
+	GPSPositionInitialize();
+	GPSTimeInitialize();
+	HomeLocationInitialize();
+
 	// TODO: Get gps settings object
 	gpsPort = PIOS_COM_GPS;

--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@ -90,17 +90,15 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
 #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)

 /**
- * 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 +108,18 @@ int32_t ManualControlStart()
 int32_t ManualControlInitialize()
 {

+	/* Check the assumptions about uavobject enum's are correct */
+	if(!assumptions)
+		return -1;
+
+	AccessoryDesiredInitialize();
+	ManualControlCommandInitialize();
+	FlightStatusInitialize();
+	StabilizationDesiredInitialize();
+
+	// ManualControlSettingsInitialize(); // this is initialized in
+	// pios_board.c
+
 	return 0;
 }
 MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
@ -584,4 +594,3 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
  * @}
  * @}
  */
-
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/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.
@ -273,7 +277,9 @@ static void stabilizationTask(void* parameters)
 		}

 		uint8_t shouldUpdate = 1;
+#if defined(DIAGNOSTICS)
 		RateDesiredSet(&rateDesired);
+#endif
 		ActuatorDesiredGet(&actuatorDesired);
 		//Calculate desired command
 		for(int8_t ct=0; ct< MAX_AXES; ct++)
--- a/flight/Modules/System/systemmod.c
+++ b/flight/Modules/System/systemmod.c
@ -43,7 +43,9 @@
 #include "objectpersistence.h"
 #include "flightstatus.h"
 #include "systemstats.h"
+#include "systemsettings.h"
 #include "i2cstats.h"
+#include "taskinfo.h"
 #include "watchdogstatus.h"
 #include "taskmonitor.h"
 #include "pios_config.h"
@ -78,11 +80,12 @@ static int32_t stackOverflow;
 // 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
@ -106,6 +109,16 @@ int32_t SystemModStart(void)
 int32_t SystemModInitialize(void)
 {

+	// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
+	SystemSettingsInitialize();
+	SystemStatsInitialize();
+	ObjectPersistenceInitialize();
+#if defined(DIAGNOSTICS)
+	TaskInfoInitialize();
+	I2CStatsInitialize();
+	WatchdogStatusInitialize();
+#endif
+
 	SystemModStart();

 	return 0;
@ -137,9 +150,10 @@ static void systemTask(void *parameters)

 		// Update the system alarms
 		updateSystemAlarms();
+#if defined(DIAGNOSTICS)
 		updateI2Cstats();
 		updateWDGstats();
-		
+#endif
 		// Update the task status object
 		TaskMonitorUpdateAll();

@ -241,9 +255,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 /**
 * Called periodically to update the I2C statistics 
 */
-#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
-static void updateI2Cstats() {} //Posix and win32 don't have I2C
-#else
+#if defined(DIAGNOSTICS)
 static void updateI2Cstats() 
 {
 #if defined(PIOS_INCLUDE_I2C)
@ -263,7 +275,6 @@ static void updateI2Cstats()
 	I2CStatsSet(&i2cStats);
 #endif
 }
-#endif

 static void updateWDGstats() 
 {
@ -272,6 +283,8 @@ static void updateWDGstats()
 	watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
 	WatchdogStatusSet(&watchdogStatus);
 }
+#endif
+

 /**
 * Called periodically to update the system stats
--- a/flight/Modules/Telemetry/telemetry.c
+++ b/flight/Modules/Telemetry/telemetry.c
@ -88,7 +88,13 @@ static void updateSettings();
 */
 int32_t TelemetryStart(void)
 {
-
+	// Process all registered objects and connect queue for updates
+	UAVObjIterate(&registerObject);
+    
+	// Listen to objects of interest
+	GCSTelemetryStatsConnectQueue(priorityQueue);
+	TelemetrySettingsConnectQueue(priorityQueue);
+    
 	// Start telemetry tasks
 	xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
 	xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
@ -111,6 +117,10 @@ int32_t TelemetryStart(void)
 int32_t TelemetryInitialize(void)
 {
 	UAVObjEvent ev;
+    
+	FlightTelemetryStatsInitialize();
+	GCSTelemetryStatsInitialize();
+	TelemetrySettingsInitialize();

 	// Initialize vars
 	timeOfLastObjectUpdate = 0;
@ -120,25 +130,19 @@ int32_t TelemetryInitialize(void)
 #if defined(PIOS_TELEM_PRIORITY_QUEUE)
 	priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 #endif
-	
-	// Get telemetry settings object
-	updateSettings();

+    // Get telemetry settings object
+	updateSettings();
+    
 	// Initialise UAVTalk
 	UAVTalkInitialize(&transmitData);
-
-	// Process all registered objects and connect queue for updates
-	UAVObjIterate(&registerObject);
-
+    
 	// Create periodic event that will be used to update the telemetry stats
 	txErrors = 0;
 	txRetries = 0;
 	memset(&ev, 0, sizeof(UAVObjEvent));
 	EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
-
-	// Listen to objects of interest
-	GCSTelemetryStatsConnectQueue(priorityQueue);
-	TelemetrySettingsConnectQueue(priorityQueue);
+    

 	return 0;
 }
--- a/flight/OpenPilot/Makefile
+++ b/flight/OpenPilot/Makefile
@ -135,7 +135,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
-SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
 else
 ## TESTCODE
 SRC += $(OPTESTS)/test_common.c
@ -388,6 +387,9 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -g$(DEBUGF) -DDEBUG
 endif

+# OP has enough memory to always enable optional objects
+CFLAGS += -DDIAGNOSTICS
+
 CFLAGS += -O$(OPT)
 CFLAGS += -mcpu=$(MCU)
 CFLAGS += $(CDEFS)
--- a/flight/OpenPilot/System/alarms.c
+++ b/flight/OpenPilot/System/alarms.c
@ -41,14 +41,12 @@ static xSemaphoreHandle lock;
 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);

 /**
- * Initialize the alarms library
+ * Initialize the alarms library
 */
 int32_t AlarmsInitialize(void)
 {
+	SystemAlarmsInitialize();
 	lock = xSemaphoreCreateRecursiveMutex();
-	//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
-	//AlarmsClearAll();
-	//AlarmsDefaultAll();
 	return 0;
 }

@ -56,7 +54,7 @@ int32_t AlarmsInitialize(void)
 * Set an alarm
 * @param alarm The system alarm to be modified
 * @param severity The alarm severity
- * @return 0 if success, -1 if an error
+ * @return 0 if success, -1 if an error
 */
 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
 {
@ -151,7 +149,7 @@ void AlarmsClearAll()

 /**
 * Check if there are any alarms with the given or higher severity
- * @return 0 if no alarms are found, 1 if at least one alarm is found
+ * @return 0 if no alarms are found, 1 if at least one alarm is found
 */
 int32_t AlarmsHasWarnings()
 {
@ -208,5 +206,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 /**
 * @}
 * @}
- */
+ */

--- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h
+++ b/flight/OpenPilot/System/inc/FreeRTOSConfig.h
@ -72,7 +72,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 +83,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)
--- a/flight/OpenPilot/System/pios_board.c
+++ b/flight/OpenPilot/System/pios_board.c
@ -1023,7 +1023,6 @@ void PIOS_Board_Init(void) {
 	/* Initialize UAVObject libraries */
 	EventDispatcherInitialize();
 	UAVObjInitialize();
-	UAVObjectsInitializeAll();

 #if defined(PIOS_INCLUDE_RTC)
 	/* Initialize the real-time clock and its associated tick */
--- a/flight/OpenPilot/System/taskmonitor.c
+++ b/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
 }
--- a/flight/OpenPilot/UAVObjects.inc
+++ b/flight/OpenPilot/UAVObjects.inc
@ -69,6 +69,8 @@ UAVOBJSRCFILENAMES += velocityactual
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus
 UAVOBJSRCFILENAMES += flightstatus
+UAVOBJSRCFILENAMES += cameradesired
+UAVOBJSRCFILENAMES += camerastabsettings

 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )
--- a/flight/PiOS.posix/inc/pios_initcall.h
+++ b/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 { \
--- a/flight/PiOS.win32/inc/pios_initcall.h
+++ b/flight/PiOS.win32/inc/pios_initcall.h
@ -38,7 +38,6 @@
 * and we cannot define a linker script for each of them atm
 */

-#define UAVOBJ_INITCALL(fn)
 #define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags)

 #define MODULE_TASKCREATE_ALL
--- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld
+++ b/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.*)
--- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
+++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
@ -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 :
    {
--- a/flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld
+++ b/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 :
    {
--- a/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld
+++ b/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld
@ -206,16 +206,6 @@ SECTIONS
        . = ALIGN(4);
    } > FLASH
 
-    
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } > FLASH

    /* the program code is stored in the .text section, which goes to Flash */
    .text :
--- a/flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld
+++ b/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 :
    {
--- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
+++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
@ -181,17 +181,6 @@ SECTIONS
        *(.flashtext)            /* Startup code */
        . = ALIGN(4);
    } >FLASH
- 
-    
-    /* init sections */
-    .initcalluavobj.init :
-    {
-        . = ALIGN(4);
-	__uavobj_initcall_start = .;
-        KEEP(*(.initcalluavobj.init))
-	. = ALIGN(4);
-	__uavobj_initcall_end   = .;
-    } >FLASH

    /* module sections */
    .initcallmodule.init :
--- a/flight/PiOS/inc/pios_initcall.h
+++ b/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++) \
--- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
+++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
@ -85,6 +85,7 @@
 		6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = "<group>"; };
 		6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = "<group>"; };
 		655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
+		655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = camerastabsettings.xml; sourceTree = "<group>"; };
 		656268C612DC1923007B0A0F /* nedaccel.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = nedaccel.xml; sourceTree = "<group>"; };
 		6562BE1713CCAD0600C823E8 /* pios_rcvr.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_rcvr.c; sourceTree = "<group>"; };
 		65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
@ -2680,7 +2681,6 @@
 		65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = watchdogstatus.xml; sourceTree = "<group>"; };
 		65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjecttemplate.c; sourceTree = "<group>"; };
 		65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinittemplate.c; sourceTree = "<group>"; };
-		65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_cc.c; sourceTree = "<group>"; };
 		65C35EA112F0A834004811C2 /* uavobjectmanager.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectmanager.c; sourceTree = "<group>"; };
 		65C35EA312F0A834004811C2 /* eventdispatcher.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = eventdispatcher.h; sourceTree = "<group>"; };
 		65C35EA412F0A834004811C2 /* uavobjectmanager.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = uavobjectmanager.h; sourceTree = "<group>"; };
@ -2690,9 +2690,13 @@
 		65C35EA812F0A834004811C2 /* eventdispatcher.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = eventdispatcher.c; sourceTree = "<group>"; };
 		65C35F6612F0DC2D004811C2 /* attitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = attitude.c; sourceTree = "<group>"; };
 		65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = "<group>"; };
+		65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = "<group>"; };
+		65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = "<group>"; };
 		65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = "<group>"; };
 		65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = "<group>"; };
+		65DEA79113F2143B00095B06 /* cameradesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = cameradesired.xml; sourceTree = "<group>"; };
 		65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = "<group>"; };
+		65E6D80713E3A4D0002A557A /* hwsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = hwsettings.xml; sourceTree = "<group>"; };
 		65E6DF7112E02E8E00058553 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = "<group>"; };
 		65E6DF7312E02E8E00058553 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = alarms.c; sourceTree = "<group>"; };
 		65E6DF7412E02E8E00058553 /* coptercontrol.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = coptercontrol.c; sourceTree = "<group>"; };
@ -3208,6 +3212,7 @@
 				650D8E2812DFE16400D05CC9 /* Altitude */,
 				65C35F6512F0DC2D004811C2 /* Attitude */,
 				650D8E2E12DFE16400D05CC9 /* Battery */,
+				65C9903B13A871B90082BD60 /* CameraStab */,
 				650D8E3212DFE16400D05CC9 /* Example */,
 				650D8E3B12DFE16400D05CC9 /* FirmwareIAP */,
 				650D8E3F12DFE16400D05CC9 /* FlightPlan */,
@ -3517,7 +3522,6 @@
 			children = (
 				65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */,
 				65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */,
-				65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */,
 				65C35EA112F0A834004811C2 /* uavobjectmanager.c */,
 				65C35EA212F0A834004811C2 /* inc */,
 				65C35EA812F0A834004811C2 /* eventdispatcher.c */,
@ -7422,6 +7426,7 @@
 		65C35E4F12EFB2F3004811C2 /* uavobjectdefinition */ = {
 			isa = PBXGroup;
 			children = (
+				65E6D80713E3A4D0002A557A /* hwsettings.xml */,
 				65E8C788139AA2A800E1F979 /* accessorydesired.xml */,
 				65C35E5012EFB2F3004811C2 /* actuatorcommand.xml */,
 				65C35E5112EFB2F3004811C2 /* actuatordesired.xml */,
@ -7434,6 +7439,7 @@
 				65E410AE12F65AEA00725888 /* attitudesettings.xml */,
 				65C35E5912EFB2F3004811C2 /* baroaltitude.xml */,
 				65C35E5A12EFB2F3004811C2 /* batterysettings.xml */,
+				655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */,
 				652C8568132B632A00BFCC70 /* firmwareiapobj.xml */,
 				65C35E5C12EFB2F3004811C2 /* flightbatterystate.xml */,
 				65C35E5D12EFB2F3004811C2 /* flightplancontrol.xml */,
@ -7468,6 +7474,7 @@
 				65C35E7712EFB2F3004811C2 /* velocityactual.xml */,
 				65C35E7812EFB2F3004811C2 /* velocitydesired.xml */,
 				65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */,
+				65DEA79113F2143B00095B06 /* cameradesired.xml */,
 			);
 			path = uavobjectdefinition;
 			sourceTree = "<group>";
@ -7501,6 +7508,23 @@
 			path = inc;
 			sourceTree = "<group>";
 		};
+		65C9903B13A871B90082BD60 /* CameraStab */ = {
+			isa = PBXGroup;
+			children = (
+				65C9903C13A871B90082BD60 /* camerastab.c */,
+				65C9903D13A871B90082BD60 /* inc */,
+			);
+			path = CameraStab;
+			sourceTree = "<group>";
+		};
+		65C9903D13A871B90082BD60 /* inc */ = {
+			isa = PBXGroup;
+			children = (
+				65C9903E13A871B90082BD60 /* camerastab.h */,
+			);
+			path = inc;
+			sourceTree = "<group>";
+		};
 		65E6DF7012E02E8E00058553 /* CopterControl */ = {
 			isa = PBXGroup;
 			children = (
--- a/flight/UAVObjects/uavobjectsinittemplate.c
+++ b/flight/UAVObjects/uavobjectsinittemplate.c
@ -36,5 +36,7 @@ $(OBJINC)
 */
 void UAVObjectsInitializeAll()
 {
+return;
+// This function is no longer used anyway
 $(OBJINIT)
 }
--- a/flight/UAVObjects/uavobjecttemplate.c
+++ b/flight/UAVObjects/uavobjecttemplate.c
@ -45,10 +45,14 @@ static UAVObjHandle handle;
 /**
 * Initialize object.
 * \return 0 Success
- * \return -1 Failure
+ * \return -1 Failure to initialize or -2 for already initialized
 */
 int32_t $(NAME)Initialize(void)
 {
+	// Don't set the handle to null if already registered
+	if(UAVObjGetByID($(NAMEUC)_OBJID) != NULL)
+		return -2;
+	
 	// Register object with the object manager
 	handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0,
 			$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
@ -64,8 +68,6 @@ int32_t $(NAME)Initialize(void)
 	}
 }

-UAVOBJ_INITCALL($(NAME)Initialize);
-
 /**
 * Initialize object fields and metadata with the default values.
 * If a default value is not specified the object fields
--- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui
+++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui
@ -0,0 +1,441 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>CameraStabilizationWidget</class>
+ <widget class="QWidget" name="CameraStabilizationWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>720</width>
+    <height>567</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Form</string>
+  </property>
+  <layout class="QVBoxLayout" name="verticalLayout_2">
+   <item>
+    <widget class="QCheckBox" name="enableCameraStabilization">
+     <property name="text">
+      <string>Enable CameraStabilization module</string>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QLabel" name="label_7">
+     <property name="text">
+      <string>After enabling the module, you must power cycle before using and configuring.</string>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="Line" name="line">
+     <property name="orientation">
+      <enum>Qt::Horizontal</enum>
+     </property>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Channel Ranges (number of degrees full range)</string>
+     </property>
+     <layout class="QHBoxLayout" name="horizontalLayout">
+      <item>
+       <layout class="QFormLayout" name="formLayout_2">
+        <item row="2" column="0">
+         <widget class="QLabel" name="label_2">
+          <property name="text">
+           <string>Pitch</string>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QLabel" name="label_3">
+          <property name="text">
+           <string>Yaw</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="0">
+         <widget class="QLabel" name="label">
+          <property name="text">
+           <string>Roll</string>
+          </property>
+         </widget>
+        </item>
+        <item row="1" column="1">
+         <widget class="QSpinBox" name="rollOutputRange">
+          <property name="maximum">
+           <number>180</number>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QSpinBox" name="pitchOutputRange">
+          <property name="maximum">
+           <number>180</number>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QSpinBox" name="yawOutputRange">
+          <property name="maximum">
+           <number>180</number>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <spacer name="horizontalSpacer_4">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>40</width>
+          <height>20</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+      <item>
+       <spacer name="horizontalSpacer_2">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>212</width>
+          <height>20</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Channel Mapping (select output channel or none to disable)</string>
+     </property>
+     <layout class="QHBoxLayout" name="horizontalLayout_2">
+      <item>
+       <layout class="QFormLayout" name="formLayout">
+        <item row="2" column="0">
+         <widget class="QLabel" name="label_4">
+          <property name="text">
+           <string>Roll</string>
+          </property>
+         </widget>
+        </item>
+        <item row="2" column="1">
+         <widget class="QComboBox" name="rollChannel">
+          <item>
+           <property name="text">
+            <string>None</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 1</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 2</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 3</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 4</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 5</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 6</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 7</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 8</string>
+           </property>
+          </item>
+         </widget>
+        </item>
+        <item row="4" column="1">
+         <widget class="QComboBox" name="yawChannel">
+          <item>
+           <property name="text">
+            <string>None</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 1</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 2</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 3</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 4</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 5</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 6</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 7</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 8</string>
+           </property>
+          </item>
+         </widget>
+        </item>
+        <item row="3" column="0">
+         <widget class="QLabel" name="label_5">
+          <property name="text">
+           <string>Pitch</string>
+          </property>
+         </widget>
+        </item>
+        <item row="3" column="1">
+         <widget class="QComboBox" name="pitchChannel">
+          <item>
+           <property name="text">
+            <string>None</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 1</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 2</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 3</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 4</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 5</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 6</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 7</string>
+           </property>
+          </item>
+          <item>
+           <property name="text">
+            <string>Channel 8</string>
+           </property>
+          </item>
+         </widget>
+        </item>
+        <item row="4" column="0">
+         <widget class="QLabel" name="label_6">
+          <property name="text">
+           <string>Yaw</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </item>
+      <item>
+       <spacer name="horizontalSpacer_5">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>40</width>
+          <height>20</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+      <item>
+       <spacer name="horizontalSpacer_3">
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="sizeHint" stdset="0">
+         <size>
+          <width>212</width>
+          <height>20</height>
+         </size>
+        </property>
+       </spacer>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
+    <spacer name="verticalSpacer">
+     <property name="orientation">
+      <enum>Qt::Vertical</enum>
+     </property>
+     <property name="sizeHint" stdset="0">
+      <size>
+       <width>20</width>
+       <height>40</height>
+      </size>
+     </property>
+    </spacer>
+   </item>
+   <item>
+    <layout class="QHBoxLayout" name="submitButtons">
+     <item>
+      <widget class="QFrame" name="frame">
+       <property name="frameShape">
+        <enum>QFrame::StyledPanel</enum>
+       </property>
+       <property name="frameShadow">
+        <enum>QFrame::Raised</enum>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QPushButton" name="camerastabilizationHelp">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="maximumSize">
+        <size>
+         <width>32</width>
+         <height>32</height>
+        </size>
+       </property>
+       <property name="font">
+        <font>
+         <kerning>true</kerning>
+        </font>
+       </property>
+       <property name="text">
+        <string/>
+       </property>
+       <property name="icon">
+        <iconset resource="../coreplugin/core.qrc">
+         <normaloff>:/core/images/helpicon.svg</normaloff>:/core/images/helpicon.svg</iconset>
+       </property>
+       <property name="iconSize">
+        <size>
+         <width>32</width>
+         <height>32</height>
+        </size>
+       </property>
+       <property name="shortcut">
+        <string>Ctrl+S</string>
+       </property>
+       <property name="default">
+        <bool>false</bool>
+       </property>
+       <property name="flat">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QPushButton" name="camerastabilizationSaveRAM">
+       <property name="toolTip">
+        <string>Save settings to the board (RAM only).
+
+This does not save the calibration settings, this is done using the
+specific calibration button on top of the screen.</string>
+       </property>
+       <property name="text">
+        <string>Apply</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QPushButton" name="camerastabilizationSaveSD">
+       <property name="toolTip">
+        <string>Send settings to the board, and save to the non-volatile memory.</string>
+       </property>
+       <property name="text">
+        <string>Save</string>
+       </property>
+       <property name="checked">
+        <bool>false</bool>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+  </layout>
+ </widget>
+ <resources>
+  <include location="../coreplugin/core.qrc"/>
+ </resources>
+ <connections/>
+</ui>
--- a/ground/openpilotgcs/src/plugins/config/config.pro
+++ b/ground/openpilotgcs/src/plugins/config/config.pro
@ -37,7 +37,8 @@ HEADERS += configplugin.h \
    calibration.h \
    defaultattitudewidget.h \
    smartsavebutton.h \
-    defaulthwsettingswidget.h
+    defaulthwsettingswidget.h \
+    configcamerastabilizationwidget.h

 SOURCES += configplugin.cpp \
    configgadgetconfiguration.cpp \
@ -65,7 +66,8 @@ SOURCES += configplugin.cpp \
    alignment-calibration.cpp \
    defaultattitudewidget.cpp \
    smartsavebutton.cpp \
-    defaulthwsettingswidget.cpp
+    defaulthwsettingswidget.cpp \
+    configcamerastabilizationwidget.cpp
    
 FORMS +=  \
    airframe.ui \
@ -78,6 +80,11 @@ FORMS +=  \
    output.ui \
    ccattitude.ui \
    defaultattitude.ui \
-    defaulthwsettings.ui
+    defaulthwsettings.ui \
+    camerastabilization.ui

 RESOURCES += configgadget.qrc
+
+
+
+
--- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp
@ -0,0 +1,237 @@
+/**
+ ******************************************************************************
+ *
+ * @file       configcamerastabilizationwidget.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup ConfigPlugin Config Plugin
+ * @{
+ * @brief The Configuration Gadget used to update settings in the firmware
+ *****************************************************************************/
+/*
+ * 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 "configcamerastabilizationwidget.h"
+
+#include <QDebug>
+#include <QTimer>
+#include <QStringList>
+#include <QtGui/QWidget>
+#include <QtGui/QTextEdit>
+#include <QtGui/QVBoxLayout>
+#include <QtGui/QPushButton>
+#include <QThread>
+#include <iostream>
+#include <QUrl>
+#include <QDesktopServices>
+
+#include "camerastabsettings.h"
+#include "hwsettings.h"
+#include "mixersettings.h"
+
+ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent) : ConfigTaskWidget(parent)
+{
+    m_camerastabilization = new Ui_CameraStabilizationWidget();
+    m_camerastabilization->setupUi(this);
+
+    connectUpdates();
+
+    // Connect buttons
+    connect(m_camerastabilization->camerastabilizationSaveRAM,SIGNAL(clicked()),this,SLOT(applySettings()));
+    connect(m_camerastabilization->camerastabilizationSaveSD,SIGNAL(clicked()),this,SLOT(saveSettings()));
+    connect(m_camerastabilization->camerastabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
+}
+
+ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget()
+{
+   // Do nothing
+}
+
+void ConfigCameraStabilizationWidget::connectUpdates()
+{
+    // Now connect the widget to the StabilizationSettings object
+    connect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+    connect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+    // TODO: This will need to support both CC and OP later
+    connect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+}
+
+void ConfigCameraStabilizationWidget::disconnectUpdates()
+{
+    // Now connect the widget to the StabilizationSettings object
+    disconnect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+    disconnect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+    // TODO: This will need to support both CC and OP later
+    disconnect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
+}
+
+/**
+  * @brief Populate the gui settings into the appropriate
+  * UAV structures
+  */
+void ConfigCameraStabilizationWidget::applySettings()
+{
+    // Enable or disable the settings
+    HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager());
+    HwSettings::DataFields hwSettingsData = hwSettings->getData();
+    hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] =
+            m_camerastabilization->enableCameraStabilization->isChecked() ?
+                HwSettings::OPTIONALMODULES_ENABLED :
+                HwSettings::OPTIONALMODULES_DISABLED;
+
+    // Update the mixer settings
+    MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager());
+    MixerSettings::DataFields mixerSettingsData = mixerSettings->getData();
+    const int NUM_MIXERS = 8;
+
+    QComboBox * selectors[3] = {
+        m_camerastabilization->rollChannel,
+        m_camerastabilization->pitchChannel,
+        m_camerastabilization->yawChannel
+    };
+
+    // TODO: Need to reformat object so types are an
+    // array themselves.  This gets really awkward
+    quint8 * mixerTypes[NUM_MIXERS] = {
+        &mixerSettingsData.Mixer1Type,
+        &mixerSettingsData.Mixer2Type,
+        &mixerSettingsData.Mixer3Type,
+        &mixerSettingsData.Mixer4Type,
+        &mixerSettingsData.Mixer5Type,
+        &mixerSettingsData.Mixer6Type,
+        &mixerSettingsData.Mixer7Type,
+        &mixerSettingsData.Mixer8Type,
+    };
+
+    for (int i = 0; i < 3; i++)
+    {
+        // Channel 1 is second entry, so becomes zero
+        int mixerNum = selectors[i]->currentIndex() - 1;
+
+        if ( mixerNum >= 0 && // Short circuit in case of none
+             *mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED &&
+             (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLL + i) ) {
+            // If the mixer channel already to something that isn't what we are
+            // about to set it to reset to none
+            selectors[i]->setCurrentIndex(0);
+        } else {
+            // Make sure no other channels have this output set
+            for (int j = 0; j < NUM_MIXERS; j++)
+                if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i))
+                    *mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED;
+
+            // If this channel is assigned to one of the outputs that is not disabled
+            // set it
+            if(mixerNum >= 0 && mixerNum < NUM_MIXERS)
+                *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLL + i;
+        }
+    }
+
+    // Update the ranges
+    CameraStabSettings * cameraStab = CameraStabSettings::GetInstance(getObjectManager());
+    CameraStabSettings::DataFields cameraStabData = cameraStab->getData();
+    cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL] = m_camerastabilization->rollOutputRange->value();
+    cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH] = m_camerastabilization->pitchOutputRange->value();
+    cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW] = m_camerastabilization->yawOutputRange->value();
+
+    // Because multiple objects are updated, and all of them trigger the callback
+    // they must be done together (if update one then load settings from second
+    // the first update would wipe the UI controls).  However to be extra cautious
+    // I'm also disabling updates during the setting to the UAVObjects
+    disconnectUpdates();
+    hwSettings->setData(hwSettingsData);
+    mixerSettings->setData(mixerSettingsData);
+    cameraStab->setData(cameraStabData);
+    connectUpdates();
+}
+
+/**
+  * Push settings into UAV objects then save them
+  */
+void ConfigCameraStabilizationWidget::saveSettings()
+{
+    applySettings();
+    UAVObject * obj = HwSettings::GetInstance(getObjectManager());
+    saveObjectToSD(obj);
+    obj = MixerSettings::GetInstance(getObjectManager());
+    saveObjectToSD(obj);
+    obj = CameraStabSettings::GetInstance(getObjectManager());
+    saveObjectToSD(obj);
+}
+
+void ConfigCameraStabilizationWidget::refreshValues()
+{
+    HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager());
+    HwSettings::DataFields hwSettingsData = hwSettings->getData();
+    m_camerastabilization->enableCameraStabilization->setChecked(
+                hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] ==
+                HwSettings::OPTIONALMODULES_ENABLED);
+
+    CameraStabSettings * cameraStabSettings = CameraStabSettings::GetInstance(getObjectManager());
+    CameraStabSettings::DataFields cameraStab = cameraStabSettings->getData();
+    m_camerastabilization->rollOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL]);
+    m_camerastabilization->pitchOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH]);
+    m_camerastabilization->yawOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW]);
+
+    MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager());
+    MixerSettings::DataFields mixerSettingsData = mixerSettings->getData();
+    const int NUM_MIXERS = 8;
+    QComboBox * selectors[3] = {
+        m_camerastabilization->rollChannel,
+        m_camerastabilization->pitchChannel,
+        m_camerastabilization->yawChannel
+    };
+
+    // TODO: Need to reformat object so types are an
+    // array themselves.  This gets really awkward
+    quint8 * mixerTypes[NUM_MIXERS] = {
+        &mixerSettingsData.Mixer1Type,
+        &mixerSettingsData.Mixer2Type,
+        &mixerSettingsData.Mixer3Type,
+        &mixerSettingsData.Mixer4Type,
+        &mixerSettingsData.Mixer5Type,
+        &mixerSettingsData.Mixer6Type,
+        &mixerSettingsData.Mixer7Type,
+        &mixerSettingsData.Mixer8Type,
+    };
+
+    for (int i = 0; i < 3; i++)
+    {
+        // Default to none if not found.  Then search for any mixer channels set to
+        // this
+        selectors[i]->setCurrentIndex(0);
+        for (int j = 0; j < NUM_MIXERS; j++)
+            if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i) &&
+                    selectors[i]->currentIndex() != (j + 1))
+                selectors[i]->setCurrentIndex(j + 1);
+    }
+}
+
+void ConfigCameraStabilizationWidget::openHelp()
+{
+    QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Stabilization", QUrl::StrictMode) );
+}
+
+void ConfigCameraStabilizationWidget::enableControls(bool enable)
+{
+    m_camerastabilization->camerastabilizationSaveSD->setEnabled(enable);
+    m_camerastabilization->camerastabilizationSaveRAM->setEnabled(enable);
+}
+
+/**
+  @}
+  @}
+  */
--- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h
@ -0,0 +1,68 @@
+/**
+ ******************************************************************************
+ *
+ * @file       configahrstwidget.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup ConfigPlugin Config Plugin
+ * @{
+ * @brief Telemetry configuration panel
+ *****************************************************************************/
+/*
+ * 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 CONFIGCAMERASTABILIZATIONWIDGET_H
+#define CONFIGCAMERASTABILIZATIONWIDGET_H
+
+#include "ui_camerastabilization.h"
+#include "configtaskwidget.h"
+#include "extensionsystem/pluginmanager.h"
+#include "uavobjectmanager.h"
+#include "uavobject.h"
+#include <QtGui/QWidget>
+#include <QtSvg/QSvgRenderer>
+#include <QtSvg/QGraphicsSvgItem>
+#include <QList>
+#include <QTimer>
+#include <QMutex>
+
+#include "camerastabsettings.h"
+
+class ConfigCameraStabilizationWidget: public ConfigTaskWidget
+{
+    Q_OBJECT
+
+public:
+    ConfigCameraStabilizationWidget(QWidget *parent = 0);
+    ~ConfigCameraStabilizationWidget();
+    
+private:
+    virtual void enableControls(bool enable);
+
+    Ui_CameraStabilizationWidget *m_camerastabilization;
+
+private slots:
+    void openHelp();
+    void applySettings();
+    void saveSettings();
+    void refreshValues();
+
+protected:
+    void connectUpdates();
+    void disconnectUpdates();
+};
+
+#endif // ConfigCameraStabilization_H
--- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc
+++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc
@ -15,5 +15,6 @@
        <file>images/coptercontrol.svg</file>
        <file>images/hw_config.png</file>
        <file>images/gyroscope.png</file>
+        <file>images/camera.png</file>
    </qresource>
 </RCC>
--- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
@ -33,6 +33,7 @@
 #include "configinputwidget.h"
 #include "configoutputwidget.h"
 #include "configstabilizationwidget.h"
+#include "configcamerastabilizationwidget.h"
 #include "config_pro_hw_widget.h"
 #include "config_cc_hw_widget.h"
 #include "defaultattitudewidget.h"
@ -81,6 +82,8 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
    qwd = new ConfigStabilizationWidget(this);
    ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization"));

+    qwd = new ConfigCameraStabilizationWidget(this);
+    ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab"));


 //    qwd = new ConfigPipXtremeWidget(this);
--- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
@ -50,7 +50,7 @@ class ConfigGadgetWidget: public QWidget
 public:
    ConfigGadgetWidget(QWidget *parent = 0);
    ~ConfigGadgetWidget();
-    enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization};
+    enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization, camerastabilization};

 public slots:
    void onAutopilotConnect();
--- a/ground/openpilotgcs/src/plugins/config/images/camera.png
+++ b/ground/openpilotgcs/src/plugins/config/images/camera.png
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
@ -31,6 +31,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
    $$UAVOBJECT_SYNTHETICS/ahrssettings.h \
    $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
    $$UAVOBJECT_SYNTHETICS/attituderaw.h \
+    $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \
    $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \
    $$UAVOBJECT_SYNTHETICS/systemstats.h \
    $$UAVOBJECT_SYNTHETICS/systemalarms.h \
@ -69,7 +70,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
    $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \
    $$UAVOBJECT_SYNTHETICS/flightstatus.h \
    $$UAVOBJECT_SYNTHETICS/hwsettings.h \
-    $$UAVOBJECT_SYNTHETICS/attitudesettings.h
+    $$UAVOBJECT_SYNTHETICS/attitudesettings.h \
+    $$UAVOBJECT_SYNTHETICS/cameradesired.h

 SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
    $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
@ -79,6 +81,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
    $$UAVOBJECT_SYNTHETICS/ahrssettings.cpp \
    $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \
    $$UAVOBJECT_SYNTHETICS/attituderaw.cpp \
+    $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \
    $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \
    $$UAVOBJECT_SYNTHETICS/systemstats.cpp \
    $$UAVOBJECT_SYNTHETICS/systemalarms.cpp \
@ -118,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
    $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
    $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \
    $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \
-    $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp
+    $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/cameradesired.cpp
--- a/shared/uavobjectdefinition/cameradesired.xml
+++ b/shared/uavobjectdefinition/cameradesired.xml
@ -0,0 +1,12 @@
+<xml>
+    <object name="CameraDesired" singleinstance="true" settings="false">
+        <description>Desired camera outputs.  Comes from @ref CameraStabilization module.</description>
+        <field name="Roll" units="" type="float" elements="1"/>
+        <field name="Pitch" units="" type="float" elements="1"/>
+        <field name="Yaw" units="" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="never" period="0"/>
+    </object>
+</xml>
--- a/shared/uavobjectdefinition/camerastabsettings.xml
+++ b/shared/uavobjectdefinition/camerastabsettings.xml
@ -0,0 +1,12 @@
+<xml>
+    <object name="CameraStabSettings" singleinstance="true" settings="true">
+        <description>Settings for the @ref CameraStab mmodule</description>
+        <field name="Inputs" units="channel" type="enum" elementnames="Roll,Pitch,Yaw" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,,None" defaultvalue="None"/>
+        <field name="InputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
+        <field name="OutputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="never" period="0"/>
+	</object>
+</xml>
--- a/shared/uavobjectdefinition/hwsettings.xml
+++ b/shared/uavobjectdefinition/hwsettings.xml
@ -1,11 +1,12 @@
 <xml>
-    <object name="HwSettings" singleinstance="true" settings="true">
-        <description>Selection of optional hardware configurations.</description>
-	<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,Spektrum,ComAux,I2C" defaultvalue="Disabled"/>
-	<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,S.Bus,GPS,Spektrum,ComAux" defaultvalue="Telemetry"/>
-        <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
-        <telemetryflight acked="true" updatemode="onchange" period="0"/>
-        <logging updatemode="never" period="0"/>
+	<object name="HwSettings" singleinstance="true" settings="true">
+		<description>Selection of optional hardware configurations.</description>
+		<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,Spektrum,ComAux,I2C" defaultvalue="Disabled"/>
+		<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,S.Bus,GPS,Spektrum,ComAux" defaultvalue="Telemetry"/>
+		<field name="OptionalModules" units="" type="enum" elementnames="CameraStabilization,GPS" options="Disabled,Enabled" defaultvalue="Disabled"/>
+		<access gcs="readwrite" flight="readwrite"/>
+		<telemetrygcs acked="true" updatemode="onchange" period="0"/>
+		<telemetryflight acked="true" updatemode="onchange" period="0"/>
+		<logging updatemode="never" period="0"/>
    </object>
 </xml>
--- a/shared/uavobjectdefinition/mixersettings.xml
+++ b/shared/uavobjectdefinition/mixersettings.xml
@ -5,24 +5,24 @@
        <field name="FeedForward" units="" type="float" elements="1" defaultvalue="0"/>
        <field name="AccelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
        <field name="DecelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
-	<field name="ThrottleCurve1" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0,0,0,0"/>
+	<field name="ThrottleCurve1" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0,0,0,0"/>
        <field name="Curve2Source" units="" type="enum" elements="1" options="Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Throttle"/>
        <field name="ThrottleCurve2" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0.25,0.5,0.75,1"/>
-        <field name="Mixer1Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer1Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer1Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer2Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer2Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer2Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer3Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer3Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer3Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer4Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer4Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer4Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer5Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer5Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer5Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer6Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer6Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer6Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer7Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer7Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer7Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
-        <field name="Mixer8Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
+        <field name="Mixer8Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
        <field name="Mixer8Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
        <access gcs="readwrite" flight="readwrite"/>
        <telemetrygcs acked="true" updatemode="onchange" period="0"/>