自学教程：C++ AlarmsSet函数代码示例

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

//.........这里部分代码省略.........#endif	/* Initialize the alarms library */	AlarmsInitialize();	/* Initialize the task monitor library */	TaskMonitorInitialize();	/* Set up pulse timers */	PIOS_TIM_InitClock(&tim_3_cfg);	PIOS_TIM_InitClock(&tim_5_cfg);	PIOS_TIM_InitClock(&tim_8_cfg);	PIOS_TIM_InitClock(&tim_9_cfg);	PIOS_TIM_InitClock(&tim_12_cfg);	NVIC_InitTypeDef tim_8_up_irq = {		.NVIC_IRQChannel                   = TIM8_UP_TIM13_IRQn,		.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,		.NVIC_IRQChannelSubPriority        = 0,		.NVIC_IRQChannelCmd                = ENABLE,	};	NVIC_Init(&tim_8_up_irq);	/* IAP System Setup */	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 */		HwSparky2SetDefaults(HwSparky2Handle(), 0);		ModuleSettingsSetDefaults(ModuleSettingsHandle(),0);		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);	}	//PIOS_IAP_Init();#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_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));#if defined(PIOS_INCLUDE_USB_CDC)	uint8_t hw_usb_vcpport;

/** * Module thread, should not return. */static void groundPathFollowerTask(void *parameters){	SystemSettingsData systemSettings;	FlightStatusData flightStatus;	uint32_t lastUpdateTime;	GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb);	PathDesiredConnectCallback(SettingsUpdatedCb);	GroundPathFollowerSettingsGet(&guidanceSettings);	PathDesiredGet(&pathDesired);	// Main task loop	lastUpdateTime = PIOS_Thread_Systime();	while (1) {		// Conditions when this runs:		// 1. Must have GROUND 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_GROUNDVEHICLECAR) &&			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) &&			(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) )		{			AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);			PIOS_Thread_Sleep(1000);			continue;		}		// Continue collecting data if not enough time		PIOS_Thread_Sleep_Until(&lastUpdateTime, 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();					updateGroundDesiredAttitude();				} else {					AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);				}				break;			case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:				if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) {					updateEndpointVelocity();					updateGroundDesiredAttitude();				} 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();					updateGroundDesiredAttitude();				} else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR ||					pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT ||					pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) {					updatePathVelocity();					updateGroundDesiredAttitude();				} else {					AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR);				}				break;			default:				// Be cleaner and get rid of global variables				northVelIntegral = 0;				eastVelIntegral = 0;				northPosIntegral = 0;				eastPosIntegral = 0;				// Track throttle before engaging this mode.  Cheap system ident				StabilizationDesiredData stabDesired;				StabilizationDesiredGet(&stabDesired);				throttleOffset = stabDesired.Throttle;				break;		}		AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);	}}

/** * Update system alarms */static void updateSystemAlarms(){	SystemStatsData stats;	UAVObjStats objStats;	EventStats evStats;	SystemStatsGet(&stats);	// Check heap, IRQ stack and malloc failures	if (PIOS_heap_malloc_failed_p()	     || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL)#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL)#endif	    ) {		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);	} else if (		(stats.HeapRemaining < HEAP_LIMIT_WARNING)#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)	     || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING)#endif	    ) {		AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);	} else {		AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);	}	// 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) {		AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);	} else {		AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);	}	// Check for event errors	UAVObjGetStats(&objStats);	EventGetStats(&evStats);	UAVObjClearStats();	EventClearStats();	if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0  || evStats.eventErrors > 0) {		AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);	} else {		AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);	}		if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) {		SystemStatsData sysStats;		SystemStatsGet(&sysStats);		sysStats.EventSystemWarningID = evStats.lastErrorID;		sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID;		sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID;		SystemStatsSet(&sysStats);	}		}

/** * Set an alarm to it's default value * @param alarm The system alarm to be modified * @return 0 if success, -1 if an error */int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm){	return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);}

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)	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);	PIOS_Assert(led_cfg);	PIOS_LED_Init(led_cfg);#endif	/* PIOS_INCLUDE_LED */	/* Set up the SPI interface to the gyro/acelerometer */	if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {		PIOS_DEBUG_Assert(0);	}		/* Set up the SPI interface to the flash and rfm22b */	if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) {		PIOS_DEBUG_Assert(0);	}#if defined(PIOS_INCLUDE_FLASH)	/* Connect flash to the approrpiate interface and configure it */	uintptr_t flash_id;	if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0)		panic(1);	uintptr_t fs_id;	if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id) != 0)		panic(1);#endif	/* Initialize UAVObject libraries */	EventDispatcherInitialize();	UAVObjInitialize();		HwFreedomInitialize();	ModuleSettingsInitialize();#if defined(PIOS_INCLUDE_RTC)	PIOS_RTC_Init(&pios_rtc_main_cfg);#endif	/* Initialize the alarms library */	AlarmsInitialize();	/* Initialize the task monitor library */	TaskMonitorInitialize();	// /* Set up pulse timers */	PIOS_TIM_InitClock(&tim_1_cfg);	PIOS_TIM_InitClock(&tim_2_cfg);	PIOS_TIM_InitClock(&tim_3_cfg);	/* IAP System Setup */	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 */		HwFreedomSetDefaults(HwFreedomHandle(), 0);		ModuleSettingsSetDefaults(ModuleSettingsHandle(),0);		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);	}	PIOS_IAP_Init();#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	uint32_t pios_usb_id;	PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));#if defined(PIOS_INCLUDE_USB_CDC)	uint8_t hw_usb_vcpport;	/* Configure the USB VCP port */	HwFreedomUSB_VCPPortGet(&hw_usb_vcpport);//.........这里部分代码省略.........

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)	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);	PIOS_Assert(led_cfg);	PIOS_LED_Init(led_cfg);#endif	/* PIOS_INCLUDE_LED */#if defined(PIOS_INCLUDE_SPI)	/* Set up the SPI interface to the serial flash */	switch(bdinfo->board_rev) {		case BOARD_REVISION_CC:			if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) {				PIOS_Assert(0);			}			break;		case BOARD_REVISION_CC3D:			if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) {				PIOS_Assert(0);			}			break;		default:			PIOS_Assert(0);	}#endif	switch(bdinfo->board_rev) {		case BOARD_REVISION_CC:			PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 1, &flash_w25x_cfg);			break;		case BOARD_REVISION_CC3D:			PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 0, &flash_m25p_cfg);			break;		default:			PIOS_DEBUG_Assert(0);	}	PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg);	/* 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 */	PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS);	/* Initialize the task monitor library */	TaskMonitorInitialize();	/* Initialize UAVObject libraries */	EventDispatcherInitialize();	UAVObjInitialize();	/* Initialize the alarms library */	AlarmsInitialize();	HwCopterControlInitialize();	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 */	PIOS_WDG_Init();#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 configuration to defaults */		HwCopterControlSetDefaults(HwCopterControlHandle(), 0);		ModuleSettingsSetDefaults(ModuleSettingsHandle(),0);		AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);	}	/* Initialize the task monitor library */	TaskMonitorInitialize();	/* Set up pulse timers */	PIOS_TIM_InitClock(&tim_1_cfg);	PIOS_TIM_InitClock(&tim_2_cfg);	PIOS_TIM_InitClock(&tim_3_cfg);	PIOS_TIM_InitClock(&tim_4_cfg);//.........这里部分代码省略.........

/** * Module task */static void stabilizationTask(void* parameters){	portTickType lastSysTime;	portTickType thisSysTime;	UAVObjEvent ev;	ActuatorDesiredData actuatorDesired;	StabilizationDesiredData stabDesired;	RateDesiredData rateDesired;	AttitudeActualData attitudeActual;	AttitudeRawData attitudeRaw;	SystemSettingsData systemSettings;	FlightStatusData flightStatus;	SettingsUpdatedCb((UAVObjEvent *) NULL);	// Main task loop	lastSysTime = xTaskGetTickCount();	ZeroPids();	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);		StabilizationDesiredGet(&stabDesired);		AttitudeActualGet(&attitudeActual);		AttitudeRawGet(&attitudeRaw);		RateDesiredGet(&rateDesired);		SystemSettingsGet(&systemSettings);#if defined(PIOS_QUATERNION_STABILIZATION)		// Quaternion calculation of error in each axis.  Uses more memory.		float rpy_desired[3];		float q_desired[4];		float q_error[4];		float local_error[3];		// Essentially zero errors for anything in rate or none		if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)			rpy_desired[0] = stabDesired.Roll;		else			rpy_desired[0] = attitudeActual.Roll;		if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)			rpy_desired[1] = stabDesired.Pitch;		else			rpy_desired[1] = attitudeActual.Pitch;		if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)			rpy_desired[2] = stabDesired.Yaw;		else			rpy_desired[2] = attitudeActual.Yaw;		RPY2Quaternion(rpy_desired, q_desired);		quat_inverse(q_desired);		quat_mult(q_desired, &attitudeActual.q1, q_error);		quat_inverse(q_error);		Quaternion2RPY(q_error, local_error);#else		// Simpler algorithm for CC, less memory		float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,			stabDesired.Pitch - attitudeActual.Pitch,			stabDesired.Yaw - attitudeActual.Yaw};		local_error[2] = fmod(local_error[2] + 180, 360) - 180;#endif		for(uint8_t i = 0; i < MAX_AXES; i++) {			gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha);		}		float *attitudeDesiredAxis = &stabDesired.Roll;		float *actuatorDesiredAxis = &actuatorDesired.Roll;		float *rateDesiredAxis = &rateDesired.Roll;		//Calculate desired rate		for(uint8_t i=0; i< MAX_AXES; i++)		{			switch(stabDesired.StabilizationMode[i])			{				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:					rateDesiredAxis[i] = attitudeDesiredAxis[i];					axis_lock_accum[i] = 0;//.........这里部分代码省略.........

static void onTimer(UAVObjEvent* ev){	static FlightBatteryStateData flightBatteryData;	FlightBatterySettingsData batterySettings;	FlightBatterySettingsGet(&batterySettings);	static float dT = SAMPLE_PERIOD_MS / 1000.0f;	float energyRemaining;	//calculate the battery parameters	if (voltageADCPin >=0) {		flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts	}	else {		flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC	}	if (currentADCPin >=0) {		flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps		if (flightBatteryData.Current > flightBatteryData.PeakCurrent) 			flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps	}	else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm		flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC	}		flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh		//Apply a 2 second rise time low-pass filter to average the current	float alpha = 1.0f-dT/(dT+2.0f);	flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps	/*The motor could regenerate power. Or we could have solar cells. 	 In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then 	 it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple	 sign check doesn't catch this.*///	//sanity checks //	if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f;//	if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f;//	if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f;	energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh	if (flightBatteryData.AvgCurrent > 0)		flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec	else		flightBatteryData.EstimatedFlightTime = 9999;	//generate alarms where needed...	if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0))	{ 		//FIXME: There's no guarantee that a floating ADC will give 0. So this 		// check might fail, even when there's nothing attached.		AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);		AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);	}	else	{		// FIXME: should make the timer alarms user configurable		if (flightBatteryData.EstimatedFlightTime < 30) 			AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);		else if (flightBatteryData.EstimatedFlightTime < 120) 			AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);		else 			AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);		// FIXME: should make the battery voltage detection dependent on battery type. 		/*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/		if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM])			AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL);		else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING])			AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);		else 			AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);	}		FlightBatteryStateSet(&flightBatteryData);}

static void SensorsTask(void *parameters){	portTickType lastSysTime;	uint32_t accel_samples = 0;	uint32_t gyro_samples = 0;	int32_t accel_accum[3] = {0, 0, 0};	int32_t gyro_accum[3] = {0,0,0};	float gyro_scaling = 0;	float accel_scaling = 0;	static int32_t timeval;	AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);	UAVObjEvent ev;	settingsUpdatedCb(&ev);	const struct pios_board_info * bdinfo = &pios_board_info_blob;		switch(bdinfo->board_rev) {		case 0x01:#if defined(PIOS_INCLUDE_L3GD20)			gyro_test = PIOS_L3GD20_Test();#endif#if defined(PIOS_INCLUDE_BMA180)			accel_test = PIOS_BMA180_Test();#endif			break;		case 0x02:#if defined(PIOS_INCLUDE_MPU6000)			gyro_test = PIOS_MPU6000_Test();			accel_test = gyro_test;#endif			break;		default:			PIOS_DEBUG_Assert(0);	}#if defined(PIOS_INCLUDE_HMC5883)	mag_test = PIOS_HMC5883_Test();#else	mag_test = 0;#endif	if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {		AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);		while(1) {			PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);			vTaskDelay(10);		}	}		// Main task loop	lastSysTime = xTaskGetTickCount();	bool error = false;	uint32_t mag_update_time = PIOS_DELAY_GetRaw();	while (1) {		// TODO: add timeouts to the sensor reads and set an error if the fail		sensor_dt_us = PIOS_DELAY_DiffuS(timeval);		timeval = PIOS_DELAY_GetRaw();		if (error) {			PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);			lastSysTime = xTaskGetTickCount();			vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);			AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);			error = false;		} else {			AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);		}		for (int i = 0; i < 3; i++) {			accel_accum[i] = 0;			gyro_accum[i] = 0;		}		accel_samples = 0;		gyro_samples = 0;		AccelsData accelsData;		GyrosData gyrosData;		switch(bdinfo->board_rev) {			case 0x01:  // L3GD20 + BMA180 board#if defined(PIOS_INCLUDE_BMA180)			{				struct pios_bma180_data accel;								int32_t read_good;				int32_t count;								count = 0;				while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error)					error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;				if (error) {					// Unfortunately if the BMA180 ever misses getting read, then it will not					// trigger more interrupts.  In this case we must force a read to kickstarts					// it.					struct pios_bma180_data data;					PIOS_BMA180_ReadAccels(&data);					continue;//.........这里部分代码省略.........

/** * Clear an alarm * @param alarm The system alarm to be modified * @return 0 if success, -1 if an error */int32_t AlarmsClear(SystemAlarmsAlarmElem alarm){	return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);}

/** * Module thread, should not return. */static void guidanceTask(void *parameters){	SystemSettingsData systemSettings;	GuidanceSettingsData guidanceSettings;	ManualControlCommandData manualControl;	portTickType thisTime;	portTickType lastUpdateTime;	UAVObjEvent ev;		float accel[3] = {0,0,0};	uint32_t accel_accum = 0;		float q[4];	float Rbe[3][3];	float accel_ned[3];		// Main task loop	lastUpdateTime = xTaskGetTickCount();	while (1) {		GuidanceSettingsGet(&guidanceSettings);		// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe		if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE )		{			AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);		} else {			AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);		}						// Collect downsampled attitude data		AttitudeRawData attitudeRaw;		AttitudeRawGet(&attitudeRaw);				accel[0] += attitudeRaw.accels[0];		accel[1] += attitudeRaw.accels[1];		accel[2] += attitudeRaw.accels[2];		accel_accum++;				// Continue collecting data if not enough time		thisTime = xTaskGetTickCount();		if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) )			continue;				lastUpdateTime = xTaskGetTickCount();		accel[0] /= accel_accum;		accel[1] /= accel_accum;		accel[2] /= accel_accum;				//rotate avg accels into earth frame and store it		AttitudeActualData attitudeActual;		AttitudeActualGet(&attitudeActual);		q[0]=attitudeActual.q1;		q[1]=attitudeActual.q2;		q[2]=attitudeActual.q3;		q[3]=attitudeActual.q4;		Quaternion2R(q, Rbe);		for (uint8_t i=0; i<3; i++){			accel_ned[i]=0;			for (uint8_t j=0; j<3; j++)				accel_ned[i] += Rbe[j][i]*accel[j];		}		accel_ned[2] += 9.81;				NedAccelData accelData;		NedAccelGet(&accelData);		// Convert from m/s to cm/s		accelData.North = accel_ned[0] * 100;		accelData.East = accel_ned[1] * 100;		accelData.Down = accel_ned[2] * 100;		NedAccelSet(&accelData);						ManualControlCommandGet(&manualControl);		SystemSettingsGet(&systemSettings);		GuidanceSettingsGet(&guidanceSettings);				if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) &&		    ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) ||		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) ||		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) ||		     (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ))		{			if(positionHoldLast == 0) {				/* When enter position hold mode save current position */				PositionDesiredData positionDesired;				PositionActualData positionActual;				PositionDesiredGet(&positionDesired);				PositionActualGet(&positionActual);				positionDesired.North = positionActual.North;				positionDesired.East = positionActual.East;				PositionDesiredSet(&positionDesired);				positionHoldLast = 1;			}						if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) 				updateVtolDesiredVelocity();			else//.........这里部分代码省略.........