Exemplo n.º 1
0
/**
 * Module task
 */
static void manualControlTask(void *parameters)
{
	ManualControlSettingsData settings;
	StabilizationSettingsData stabSettings;
	ManualControlCommandData cmd;
	ActuatorDesiredData actuator;
	AttitudeDesiredData attitude;
	RateDesiredData rate;
	portTickType lastSysTime;
	

	float flightMode;

	uint8_t disconnected_count = 0;
	uint8_t connected_count = 0;
	enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;

	// Make sure unarmed on power up
	ManualControlCommandGet(&cmd);
	cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
	ManualControlCommandSet(&cmd);
	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);
		StabilizationSettingsGet(&stabSettings);

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

			// 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.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
				ManualControlCommandSet(&cmd);
				continue;
			}
			// 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]);
			}

			// 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];

			if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
				cmd.Accessory1 = scaledChannel[settings.Accessory1];
			else
				cmd.Accessory1 = 0;

			if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
				cmd.Accessory2 = scaledChannel[settings.Accessory2];
			else
				cmd.Accessory2 = 0;

			if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
				cmd.Accessory3 = scaledChannel[settings.Accessory3];
			else
				cmd.Accessory3 = 0;

			if (flightMode < -FLIGHT_MODE_LIMIT) {
				// Position 1
				for(int i = 0; i < 3; i++) {
					cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i];	// See assumptions1
				}
				cmd.FlightMode = settings.Pos1FlightMode;	// See assumptions2
			} else if (flightMode > FLIGHT_MODE_LIMIT) {
				// Position 3
				for(int i = 0; i < 3; i++) {
					cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i];	// See assumptions5
				}
				cmd.FlightMode = settings.Pos3FlightMode;	// See assumptions6
			} else {
				// Position 2
				for(int i = 0; i < 3; i++) {
					cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i];	// See assumptions3
				}
				cmd.FlightMode = settings.Pos2FlightMode;	// See assumptions4
			}
			// Update the ManualControlCommand object
			ManualControlCommandSet(&cmd);
			// This seems silly to set then get, but the reason is if the GCS is
			// the control input, the set command will be blocked by the read only
			// setting and the get command will pull the right values from telemetry
		} else
			ManualControlCommandGet(&cmd);	/* Under GCS control */
		

		// Implement hysteresis loop on connection status
		// Must check both Max and Min in case they reversed
		if (!ManualControlCommandReadOnly(&cmd) &&
			cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&
			cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {
			if (disconnected_count++ > 10) {
				connection_state = DISCONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				disconnected_count++;
		} else {
			if (connected_count++ > 10) {
				connection_state = CONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				connected_count++;
		}

		if (connection_state == DISCONNECTED) {
			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
			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
			ManualControlCommandSet(&cmd);
		} else {
			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
			ManualControlCommandSet(&cmd);
		} 

		// Arming and Disarming mechanism
		if (cmd.Throttle < 0) {
			// Throttle is low, in this condition the arming state could change

			uint8_t newCmdArmed = cmd.Armed;
			static portTickType armedDisarmStart;

			// Look for state changes and write in newArmState
			if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) {
				// No channel assigned to arming -> arm immediately when throttle is low
				newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
			} else {
				float armStickLevel;
				uint8_t channel = settings.Arming/2;    // 0=Channel1, 1=Channel1_Rev, 2=Channel2, ....
				bool reverse = (settings.Arming%2)==1;
				bool manualArm = false;
				bool manualDisarm = false;

				if (connection_state == CONNECTED) {
					// Should use RC input only if RX is connected
					armStickLevel = scaledChannel[channel];
					if (reverse)
						armStickLevel =-armStickLevel;

					if (armStickLevel <= -0.90)
						manualArm = true;
					else if (armStickLevel >= +0.90)
						manualDisarm = true;
				}

				switch(armState) {
				case ARM_STATE_DISARMED:
					newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
					if (manualArm) {
						armedDisarmStart = lastSysTime;
						armState = ARM_STATE_ARMING_MANUAL;
					}
					break;

				case ARM_STATE_ARMING_MANUAL:
					if (manualArm) {
						if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
							armState = ARM_STATE_ARMED;
					}
					else
						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;
					newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
					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)
						if (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) {
						if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
							armState = ARM_STATE_DISARMED;
					}
					else
						armState = ARM_STATE_ARMED;
					break;
				}
			}
			// Update cmd object when needed
			if (newCmdArmed != cmd.Armed) {
				cmd.Armed = newCmdArmed;
				ManualControlCommandSet(&cmd);
			}
		} else {
			// The throttle is not low, in case we where arming or disarming, abort
			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;
			}
		}
		// End of arming/disarming



		// Depending on the mode update the Stabilization or Actuator objects
		if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) {
			actuator.Roll = cmd.Roll;
			actuator.Pitch = cmd.Pitch;
			actuator.Yaw = cmd.Yaw;
			actuator.Throttle = cmd.Throttle;
			ActuatorDesiredSet(&actuator);
		} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {
			attitude.Roll = cmd.Roll * stabSettings.RollMax;
			attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
			attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
			attitude.Throttle =  (cmd.Throttle < 0) ? -1 : cmd.Throttle;
			rate.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
			rate.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
			rate.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];

			AttitudeDesiredSet(&attitude);
			RateDesiredSet(&rate);
		}
	}
}
Exemplo n.º 2
0
/**
 * Module task
 */
static void manualControlTask(void *parameters)
{
	ManualControlSettingsData settings;
	ManualControlCommandData cmd;
	portTickType lastSysTime;
	
	float flightMode = 0;

	uint8_t disconnected_count = 0;
	uint8_t connected_count = 0;
	enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;

	// Make sure unarmed on power up
	ManualControlCommandGet(&cmd);
	cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
	ManualControlCommandSet(&cmd);
	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)) {

			// 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;
			}
			// 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], 0);
			}

			// 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];

			if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
				cmd.Accessory1 = scaledChannel[settings.Accessory1];
			else
				cmd.Accessory1 = 0;

			if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
				cmd.Accessory2 = scaledChannel[settings.Accessory2];
			else
				cmd.Accessory2 = 0;

			if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
				cmd.Accessory3 = scaledChannel[settings.Accessory3];
			else
				cmd.Accessory3 = 0;

			// Note here the code is ass
			if (flightMode < -FLIGHT_MODE_LIMIT) 
				cmd.FlightMode = settings.FlightModePosition[0];
			else if (flightMode > FLIGHT_MODE_LIMIT) 
				cmd.FlightMode = settings.FlightModePosition[2];
			else 
				cmd.FlightMode = settings.FlightModePosition[1];
			
			// Update the ManualControlCommand object
			ManualControlCommandSet(&cmd);
			// This seems silly to set then get, but the reason is if the GCS is
			// the control input, the set command will be blocked by the read only
			// setting and the get command will pull the right values from telemetry
		} else
			ManualControlCommandGet(&cmd);	/* Under GCS control */


		// Implement hysteresis loop on connection status
		// Must check both Max and Min in case they reversed
		if (!ManualControlCommandReadOnly(&cmd) &&
			cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&
			cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {
			if (disconnected_count++ > 10) {
				connection_state = DISCONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				disconnected_count++;
		} else {
			if (connected_count++ > 10) {
				connection_state = CONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				connected_count++;
		}

		if (connection_state == DISCONNECTED) {
			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
			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
			ManualControlCommandSet(&cmd);
		} else {
			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
			ManualControlCommandSet(&cmd);
		}

		//
		// Arming and Disarming mechanism
		//
		// Look for state changes and write in newArmState
		uint8_t newCmdArmed = cmd.Armed;	// By default, keep the arming state the same

		if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
			// In this configuration we always disarm
			newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
		} else {
			// In all other cases, we will not change the arm state when disconnected
			if (connection_state == CONNECTED)
			{
				if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
					// In this configuration, we go into armed state as soon as the throttle is low, never disarm
					if (cmd.Throttle < 0) {
						newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
					}
				} else {
					// When the configuration is not "Always armed" and no "Always disarmed",
					// the state will not be changed when the throttle is not low
					if (cmd.Throttle < 0) {
						static portTickType armedDisarmStart;
						float armingInputLevel = 0;

						// Calc channel see assumptions7
						switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
						case ARMING_CHANNEL_ROLL: 	armingInputLevel = cmd.Roll; 	break;
						case ARMING_CHANNEL_PITCH: 	armingInputLevel = cmd.Pitch; 	break;
						case ARMING_CHANNEL_YAW: 	armingInputLevel = cmd.Yaw; 	break;
						}

						bool manualArm = false;
						bool manualDisarm = false;

						if (connection_state == CONNECTED) {
							// Should use RC input only if RX is connected
							if (armingInputLevel <= -0.90)
								manualArm = true;
							else if (armingInputLevel >= +0.90)
								manualDisarm = true;
						}

						// Swap arm-disarming see assumptions8
						if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) {
							bool temp = manualArm;
							manualArm = manualDisarm;
							manualDisarm = temp;
						}

						switch(armState) {
						case ARM_STATE_DISARMED:
							newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;

							if (manualArm)
							{
								if (okToArm())	// only allow arming if it's OK too
								{
									armedDisarmStart = lastSysTime;
									armState = ARM_STATE_ARMING_MANUAL;
								}
							}
							break;

						case ARM_STATE_ARMING_MANUAL:
							if (manualArm) {
								if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
									armState = ARM_STATE_ARMED;
							}
							else
								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;
							newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
							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)
								if (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) {
								if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
									armState = ARM_STATE_DISARMED;
							}
							else
								armState = ARM_STATE_ARMED;
							break;
						}	// End Switch
					} else {
						// The throttle is not low, in case we where arming or disarming, abort
						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;
						}
					}
				}
			}
		}
		// Update cmd object when needed
		if (newCmdArmed != cmd.Armed) {
			cmd.Armed = newCmdArmed;
			ManualControlCommandSet(&cmd);
		}
		//
		// End of arming/disarming
		//



		// Depending on the mode update the Stabilization or Actuator objects
		switch(PARSE_FLIGHT_MODE(cmd.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;
		}				
	}
}
Exemplo n.º 3
0
/**
 * Module task
 */
static void manualControlTask(void *parameters)
{
	ManualControlSettingsData settings;
	StabilizationSettingsData stabSettings;
	ManualControlCommandData cmd;
	ActuatorDesiredData actuator;
	AttitudeDesiredData attitude;
	portTickType lastSysTime;
	portTickType armedDisarmStart = 0;
	float flightMode;

	uint8_t disconnected_count = 0;
	uint8_t connected_count = 0;
	enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;

	// Make sure unarmed on power up
	ManualControlCommandGet(&cmd);
	cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
	ManualControlCommandSet(&cmd);

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while (1) {
		// Wait until next update
		vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);

		// Read settings
		ManualControlSettingsGet(&settings);
		StabilizationSettingsGet(&stabSettings);

		if (!ManualControlCommandReadOnly(&cmd)) {
			// 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.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
				cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
				ManualControlCommandSet(&cmd);
				continue;
			}
			// 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
			}

			// Calculate roll command in range +1 to -1
			cmd.Roll = scaleChannel(cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll],
						settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll]);

			// Calculate pitch command in range +1 to -1
			cmd.Pitch = scaleChannel(cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch],
						 settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch]);

			// Calculate yaw command in range +1 to -1
			cmd.Yaw = scaleChannel(cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw],
					       settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw]);

			// Calculate throttle command in range +1 to -1
			cmd.Throttle = scaleChannel(cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle],
						    settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle]);

			if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
				cmd.Accessory1 = scaleChannel(cmd.Channel[settings.Accessory1], settings.ChannelMax[settings.Accessory1],
							      settings.ChannelMin[settings.Accessory1], settings.ChannelNeutral[settings.Accessory1]);
			else
				cmd.Accessory1 = 0;

			if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
				cmd.Accessory2 = scaleChannel(cmd.Channel[settings.Accessory2], settings.ChannelMax[settings.Accessory2],
							      settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2]);
			else
				cmd.Accessory2 = 0;

			if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
				cmd.Accessory3 = scaleChannel(cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3],
							      settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3]);
			else
				cmd.Accessory3 = 0;

			// Update flight mode
			flightMode = scaleChannel(cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode],
						  settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode]);

			if (flightMode < -FLIGHT_MODE_LIMIT) {       // Position 1
				for(int i = 0; i < 3; i++) {
					if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
					else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
					else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
				}
				if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
				else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
				else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
			} else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3
				for(int i = 0; i < 3; i++) {
					if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
					else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
					else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
				}
				if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
				else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
				else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
			} else {                                     // Position 2
				for(int i = 0; i < 3; i++) {
					if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
					else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
					else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE)
						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
				}
				if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
				else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
				else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO)
					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
			}
			// Update the ManualControlCommand object
			ManualControlCommandSet(&cmd);
			// This seems silly to set then get, but the reason is if the GCS is
			// the control input, the set command will be blocked by the read only
			// setting and the get command will pull the right values from telemetry
		} else
			ManualControlCommandGet(&cmd);	/* Under GCS control */

		// Implement hysteresis loop on connection status
		// Must check both Max and Min in case they reversed
		if (!ManualControlCommandReadOnly(&cmd) &&
		    cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&
		    cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {
			if (disconnected_count++ > 10) {
				connection_state = DISCONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				disconnected_count++;
		} else {
			if (connected_count++ > 10) {
				connection_state = CONNECTED;
				connected_count = 0;
				disconnected_count = 0;
			} else
				connected_count++;
		}

		if (connection_state == DISCONNECTED) {
			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
			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
			ManualControlCommandSet(&cmd);
		} else {
			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
			ManualControlCommandSet(&cmd);
		}

		/* Look for arm or disarm signal */
		if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) {
			if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart))	// store when started, deal with rollover
				armedDisarmStart = lastSysTime;
			else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
				cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
		} else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) {
			if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart))	// store when started, deal with rollover
				armedDisarmStart = lastSysTime;
			else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
				cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
		} else {
			armedDisarmStart = 0;
		}

		// Depending on the mode update the Stabilization or Actuator objects
		if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) {
			actuator.Roll = cmd.Roll;
			actuator.Pitch = cmd.Pitch;
			actuator.Yaw = cmd.Yaw;
			actuator.Throttle = cmd.Throttle;
			ActuatorDesiredSet(&actuator);
		} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {
			attitude.Roll = cmd.Roll * stabSettings.RollMax;
			attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
			attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
			attitude.Throttle =  (cmd.Throttle < 0) ? -1 : cmd.Throttle;
			AttitudeDesiredSet(&attitude);
		}

		if (cmd.Accessory3 < -.5) {	//TODO: Make what happens here depend on GCS
			AHRSSettingsData attitudeSettings;
			AHRSSettingsGet(&attitudeSettings);
			// Hard coding a maximum bias of 15 for now... maybe mistake
			attitudeSettings.PitchBias = cmd.Accessory1 * 15;
			attitudeSettings.RollBias = cmd.Accessory2 * 15;
			AHRSSettingsSet(&attitudeSettings);
		}
	}
}