void VtolFlyController::UpdateAutoPilot()
{
    if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
        mManualThrust = true;
    }

    uint8_t result = RunAutoPilot();

    if (result) {
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
    } else {
        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
    }

    PathStatusSet(pathStatus);

    // If rtbl, detect arrival at the endpoint and then triggers a change
    // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
    // can't manage this.  And pathplanner whilst similar does not manage this as it is not a
    // waypoint traversal and is not aware of flight modes other than path plan.
    if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) {
        if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
            if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
                if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
                    plan_setup_land();
                }
            }
        }
    }
}

void FixedWingFlyController::UpdateAutoPilot()
{
    uint8_t result = updateAutoPilotFixedWing();

    if (result) {
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
    } else {
        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
    }

    PathStatusSet(pathStatus);
}

/**
 * Module thread, should not return.
 */
static void ahrscommsTask(void* parameters)
{
    portTickType lastSysTime;
    AhrsStatusData data;

    AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);

    /*Until AHRS connects, assume it doesn't know home */
    AhrsStatusGet(&data);
    data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
    //data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
    data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
    AhrsStatusSet(&data);

    // Main task loop
    while (1)
    {
        AHRSSettingsData settings;
        AHRSSettingsGet(&settings);

        AhrsSendObjects();
		AhrsCommStatus stat = AhrsGetStatus();
		if(stat.linkOk)
		{
			AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
		}else
		{
			AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
		}
		AhrsStatusData sData;
		AhrsStatusGet(&sData);

		sData.LinkRunning = stat.linkOk;
		sData.AhrsKickstarts = stat.remote.kickStarts;
		sData.AhrsCrcErrors = stat.remote.crcErrors;
		sData.AhrsRetries = stat.remote.retries;
		sData.AhrsInvalidPackets = stat.remote.invalidPacket;
		sData.OpCrcErrors = stat.local.crcErrors;
		sData.OpRetries = stat.local.retries;
		sData.OpInvalidPackets = stat.local.invalidPacket;

		AhrsStatusSet(&sData);
        /* Wait for the next update interval */
        vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );

    }
}

/**
 * Module task
 */
static void stabilizationTask(void* parameters)
{
	portTickType lastSysTime;
	portTickType thisSysTime;
	UAVObjEvent ev;


	QuadMotorsDesiredData actuatorDesired;
	FlightStatusData flightStatus;

	//SettingsUpdatedCb((UAVObjEvent *) NULL);

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while(1) {
		PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);

		// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
		if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
		{
			AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
			continue;
		}

		// Check how long since last update
		thisSysTime = xTaskGetTickCount();
		if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
			dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
		lastSysTime = thisSysTime;

		FlightStatusGet(&flightStatus);
		QuadMotorsDesiredGet(&actuatorDesired);

		//if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED)
		//{
			// ZERO MOTORS
		//}
		//else
		//{

		//}

		//ActuatorSettingsData settings;
			//ActuatorSettingsGet(&settings);

		//PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);

		PIOS_SetMKSpeed(0,actuatorDesired.motorFront_NW);
		PIOS_SetMKSpeed(1,actuatorDesired.motorRight_NE);
		PIOS_SetMKSpeed(2,actuatorDesired.motorBack_SE);
		PIOS_SetMKSpeed(3,actuatorDesired.motorLeft_SW);

		// Clear alarms
		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
	}
}

/* stub: module has no module thread */
int32_t GeofenceStart(void)
{
	if (geofenceSettings == NULL) {
		return -1;
	}

	// Schedule periodic task to check position
	UAVObjEvent ev = {
		.obj = PositionActualHandle(),
		.instId = 0,
		.event = 0,
	};
	EventPeriodicCallbackCreate(&ev, checkPosition, SAMPLE_PERIOD_MS);

	return 0;
}

MODULE_INITCALL(GeofenceInitialize, GeofenceStart);

/**
 * Periodic callback that processes changes in position and
 * sets the alarm.
 */
static void checkPosition(UAVObjEvent* ev, void *ctx, void *obj, int len)
{
	(void) ev; (void) ctx; (void) obj; (void) len;
	if (PositionActualHandle()) {
		PositionActualData positionActual;
		PositionActualGet(&positionActual);

		const float distance2 = powf(positionActual.North, 2) + powf(positionActual.East, 2);

		// ErrorRadius is squared when it is fetched, so this is correct
		if (distance2 > geofenceSettings->ErrorRadius) {
			AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_ERROR);
		} else if (distance2 > geofenceSettings->WarningRadius) {
			AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_WARNING);
		} else {
			AlarmsClear(SYSTEMALARMS_ALARM_GEOFENCE);
		}
	}
}

int32_t GPSStart(void)
{
	if (module_enabled) {
		if (gpsPort) {
			// Start gps task
			xTaskCreate(gpsTask, (signed char *)"GPS", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &gpsTaskHandle);
			TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
			return 0;
		}

		AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
	}
	return -1;
}

int32_t GPSStart(void)
{
	if (module_enabled) {
		if (gpsPort) {
			// Start gps task
			gpsTaskHandle = PIOS_Thread_Create(gpsTask, "GPS", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
			TaskMonitorAdd(TASKINFO_RUNNING_GPS, gpsTaskHandle);
			return 0;
		}

		AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
	}
	return -1;
}

/**
 * Update system alarms
 */
static void updateSystemAlarms()
{
	SystemStatsData stats;
	UAVObjStats objStats;
	EventStats evStats;
	SystemStatsGet(&stats);

	// Check heap
	if (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) {
		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
	} else if (stats.HeapRemaining < HEAP_LIMIT_WARNING) {
		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
	} else {
		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
	}

	// Check CPU load
	if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) {
		AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
	} else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) {
		AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
	} else {
		AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
	}

	// Check for stack overflow
	if (stackOverflow == 1) {
		AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
	} else {
		AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
	}

#if defined(PIOS_INCLUDE_SDCARD)
	// Check for SD card
	if (PIOS_SDCARD_IsMounted() == 0) {
		AlarmsSet(SYSTEMALARMS_ALARM_SDCARD, SYSTEMALARMS_ALARM_ERROR);
	} else {
		AlarmsClear(SYSTEMALARMS_ALARM_SDCARD);
	}
#endif

	// Check for event errors
	UAVObjGetStats(&objStats);
	EventGetStats(&evStats);
	UAVObjClearStats();
	EventClearStats();
	if (objStats.eventErrors > 0 || evStats.eventErrors > 0) {
		AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
	} else {
		AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
	}
}

/**
 * Module thread, should not return.
 */
static void ahrscommsTask(void *parameters)
{
	portTickType lastSysTime;

	AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);

	// Main task loop
	while (1) {
		PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS);
		AhrsCommStatus stat;
		
		AhrsSendObjects();
		AhrsGetStatus(&stat);
		if (stat.linkOk) {
			AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
		} else {
			AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
		}
		InsStatusData sData;
		InsStatusGet(&sData);

		sData.LinkRunning = stat.linkOk;
		sData.AhrsKickstarts = stat.remote.kickStarts;
		sData.AhrsCrcErrors = stat.remote.crcErrors;
		sData.AhrsRetries = stat.remote.retries;
		sData.AhrsInvalidPackets = stat.remote.invalidPacket;
		sData.OpCrcErrors = stat.local.crcErrors;
		sData.OpRetries = stat.local.retries;
		sData.OpInvalidPackets = stat.local.invalidPacket;

		InsStatusSet(&sData);
		/* Wait for the next update interval */
		vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS);

	}
}

static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
	StabilizationDesiredData stabilization;
	StabilizationDesiredGet(&stabilization);
	
	StabilizationSettingsData stabSettings;
	StabilizationSettingsGet(&stabSettings);
		
	uint8_t * stab_settings;
	switch(cmd->FlightMode) {
		case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1:
			stab_settings = settings->Stabilization1Settings;
			break;
		case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2:
			stab_settings = settings->Stabilization2Settings;
			break;
		case MANUALCONTROLCOMMAND_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);
			break;	
	}
	
	// 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.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL] :
	     (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? 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.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] :
	     (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? 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.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] :
	     (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
	     0; // this is an invalid mode
	
	stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; 
	StabilizationDesiredSet(&stabilization);
}

/**
 * Module thread, should not return.
 */
static void AttitudeTask(void *parameters)
{
	bool first_run = true;
	uint32_t last_algorithm;
	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

	// Force settings update to make sure rotation loaded
	settingsUpdatedCb(NULL);

	// Wait for all the sensors be to read
	vTaskDelay(100);

	// Invalidate previous algorithm to trigger a first run
	last_algorithm = 0xfffffff;

	// Main task loop
	while (1) {

		int32_t ret_val = -1;

		if (last_algorithm != revoSettings.FusionAlgorithm) {
			last_algorithm = revoSettings.FusionAlgorithm;
			first_run = true;
		}

		// This  function blocks on data queue
		switch (revoSettings.FusionAlgorithm ) {
			case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
				ret_val = updateAttitudeComplementary(first_run);
				break;
			case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR:
				ret_val = updateAttitudeINSGPS(first_run, true);
				break;
			case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR:
				ret_val = updateAttitudeINSGPS(first_run, false);
				break;
			default:
				AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL);
				break;
		}

		if(ret_val == 0)
			first_run = false;

		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
	}
}

/**
 * @brief Main module task
 */
static void actuatorTask(void* parameters)
{
//	UAVObjEvent ev;
	portTickType lastSysTime;
    ActuatorCommandData command;
	ActuatorSettingsData settings;

	// Set servo update frequency (done only on start-up)
	ActuatorSettingsGet(&settings);
	PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);

	// Go to the neutral (failsafe) values until an ActuatorDesired update is received
	setFailsafe();

	// Main task loop
	lastSysTime = xTaskGetTickCount();
	while (1)
	{

		ActuatorCommandGet(&command);
        ActuatorSettingsGet(&settings);
        if ( RunMixers(&command, &settings) == -1 )
        {
            AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
        }
        else
        {
            AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
        }

		// Update output object
		ActuatorCommandSet(&command);
		// Update in case read only (eg. during servo configuration)
		ActuatorCommandGet(&command);

		// Update servo outputs
		for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
		{
			PIOS_Servo_Set( n, command.Channel[n] );
		}
		// Wait until next update
		vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );

	}
}

/**
 * Set the error code and alarm state
 * @param[in] error code
 */
void set_config_error(SystemAlarmsConfigErrorOptions error_code)
{
	// Get the severity of the alarm given the error code
	SystemAlarmsAlarmOptions severity;

	static bool sticky = false;

	/* Once a sticky error occurs, never change the error code */
	if (sticky) return;

	switch (error_code) {
	case SYSTEMALARMS_CONFIGERROR_NONE:
		severity = SYSTEMALARMS_ALARM_OK;
		break;
	default:
		error_code = SYSTEMALARMS_CONFIGERROR_UNDEFINED;
		/* and fall through */

	case SYSTEMALARMS_CONFIGERROR_DUPLICATEPORTCFG:
		sticky = true;
		/* and fall through */
	case SYSTEMALARMS_CONFIGERROR_AUTOTUNE:
	case SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD:
	case SYSTEMALARMS_CONFIGERROR_MULTIROTOR:
	case SYSTEMALARMS_CONFIGERROR_NAVFILTER:
	case SYSTEMALARMS_CONFIGERROR_PATHPLANNER:
	case SYSTEMALARMS_CONFIGERROR_POSITIONHOLD:
	case SYSTEMALARMS_CONFIGERROR_STABILIZATION:
	case SYSTEMALARMS_CONFIGERROR_UNDEFINED:
	case SYSTEMALARMS_CONFIGERROR_UNSAFETOARM:
		severity = SYSTEMALARMS_ALARM_ERROR;
		break;
	}

	// Make sure not to set the error code if it didn't change
	SystemAlarmsConfigErrorOptions current_error_code;
	SystemAlarmsConfigErrorGet((uint8_t *) &current_error_code);
	if (current_error_code != error_code) {
		SystemAlarmsConfigErrorSet((uint8_t *) &error_code);
	}

	// AlarmSet checks only updates on toggle
	AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, (uint8_t) severity);
}

/**
 * Module thread, should not return.
 */
static void altitudeHoldTask(void *parameters)
{
	bool engaged = false;

	AltitudeHoldDesiredData altitudeHoldDesired;
	StabilizationDesiredData stabilizationDesired;
	AltitudeHoldSettingsData altitudeHoldSettings;

	UAVObjEvent ev;
	struct pid velocity_pid;

	// Listen for object updates.
	AltitudeHoldDesiredConnectQueue(queue);
	AltitudeHoldSettingsConnectQueue(queue);
	FlightStatusConnectQueue(queue);

	AltitudeHoldSettingsGet(&altitudeHoldSettings);
	pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
		          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);

	AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);

	// Main task loop
	const uint32_t dt_ms = 5;
	const float dt_s = dt_ms * 0.001f;
	uint32_t timeout = dt_ms;

	while (1) {
		if (PIOS_Queue_Receive(queue, &ev, timeout) != true) {

		} else if (ev.obj == FlightStatusHandle()) {

			uint8_t flight_mode;
			FlightStatusFlightModeGet(&flight_mode);

			if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) {
				// Copy the current throttle as a starting point for integral
				StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator);
				velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000
				engaged = true;
			} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
				engaged = false;

			// Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged
			timeout = engaged ? dt_ms : 100;

		} else if (ev.obj == AltitudeHoldDesiredHandle()) {
			AltitudeHoldDesiredGet(&altitudeHoldDesired);
		} else if (ev.obj == AltitudeHoldSettingsHandle()) {
			AltitudeHoldSettingsGet(&altitudeHoldSettings);

			pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
				          altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
		}

		bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE;

		// For landing mode allow throttle to go negative to allow the integrals
		// to stop winding up
		const float min_throttle = landing ? -0.1f : 0.0f;

		// When engaged compute altitude controller output
		if (engaged) {
			float position_z, velocity_z, altitude_error;

			PositionActualDownGet(&position_z);
			VelocityActualDownGet(&velocity_z);
			position_z = -position_z; // Use positive up convention
			velocity_z = -velocity_z; // Use positive up convention

			// Compute the altitude error
			altitude_error = altitudeHoldDesired.Altitude - position_z;

			// Velocity desired is from the outer controller plus the set point
			float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
			float throttle_desired = pid_apply_antiwindup(&velocity_pid, 
			                    velocity_desired - velocity_z,
			                    min_throttle, 1.0f, // positive limits since this is throttle
			                    dt_s);

			if (altitudeHoldSettings.AttitudeComp > 0) {
				// Throttle desired is at this point the mount desired in the up direction, we can
				// account for the attitude if desired
				AttitudeActualData attitudeActual;
				AttitudeActualGet(&attitudeActual);

				// Project a unit vector pointing up into the body frame and
				// get the z component
				float fraction = attitudeActual.q1 * attitudeActual.q1 -
				                 attitudeActual.q2 * attitudeActual.q2 -
				                 attitudeActual.q3 * attitudeActual.q3 +
				                 attitudeActual.q4 * attitudeActual.q4;

				// Add ability to scale up the amount of compensation to achieve
				// level forward flight
				fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f);

//.........这里部分代码省略.........

/**
 * 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];
//.........这里部分代码省略.........

/**
 * Module thread, should not return.
 */
static void vtolPathFollowerTask(void *parameters)
{
	SystemSettingsData systemSettings;
	FlightStatusData flightStatus;

	portTickType lastUpdateTime;
	
	VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
	PathDesiredConnectCallback(SettingsUpdatedCb);
	
	VtolPathFollowerSettingsGet(&guidanceSettings);
	PathDesiredGet(&pathDesired);
	
	// Main task loop
	lastUpdateTime = xTaskGetTickCount();
	while (1) {

		// Conditions when this runs:
		// 1. Must have VTOL type airframe
		// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
		//    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path

		SystemSettingsGet(&systemSettings);
		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
		{
			AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
			vTaskDelay(1000);
			continue;
		}

		// Continue collecting data if not enough time
		vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod));

		// Convert the accels into the NED frame
		updateNedAccel();
		
		FlightStatusGet(&flightStatus);

		// Check the combinations of flightmode and pathdesired mode
		switch(flightStatus.FlightMode) {
			/* This combination of RETURNTOHOME and HOLDPOSITION looks strange but
			 * is correct.  RETURNTOHOME mode uses HOLDPOSITION with the position
			 * set to home */
			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
				if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
					updateEndpointVelocity();
					updateVtolDesiredAttitude();
				} else {
					AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
				}
				break;
			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
				if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
					updateEndpointVelocity();
					updateVtolDesiredAttitude();
				} else {
					AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
				}
				break;
			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
				if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT ||
					pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {
					updateEndpointVelocity();
					updateVtolDesiredAttitude();
				} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||
					pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||
					pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {
					updatePathVelocity();
					updateVtolDesiredAttitude();
				} else {
					AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);
				}
				break;
			default:

				for (uint32_t i = 0; i < VTOL_PID_NUM; i++)
					pid_zero(&vtol_pids[i]);

				// Track throttle before engaging this mode.  Cheap system ident
				StabilizationDesiredData stabDesired;
				StabilizationDesiredGet(&stabDesired);
				throttleOffset = stabDesired.Throttle;

				break;
		}

		AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
//.........这里部分代码省略.........

/**
 * 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
//.........这里部分代码省略.........

/**
 * Module thread, should not return.
 */
static void pathfollowerTask(void *parameters)
{
	SystemSettingsData systemSettings;
	FlightStatusData flightStatus;
	
	uint32_t lastUpdateTime;
	
	AirspeedActualConnectCallback(airspeedActualUpdatedCb);
	FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
	FixedWingAirspeedsConnectCallback(SettingsUpdatedCb);
	PathDesiredConnectCallback(SettingsUpdatedCb);

	// Force update of all the settings
	SettingsUpdatedCb(NULL);
	
	FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
	path_desired_updated = false;
	PathDesiredGet(&pathDesired);
	PathDesiredConnectCallback(pathDesiredUpdated);

	// Main task loop
	lastUpdateTime = PIOS_Thread_Systime();
	while (1) {

		// Conditions when this runs:
		// 1. Must have FixedWing type airframe
		// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint  OR
		//    FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path

		SystemSettingsGet(&systemSettings);
		if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
		{
			AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
			PIOS_Thread_Sleep(1000);
			continue;
		}

		// Continue collecting data if not enough time
		PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod);

		static uint8_t last_flight_mode;
		FlightStatusGet(&flightStatus);
		PathStatusGet(&pathStatus);

		PositionActualData positionActual;

		static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE;

		// Check whether an update to the path desired occured and we should
		// process it. This makes sure that the follower alarm state is
		// updated.
		bool process_path_desired_update = 
		    (last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ||
		     last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
		    path_desired_updated;
		path_desired_updated = false;

		// Process most of these when the flight mode changes
		// except when in path following mode in which case
		// each iteration must make sure this has the latest
		// PathDesired
		if (flightStatus.FlightMode != last_flight_mode ||
			process_path_desired_update) {
			
			last_flight_mode = flightStatus.FlightMode;

			switch(flightStatus.FlightMode) {
			case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
				state = FW_FOLLOWER_RUNNING;

				PositionActualGet(&positionActual);

				pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
				pathDesired.Start[0] = positionActual.North;
				pathDesired.Start[1] = positionActual.East;
				pathDesired.Start[2] = positionActual.Down;
				pathDesired.End[0] = 0;
				pathDesired.End[1] = 0;
				pathDesired.End[2] = -30.0f;
				pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
				pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
				pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
				PathDesiredSet(&pathDesired);

				break;
			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
				state = FW_FOLLOWER_RUNNING;

				PositionActualGet(&positionActual);

				pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
				pathDesired.Start[0] = positionActual.North;
				pathDesired.Start[1] = positionActual.East;
				pathDesired.Start[2] = positionActual.Down;
				pathDesired.End[0] = positionActual.North;
//.........这里部分代码省略.........

/**
 * PIOS_Board_Init()
 * initializes all the core subsystems on this specific hardware
 * called from System/openpilot.c
 */
void PIOS_Board_Init(void) {

	/* Delay system */
	PIOS_DELAY_Init();

	const struct pios_board_info * bdinfo = &pios_board_info_blob;

#if defined(PIOS_INCLUDE_LED)
	PIOS_LED_Init(&pios_led_cfg);
#endif	/* PIOS_INCLUDE_LED */

#if defined(PIOS_INCLUDE_FLASH)
	/* Inititialize all flash drivers */
	if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0)
		panic(1);

	/* Register the partition table */
	const struct pios_flash_partition * flash_partition_table;
	uint32_t num_partitions;
	flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions);
	PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions);

	/* Mount all filesystems */
	if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0)
		panic(1);
#endif	/* PIOS_INCLUDE_FLASH */

	/* Initialize the task monitor library */
	TaskMonitorInitialize();

	/* Initialize UAVObject libraries */
	EventDispatcherInitialize();
	UAVObjInitialize();

	/* Initialize the alarms library. Reads RCC reset flags */
	AlarmsInitialize();
	PIOS_RESET_Clear(); // Clear the RCC reset flags after use.

	/* Initialize the hardware UAVOs */
	HwDiscoveryF4Initialize();
	ModuleSettingsInitialize();

#if defined(PIOS_INCLUDE_RTC)
	/* Initialize the real-time clock and its associated tick */
	PIOS_RTC_Init(&pios_rtc_main_cfg);
#endif

#ifndef ERASE_FLASH
	/* Initialize watchdog as early as possible to catch faults during init */
#ifndef DEBUG
	//PIOS_WDG_Init();
#endif
#endif

	/* Check for repeated boot failures */
	PIOS_IAP_Init();
	uint16_t boot_count = PIOS_IAP_ReadBootCount();
	if (boot_count < 3) {
		PIOS_IAP_WriteBootCount(++boot_count);
		AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
	} else {
		/* Too many failed boot attempts, force hw config to defaults */
		HwDiscoveryF4SetDefaults(HwDiscoveryF4Handle(), 0);
		ModuleSettingsSetDefaults(ModuleSettingsHandle(),0);
		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
	}

#if defined(PIOS_INCLUDE_USB)
	/* Initialize board specific USB data */
	PIOS_USB_BOARD_DATA_Init();

	/* Flags to determine if various USB interfaces are advertised */
	bool usb_hid_present = false;
	bool usb_cdc_present = false;

#if defined(PIOS_INCLUDE_USB_CDC)
	if (PIOS_USB_DESC_HID_CDC_Init()) {
		PIOS_Assert(0);
	}
	usb_hid_present = true;
	usb_cdc_present = true;
#else
	if (PIOS_USB_DESC_HID_ONLY_Init()) {
		PIOS_Assert(0);
	}
	usb_hid_present = true;
#endif

	uintptr_t pios_usb_id;
	PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);

#if defined(PIOS_INCLUDE_USB_CDC)

	uint8_t hw_usb_vcpport;
	/* Configure the USB VCP port */
//.........这里部分代码省略.........