diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index 37a5fa063..d496ba08f 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -1,637 +1,637 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup ManualControlModule Manual Control Module
- * @brief Provide manual control or allow it alter flight mode.
- * @{
- *
- * Reads in the ManualControlCommand FlightMode setting from receiver then either
- * pass the settings straght to ActuatorDesired object (manual mode) or to
- * AttitudeDesired object (stabilized mode)
- *
- * @file       manualcontrol.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      ManualControl module. Handles safety R/C link and flight mode.
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#include "openpilot.h"
-#include "manualcontrol.h"
-#include "manualcontrolsettings.h"
-#include "stabilizationsettings.h"
-#include "manualcontrolcommand.h"
-#include "actuatordesired.h"
-#include "stabilizationdesired.h"
-#include "flighttelemetrystats.h"
-#include "flightstatus.h"
-#include "accessorydesired.h"
-
-// Private constants
-#if defined(PIOS_MANUAL_STACK_SIZE)
-#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
-#else
-#define STACK_SIZE_BYTES 824
-#endif
-
-#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
-#define UPDATE_PERIOD_MS 20
-#define THROTTLE_FAILSAFE -0.1
-#define FLIGHT_MODE_LIMIT 1.0/3.0
-#define ARMED_TIME_MS      1000
-#define ARMED_THRESHOLD    0.50
-//safe band to allow a bit of calibration error or trim offset (in microseconds)
-#define CONNECTION_OFFSET 150
-
-// Private types
-typedef enum
-{
-	ARM_STATE_DISARMED,
-	ARM_STATE_ARMING_MANUAL,
-	ARM_STATE_ARMED,
-	ARM_STATE_DISARMING_MANUAL,
-	ARM_STATE_DISARMING_TIMEOUT
-} ArmState_t;
-
-// Private variables
-static xTaskHandle taskHandle;
-static ArmState_t armState;
-static portTickType lastSysTime;
-
-// Private functions
-static void updateActuatorDesired(ManualControlCommandData * cmd);
-static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
-static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
-static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
-
-static void manualControlTask(void *parameters);
-static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
-static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
-static bool okToArm(void);
-static bool validInputRange(int16_t min, int16_t max, uint16_t value);
-
-#define assumptions1 ( \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)       && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)       && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)      \
-		)
-
-#define assumptions3 ( \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)      && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)      && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
-		)
-
-#define assumptions5 ( \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)      && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)      && \
-		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
-		)
-
-
-
-#define ARMING_CHANNEL_ROLL     0
-#define ARMING_CHANNEL_PITCH    1
-#define ARMING_CHANNEL_YAW      2
-
-#define assumptions7 ( \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_ROLL) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT          -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_ROLL) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD       -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_PITCH) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_PITCH) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT            -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_YAW) 		&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_YAW)		\
-		)
-
-#define assumptions8 ( \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT          -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD       -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0) 	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT            -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0)	&& \
-		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0)	\
-		)
-
-
-#define assumptions_flightmode ( \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
-		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
-		)
-
-#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
-
-/**
- * Module initialization
- */
-int32_t ManualControlInitialize()
-{
-	/* Check the assumptions about uavobject enum's are correct */
-	if(!assumptions)
-		return -1;
-
-	AccessoryDesiredInitialize();
-	ManualControlSettingsInitialize();
-	ManualControlCommandInitialize();
-	FlightStatusInitialize();
-	StabilizationDesiredInitialize();
-
-	// 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;
-}
-
-/**
- * Module task
- */
-static void manualControlTask(void *parameters)
-{
-	ManualControlSettingsData settings;
-	ManualControlCommandData cmd;
-	FlightStatusData flightStatus;
-	float flightMode = 0;
-
-	uint8_t disconnected_count = 0;
-	uint8_t connected_count = 0;
-
-	// For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
-	// this includes not even registering it if not used
-	AccessoryDesiredCreateInstance();
-	AccessoryDesiredCreateInstance();
-
-	// Make sure unarmed on power up
-	ManualControlCommandGet(&cmd);
-	FlightStatusGet(&flightStatus);
-	flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
-	armState = ARM_STATE_DISARMED;
-
-	// Main task loop
-	lastSysTime = xTaskGetTickCount();
-	while (1) {
-		float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
-
-		// Wait until next update
-		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
-		PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
-
-		// Read settings
-		ManualControlSettingsGet(&settings);
-
-		if (ManualControlCommandReadOnly(&cmd)) {
-			FlightTelemetryStatsData flightTelemStats;
-			FlightTelemetryStatsGet(&flightTelemStats);
-			if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-				/* trying to fly via GCS and lost connection.  fall back to transmitter */
-				UAVObjMetadata metadata;
-				UAVObjGetMetadata(&cmd, &metadata);
-				metadata.access = ACCESS_READWRITE;
-				UAVObjSetMetadata(&cmd, &metadata);
-			}
-		}
-
-		if (!ManualControlCommandReadOnly(&cmd)) {
-
-			// Read channel values in us
-			// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
-			// selection of PWM and PPM. The configuration is currently done at compile time in
-			// the pios_config.h file.
-			for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
-#if defined(PIOS_INCLUDE_PWM)
-				cmd.Channel[n] = PIOS_PWM_Get(n);
-#elif defined(PIOS_INCLUDE_PPM)
-				cmd.Channel[n] = PIOS_PPM_Get(n);
-#elif defined(PIOS_INCLUDE_SPEKTRUM)
-				cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
-#endif
-				scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n],	settings.ChannelMin[n], settings.ChannelNeutral[n]);
-			}
-
-			// Check settings, if error raise alarm
-			if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
-			    settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
-			    settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
-			    settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
-			    settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-				ManualControlCommandSet(&cmd);
-				continue;
-			}
-
-			// decide if we have valid manual input or not
-			bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
-			     validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
-			     validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
-			     validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
-
-			// Implement hysteresis loop on connection status
-			if (valid_input_detected && (++connected_count > 10)) {
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
-				connected_count = 0;
-				disconnected_count = 0;
-			} else if (!valid_input_detected && (++disconnected_count > 10)) {
-				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
-				connected_count = 0;
-				disconnected_count = 0;
-			}
-
-			if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
-				cmd.Throttle = -1;	// Shut down engine with no control
-				cmd.Roll = 0;
-				cmd.Yaw = 0;
-				cmd.Pitch = 0;
-				//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
-				// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
-				// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				ManualControlCommandSet(&cmd);
-			} else {
-				AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
-
-				// Scale channels to -1 -> +1 range
-				cmd.Roll           = scaledChannel[settings.Roll];
-				cmd.Pitch          = scaledChannel[settings.Pitch];
-				cmd.Yaw            = scaledChannel[settings.Yaw];
-				cmd.Throttle       = scaledChannel[settings.Throttle];
-				flightMode         = scaledChannel[settings.FlightMode];
-
-				AccessoryDesiredData accessory;
-				// Set Accessory 0
-				if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
-					accessory.AccessoryVal = scaledChannel[settings.Accessory0];
-					if(AccessoryDesiredInstSet(0, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accessory 1
-				if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
-					accessory.AccessoryVal = scaledChannel[settings.Accessory1];
-					if(AccessoryDesiredInstSet(1, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-				// Set Accsesory 2
-				if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
-					accessory.AccessoryVal = scaledChannel[settings.Accessory2];
-					if(AccessoryDesiredInstSet(2, &accessory) != 0)
-						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
-				}
-
-
-				processFlightMode(&settings, flightMode);
-				processArm(&cmd, &settings);
-
-				// Update cmd object
-				ManualControlCommandSet(&cmd);
-
-			}
-
-		} else {
-			ManualControlCommandGet(&cmd);	/* Under GCS control */
-		}
-
-
-		FlightStatusGet(&flightStatus);
-
-		// Depending on the mode update the Stabilization or Actuator objects
-		switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
-			case FLIGHTMODE_UNDEFINED:
-				// This reflects a bug in the code architecture!
-				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-				break;
-			case FLIGHTMODE_MANUAL:
-				updateActuatorDesired(&cmd);
-				break;
-			case FLIGHTMODE_STABILIZED:
-				updateStabilizationDesired(&cmd, &settings);
-				break;
-			case FLIGHTMODE_GUIDANCE:
-				// TODO: Implement
-				break;
-		}
-	}
-}
-
-static void updateActuatorDesired(ManualControlCommandData * cmd)
-{
-	ActuatorDesiredData actuator;
-	ActuatorDesiredGet(&actuator);
-	actuator.Roll = cmd->Roll;
-	actuator.Pitch = cmd->Pitch;
-	actuator.Yaw = cmd->Yaw;
-	actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-	ActuatorDesiredSet(&actuator);
-}
-
-static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
-{
-	StabilizationDesiredData stabilization;
-	StabilizationDesiredGet(&stabilization);
-
-	StabilizationSettingsData stabSettings;
-	StabilizationSettingsGet(&stabSettings);
-
-	uint8_t * stab_settings;
-	FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
-	switch(flightStatus.FlightMode) {
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
-			stab_settings = settings->Stabilization1Settings;
-			break;
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
-			stab_settings = settings->Stabilization2Settings;
-			break;
-		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
-			stab_settings = settings->Stabilization3Settings;
-			break;
-		default:
-			// Major error, this should not occur because only enter this block when one of these is true
-			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
-			return;
-	}
-
-	// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL]  = stab_settings[0];
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
-	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW]   = stab_settings[2];
-
-	stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
-		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
-		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
-		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax :
-	     0; // this is an invalid mode
-					      ;
-	stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
-		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
-		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
-		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax :
-		0; // this is an invalid mode
-
-	stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
-		(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
-		(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) :
-		0; // this is an invalid mode
-
-	stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
-	StabilizationDesiredSet(&stabilization);
-}
-
-/**
- * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
- */
-static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
-{
-	float valueScaled;
-
-	// Scale
-	if ((max > min && value >= neutral) || (min > max && value <= neutral))
-	{
-		if (max != neutral)
-			valueScaled = (float)(value - neutral) / (float)(max - neutral);
-		else
-			valueScaled = 0;
-	}
-	else
-	{
-		if (min != neutral)
-			valueScaled = (float)(value - neutral) / (float)(neutral - min);
-		else
-			valueScaled = 0;
-	}
-
-	// Bound
-	if (valueScaled >  1.0) valueScaled =  1.0;
-	else
-	if (valueScaled < -1.0) valueScaled = -1.0;
-
-	return valueScaled;
-}
-
-static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
-	if(end_time > start_time)
-		return (end_time - start_time) * portTICK_RATE_MS;
-	return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
-}
-
-/**
- * @brief Determine if the aircraft is safe to arm
- * @returns True if safe to arm, false otherwise
- */
-static bool okToArm(void)
-{
-	// read alarms
-	SystemAlarmsData alarms;
-	SystemAlarmsGet(&alarms);
-
-
-	// Check each alarm
-	for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
-	{
-		if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
-		{	// found an alarm thats set
-			if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
-				continue;
-
-			return false;
-		}
-	}
-
-	return true;
-}
-
-/**
- * @brief Update the flightStatus object only if value changed.  Reduces callbacks
- * @param[in] val The new value
- */
-static void setArmedIfChanged(uint8_t val) {
-	FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
-
-	if(flightStatus.Armed != val) {
-		flightStatus.Armed = val;
-		FlightStatusSet(&flightStatus);
-	}
-}
-
-/**
- * @brief Process the inputs and determine whether to arm or not
- * @param[out] cmd The structure to set the armed in
- * @param[in] settings Settings indicating the necessary position
- */
-static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
-{
-
-	bool lowThrottle = cmd->Throttle <= 0;
-
-	if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
-		// In this configuration we always disarm
-		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-	} else {
-		// Not really needed since this function not called when disconnected
-		if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
-			return;
-
-		// The throttle is not low, in case we where arming or disarming, abort
-		if (!lowThrottle) {
-			switch(armState) {
-				case ARM_STATE_DISARMING_MANUAL:
-				case ARM_STATE_DISARMING_TIMEOUT:
-					armState = ARM_STATE_ARMED;
-					break;
-				case ARM_STATE_ARMING_MANUAL:
-					armState = ARM_STATE_DISARMED;
-					break;
-				default:
-					// Nothing needs to be done in the other states
-					break;
-			}
-			return;
-		}
-
-		// The rest of these cases throttle is low
-		if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
-			// In this configuration, we go into armed state as soon as the throttle is low, never disarm
-			setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-			return;
-		}
-
-
-		// When the configuration is not "Always armed" and no "Always disarmed",
-		// the state will not be changed when the throttle is not low
-		static portTickType armedDisarmStart;
-		float armingInputLevel = 0;
-
-		// Calc channel see assumptions7
-		int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
-		switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
-			case ARMING_CHANNEL_ROLL:    armingInputLevel = sign * cmd->Roll;    break;
-			case ARMING_CHANNEL_PITCH:   armingInputLevel = sign * cmd->Pitch;   break;
-			case ARMING_CHANNEL_YAW:     armingInputLevel = sign * cmd->Yaw;     break;
-		}
-
-		bool manualArm = false;
-		bool manualDisarm = false;
-
-		if (armingInputLevel <= -ARMED_THRESHOLD)
-			manualArm = true;
-		else if (armingInputLevel >= +ARMED_THRESHOLD)
-			manualDisarm = true;
-
-		switch(armState) {
-			case ARM_STATE_DISARMED:
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
-
-				// only allow arming if it's OK too
-				if (manualArm && okToArm()) {
-					armedDisarmStart = lastSysTime;
-					armState = ARM_STATE_ARMING_MANUAL;
-				}
-				break;
-
-			case ARM_STATE_ARMING_MANUAL:
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
-
-				if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
-					armState = ARM_STATE_ARMED;
-				else if (!manualArm)
-					armState = ARM_STATE_DISARMED;
-				break;
-
-			case ARM_STATE_ARMED:
-				// When we get here, the throttle is low,
-				// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
-				armedDisarmStart = lastSysTime;
-				armState = ARM_STATE_DISARMING_TIMEOUT;
-				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
-				break;
-
-			case ARM_STATE_DISARMING_TIMEOUT:
-				// We get here when armed while throttle low, even when the arming timeout is not enabled
-				if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
-					armState = ARM_STATE_DISARMED;
-
-				// Switch to disarming due to manual control when needed
-				if (manualDisarm) {
-					armedDisarmStart = lastSysTime;
-					armState = ARM_STATE_DISARMING_MANUAL;
-				}
-				break;
-
-			case ARM_STATE_DISARMING_MANUAL:
-				if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
-					armState = ARM_STATE_DISARMED;
-				else if (!manualDisarm)
-					armState = ARM_STATE_ARMED;
-				break;
-		}	// End Switch
-	}
-}
-
-/**
- * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
- * @param[out] cmd Pointer to the command structure to set the flight mode in
- * @param[in] settings The settings which indicate which position is which mode
- * @param[in] flightMode the value of the switch position
- */
-static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
-{
-	FlightStatusData flightStatus;
-	FlightStatusGet(&flightStatus);
-
-	uint8_t newMode;
-	// Note here the code is ass
-	if (flightMode < -FLIGHT_MODE_LIMIT)
-		newMode = settings->FlightModePosition[0];
-	else if (flightMode > FLIGHT_MODE_LIMIT)
-		newMode = settings->FlightModePosition[2];
-	else
-		newMode = settings->FlightModePosition[1];
-
-	if(flightStatus.FlightMode != newMode) {
-		flightStatus.FlightMode = newMode;
-		FlightStatusSet(&flightStatus);
-	}
-
-}
-
-/**
- * @brief Determine if the manual input value is within acceptable limits
- * @returns return TRUE if so, otherwise return FALSE
- */
-bool validInputRange(int16_t min, int16_t max, uint16_t value)
-{
-	if (min > max)
-	{
-		int16_t tmp = min;
-		min = max;
-		max = tmp;
-	}
-	return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
-}
-
-/**
-  * @}
-  * @}
-  */
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotModules OpenPilot Modules
+ * @{
+ * @addtogroup ManualControlModule Manual Control Module
+ * @brief Provide manual control or allow it alter flight mode.
+ * @{
+ *
+ * Reads in the ManualControlCommand FlightMode setting from receiver then either
+ * pass the settings straght to ActuatorDesired object (manual mode) or to
+ * AttitudeDesired object (stabilized mode)
+ *
+ * @file       manualcontrol.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      ManualControl module. Handles safety R/C link and flight mode.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+#include "manualcontrol.h"
+#include "manualcontrolsettings.h"
+#include "stabilizationsettings.h"
+#include "manualcontrolcommand.h"
+#include "actuatordesired.h"
+#include "stabilizationdesired.h"
+#include "flighttelemetrystats.h"
+#include "flightstatus.h"
+#include "accessorydesired.h"
+
+// Private constants
+#if defined(PIOS_MANUAL_STACK_SIZE)
+#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
+#else
+#define STACK_SIZE_BYTES 824
+#endif
+
+#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
+#define UPDATE_PERIOD_MS 20
+#define THROTTLE_FAILSAFE -0.1
+#define FLIGHT_MODE_LIMIT 1.0/3.0
+#define ARMED_TIME_MS      1000
+#define ARMED_THRESHOLD    0.50
+//safe band to allow a bit of calibration error or trim offset (in microseconds)
+#define CONNECTION_OFFSET 150
+
+// Private types
+typedef enum
+{
+	ARM_STATE_DISARMED,
+	ARM_STATE_ARMING_MANUAL,
+	ARM_STATE_ARMED,
+	ARM_STATE_DISARMING_MANUAL,
+	ARM_STATE_DISARMING_TIMEOUT
+} ArmState_t;
+
+// Private variables
+static xTaskHandle taskHandle;
+static ArmState_t armState;
+static portTickType lastSysTime;
+
+// Private functions
+static void updateActuatorDesired(ManualControlCommandData * cmd);
+static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+static void processFlightMode(ManualControlSettingsData * settings, float flightMode);
+static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
+
+static void manualControlTask(void *parameters);
+static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
+static bool okToArm(void);
+static bool validInputRange(int16_t min, int16_t max, uint16_t value);
+
+#define assumptions1 ( \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)       && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)       && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)      \
+		)
+
+#define assumptions3 ( \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)      && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)      && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
+		)
+
+#define assumptions5 ( \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE)      && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE      == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE)      && \
+		((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE  == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)     \
+		)
+
+
+
+#define ARMING_CHANNEL_ROLL     0
+#define ARMING_CHANNEL_PITCH    1
+#define ARMING_CHANNEL_YAW      2
+
+#define assumptions7 ( \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_ROLL) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT          -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_ROLL) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD       -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_PITCH) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_PITCH) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT            -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_YAW) 		&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 	== ARMING_CHANNEL_YAW)		\
+		)
+
+#define assumptions8 ( \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT          -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD       -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0) 	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT            -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	== 0)	&& \
+		( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT           -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 	!= 0)	\
+		)
+
+
+#define assumptions_flightmode ( \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \
+		( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \
+		)
+
+#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
+
+/**
+ * Module initialization
+ */
+int32_t ManualControlInitialize()
+{
+	/* Check the assumptions about uavobject enum's are correct */
+	if(!assumptions)
+		return -1;
+
+	AccessoryDesiredInitialize();
+	ManualControlSettingsInitialize();
+	ManualControlCommandInitialize();
+	FlightStatusInitialize();
+	StabilizationDesiredInitialize();
+
+	// 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;
+}
+
+/**
+ * Module task
+ */
+static void manualControlTask(void *parameters)
+{
+	ManualControlSettingsData settings;
+	ManualControlCommandData cmd;
+	FlightStatusData flightStatus;
+	float flightMode = 0;
+
+	uint8_t disconnected_count = 0;
+	uint8_t connected_count = 0;
+
+	// For now manual instantiate extra instances of Accessory Desired.  In future should be done dynamically
+	// this includes not even registering it if not used
+	AccessoryDesiredCreateInstance();
+	AccessoryDesiredCreateInstance();
+
+	// Make sure unarmed on power up
+	ManualControlCommandGet(&cmd);
+	FlightStatusGet(&flightStatus);
+	flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
+	armState = ARM_STATE_DISARMED;
+
+	// Main task loop
+	lastSysTime = xTaskGetTickCount();
+	while (1) {
+		float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
+
+		// Wait until next update
+		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
+		PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
+
+		// Read settings
+		ManualControlSettingsGet(&settings);
+
+		if (ManualControlCommandReadOnly(&cmd)) {
+			FlightTelemetryStatsData flightTelemStats;
+			FlightTelemetryStatsGet(&flightTelemStats);
+			if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
+				/* trying to fly via GCS and lost connection.  fall back to transmitter */
+				UAVObjMetadata metadata;
+				UAVObjGetMetadata(&cmd, &metadata);
+				metadata.access = ACCESS_READWRITE;
+				UAVObjSetMetadata(&cmd, &metadata);
+			}
+		}
+
+		if (!ManualControlCommandReadOnly(&cmd)) {
+
+			// Read channel values in us
+			// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
+			// selection of PWM and PPM. The configuration is currently done at compile time in
+			// the pios_config.h file.
+			for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
+#if defined(PIOS_INCLUDE_PWM)
+				cmd.Channel[n] = PIOS_PWM_Get(n);
+#elif defined(PIOS_INCLUDE_PPM)
+				cmd.Channel[n] = PIOS_PPM_Get(n);
+#elif defined(PIOS_INCLUDE_SPEKTRUM)
+				cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
+#endif
+				scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n],	settings.ChannelMin[n], settings.ChannelNeutral[n]);
+			}
+
+			// Check settings, if error raise alarm
+			if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
+			    settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
+			    settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
+			    settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
+			    settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
+				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+				ManualControlCommandSet(&cmd);
+				continue;
+			}
+
+			// decide if we have valid manual input or not
+			bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
+			     validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
+			     validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
+			     validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
+
+			// Implement hysteresis loop on connection status
+			if (valid_input_detected && (++connected_count > 10)) {
+				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
+				connected_count = 0;
+				disconnected_count = 0;
+			} else if (!valid_input_detected && (++disconnected_count > 10)) {
+				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
+				connected_count = 0;
+				disconnected_count = 0;
+			}
+
+			if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
+				cmd.Throttle = -1;	// Shut down engine with no control
+				cmd.Roll = 0;
+				cmd.Yaw = 0;
+				cmd.Pitch = 0;
+				//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
+				// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
+				// or leave throttle at IDLE speed or above when going into AUTO-failsafe.
+				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				ManualControlCommandSet(&cmd);
+			} else {
+				AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
+
+				// Scale channels to -1 -> +1 range
+				cmd.Roll           = scaledChannel[settings.Roll];
+				cmd.Pitch          = scaledChannel[settings.Pitch];
+				cmd.Yaw            = scaledChannel[settings.Yaw];
+				cmd.Throttle       = scaledChannel[settings.Throttle];
+				flightMode         = scaledChannel[settings.FlightMode];
+
+				AccessoryDesiredData accessory;
+				// Set Accessory 0
+				if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) {
+					accessory.AccessoryVal = scaledChannel[settings.Accessory0];
+					if(AccessoryDesiredInstSet(0, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+				// Set Accessory 1
+				if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) {
+					accessory.AccessoryVal = scaledChannel[settings.Accessory1];
+					if(AccessoryDesiredInstSet(1, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+				// Set Accsesory 2
+				if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) {
+					accessory.AccessoryVal = scaledChannel[settings.Accessory2];
+					if(AccessoryDesiredInstSet(2, &accessory) != 0)
+						AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
+				}
+
+
+				processFlightMode(&settings, flightMode);
+				processArm(&cmd, &settings);
+
+				// Update cmd object
+				ManualControlCommandSet(&cmd);
+
+			}
+
+		} else {
+			ManualControlCommandGet(&cmd);	/* Under GCS control */
+		}
+
+
+		FlightStatusGet(&flightStatus);
+
+		// Depending on the mode update the Stabilization or Actuator objects
+		switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
+			case FLIGHTMODE_UNDEFINED:
+				// This reflects a bug in the code architecture!
+				AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+				break;
+			case FLIGHTMODE_MANUAL:
+				updateActuatorDesired(&cmd);
+				break;
+			case FLIGHTMODE_STABILIZED:
+				updateStabilizationDesired(&cmd, &settings);
+				break;
+			case FLIGHTMODE_GUIDANCE:
+				// TODO: Implement
+				break;
+		}
+	}
+}
+
+static void updateActuatorDesired(ManualControlCommandData * cmd)
+{
+	ActuatorDesiredData actuator;
+	ActuatorDesiredGet(&actuator);
+	actuator.Roll = cmd->Roll;
+	actuator.Pitch = cmd->Pitch;
+	actuator.Yaw = cmd->Yaw;
+	actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
+	ActuatorDesiredSet(&actuator);
+}
+
+static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
+{
+	StabilizationDesiredData stabilization;
+	StabilizationDesiredGet(&stabilization);
+
+	StabilizationSettingsData stabSettings;
+	StabilizationSettingsGet(&stabSettings);
+
+	uint8_t * stab_settings;
+	FlightStatusData flightStatus;
+	FlightStatusGet(&flightStatus);
+	switch(flightStatus.FlightMode) {
+		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
+			stab_settings = settings->Stabilization1Settings;
+			break;
+		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
+			stab_settings = settings->Stabilization2Settings;
+			break;
+		case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
+			stab_settings = settings->Stabilization3Settings;
+			break;
+		default:
+			// Major error, this should not occur because only enter this block when one of these is true
+			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
+			return;
+	}
+
+	// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
+	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL]  = stab_settings[0];
+	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
+	stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW]   = stab_settings[2];
+
+	stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
+		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
+		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
+		(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax :
+	     0; // this is an invalid mode
+					      ;
+	stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
+		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
+		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
+		(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax :
+		0; // this is an invalid mode
+
+	stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
+		(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
+		(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) :
+		0; // this is an invalid mode
+
+	stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
+	StabilizationDesiredSet(&stabilization);
+}
+
+/**
+ * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
+ */
+static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
+{
+	float valueScaled;
+
+	// Scale
+	if ((max > min && value >= neutral) || (min > max && value <= neutral))
+	{
+		if (max != neutral)
+			valueScaled = (float)(value - neutral) / (float)(max - neutral);
+		else
+			valueScaled = 0;
+	}
+	else
+	{
+		if (min != neutral)
+			valueScaled = (float)(value - neutral) / (float)(neutral - min);
+		else
+			valueScaled = 0;
+	}
+
+	// Bound
+	if (valueScaled >  1.0) valueScaled =  1.0;
+	else
+	if (valueScaled < -1.0) valueScaled = -1.0;
+
+	return valueScaled;
+}
+
+static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
+	if(end_time > start_time)
+		return (end_time - start_time) * portTICK_RATE_MS;
+	return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
+}
+
+/**
+ * @brief Determine if the aircraft is safe to arm
+ * @returns True if safe to arm, false otherwise
+ */
+static bool okToArm(void)
+{
+	// read alarms
+	SystemAlarmsData alarms;
+	SystemAlarmsGet(&alarms);
+
+
+	// Check each alarm
+	for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
+	{
+		if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
+		{	// found an alarm thats set
+			if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
+				continue;
+
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/**
+ * @brief Update the flightStatus object only if value changed.  Reduces callbacks
+ * @param[in] val The new value
+ */
+static void setArmedIfChanged(uint8_t val) {
+	FlightStatusData flightStatus;
+	FlightStatusGet(&flightStatus);
+
+	if(flightStatus.Armed != val) {
+		flightStatus.Armed = val;
+		FlightStatusSet(&flightStatus);
+	}
+}
+
+/**
+ * @brief Process the inputs and determine whether to arm or not
+ * @param[out] cmd The structure to set the armed in
+ * @param[in] settings Settings indicating the necessary position
+ */
+static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
+{
+
+	bool lowThrottle = cmd->Throttle <= 0;
+
+	if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
+		// In this configuration we always disarm
+		setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+	} else {
+		// Not really needed since this function not called when disconnected
+		if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
+			return;
+
+		// The throttle is not low, in case we where arming or disarming, abort
+		if (!lowThrottle) {
+			switch(armState) {
+				case ARM_STATE_DISARMING_MANUAL:
+				case ARM_STATE_DISARMING_TIMEOUT:
+					armState = ARM_STATE_ARMED;
+					break;
+				case ARM_STATE_ARMING_MANUAL:
+					armState = ARM_STATE_DISARMED;
+					break;
+				default:
+					// Nothing needs to be done in the other states
+					break;
+			}
+			return;
+		}
+
+		// The rest of these cases throttle is low
+		if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
+			// In this configuration, we go into armed state as soon as the throttle is low, never disarm
+			setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+			return;
+		}
+
+
+		// When the configuration is not "Always armed" and no "Always disarmed",
+		// the state will not be changed when the throttle is not low
+		static portTickType armedDisarmStart;
+		float armingInputLevel = 0;
+
+		// Calc channel see assumptions7
+		int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
+		switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
+			case ARMING_CHANNEL_ROLL:    armingInputLevel = sign * cmd->Roll;    break;
+			case ARMING_CHANNEL_PITCH:   armingInputLevel = sign * cmd->Pitch;   break;
+			case ARMING_CHANNEL_YAW:     armingInputLevel = sign * cmd->Yaw;     break;
+		}
+
+		bool manualArm = false;
+		bool manualDisarm = false;
+
+		if (armingInputLevel <= -ARMED_THRESHOLD)
+			manualArm = true;
+		else if (armingInputLevel >= +ARMED_THRESHOLD)
+			manualDisarm = true;
+
+		switch(armState) {
+			case ARM_STATE_DISARMED:
+				setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+
+				// only allow arming if it's OK too
+				if (manualArm && okToArm()) {
+					armedDisarmStart = lastSysTime;
+					armState = ARM_STATE_ARMING_MANUAL;
+				}
+				break;
+
+			case ARM_STATE_ARMING_MANUAL:
+				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
+
+				if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
+					armState = ARM_STATE_ARMED;
+				else if (!manualArm)
+					armState = ARM_STATE_DISARMED;
+				break;
+
+			case ARM_STATE_ARMED:
+				// When we get here, the throttle is low,
+				// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
+				armedDisarmStart = lastSysTime;
+				armState = ARM_STATE_DISARMING_TIMEOUT;
+				setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
+				break;
+
+			case ARM_STATE_DISARMING_TIMEOUT:
+				// We get here when armed while throttle low, even when the arming timeout is not enabled
+				if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
+					armState = ARM_STATE_DISARMED;
+
+				// Switch to disarming due to manual control when needed
+				if (manualDisarm) {
+					armedDisarmStart = lastSysTime;
+					armState = ARM_STATE_DISARMING_MANUAL;
+				}
+				break;
+
+			case ARM_STATE_DISARMING_MANUAL:
+				if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
+					armState = ARM_STATE_DISARMED;
+				else if (!manualDisarm)
+					armState = ARM_STATE_ARMED;
+				break;
+		}	// End Switch
+	}
+}
+
+/**
+ * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
+ * @param[out] cmd Pointer to the command structure to set the flight mode in
+ * @param[in] settings The settings which indicate which position is which mode
+ * @param[in] flightMode the value of the switch position
+ */
+static void processFlightMode(ManualControlSettingsData * settings, float flightMode)
+{
+	FlightStatusData flightStatus;
+	FlightStatusGet(&flightStatus);
+
+	uint8_t newMode;
+	// Note here the code is ass
+	if (flightMode < -FLIGHT_MODE_LIMIT)
+		newMode = settings->FlightModePosition[0];
+	else if (flightMode > FLIGHT_MODE_LIMIT)
+		newMode = settings->FlightModePosition[2];
+	else
+		newMode = settings->FlightModePosition[1];
+
+	if(flightStatus.FlightMode != newMode) {
+		flightStatus.FlightMode = newMode;
+		FlightStatusSet(&flightStatus);
+	}
+
+}
+
+/**
+ * @brief Determine if the manual input value is within acceptable limits
+ * @returns return TRUE if so, otherwise return FALSE
+ */
+bool validInputRange(int16_t min, int16_t max, uint16_t value)
+{
+	if (min > max)
+	{
+		int16_t tmp = min;
+		min = max;
+		max = tmp;
+	}
+	return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
+}
+
+/**
+  * @}
+  * @}
+  */