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

/** * 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 AttitudeTask(void *parameters){	uint8_t init = 0;	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);	PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);	// Keep flash CS pin high while talking accel	PIOS_FLASH_DISABLE;			PIOS_ADXL345_Init();				// Main task loop	while (1) {				if(xTaskGetTickCount() < 10000) {			// For first 5 seconds use accels to get gyro bias			accelKp = 1;			// Decrease the rate of gyro learning during init			accelKi = .5 / (1 + xTaskGetTickCount() / 5000);		} else if (init == 0) {			settingsUpdatedCb(AttitudeSettingsHandle());			init = 1;		}											PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);				AttitudeRawData attitudeRaw;		AttitudeRawGet(&attitudeRaw);				updateSensors(&attitudeRaw);				updateAttitude(&attitudeRaw);		AttitudeRawSet(&attitudeRaw); 		}}

/** * Module starting */int32_t ManualControlStart(){    // Run this initially to make sure the configuration is checked    configuration_check();    // Whenever the configuration changes, make sure it is safe to fly    SystemSettingsConnectCallback(configurationUpdatedCb);    ManualControlSettingsConnectCallback(configurationUpdatedCb);    ManualControlCommandConnectCallback(commandUpdatedCb);    // clear alarms    AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);    SettingsUpdatedCb(NULL);    // Make sure unarmed on power up    armHandler(true, frameType);#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES    takeOffLocationHandlerInit();#endif    // Start main task    PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);    return 0;}

/** * Clear all alarms */void AlarmsClearAll(){	uint32_t n;    for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)    {    	AlarmsClear(n);    }}

/** * 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);	}}

/** * Module thread, should not return. */static void AttitudeTask(void *parameters){	uint8_t init = 0;	AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);	PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE);	// Keep flash CS pin high while talking accel	PIOS_FLASH_DISABLE;	PIOS_ADXL345_Init();	// Force settings update to make sure rotation loaded	settingsUpdatedCb(AttitudeSettingsHandle());	// Main task loop	while (1) {		FlightStatusData flightStatus;		FlightStatusGet(&flightStatus);		if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {			// For first 7 seconds use accels to get gyro bias			accelKp = 1;			accelKi = 0.9;			yawBiasRate = 0.23;			init = 0;		}		else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {			accelKp = 1;			accelKi = 0.9;			yawBiasRate = 0.23;			init = 0;		} else if (init == 0) {			// Reload settings (all the rates)			AttitudeSettingsAccelKiGet(&accelKi);			AttitudeSettingsAccelKpGet(&accelKp);			AttitudeSettingsYawBiasRateGet(&yawBiasRate);			init = 1;		}		PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);		AttitudeRawData attitudeRaw;		AttitudeRawGet(&attitudeRaw);		updateSensors(&attitudeRaw);		updateAttitude(&attitudeRaw);		AttitudeRawSet(&attitudeRaw);	}}

/** * 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);	}}

/** * 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 );    }}

/** * @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 );	}}

/* 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);		}	}}

/** * 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);	}}

//.........这里部分代码省略.........	 * but do it only if there is no debugger connected	 */	if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) {		PIOS_WDG_Init();	}#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;

/** * 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);	}		}

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

//.........这里部分代码省略.........				// Implement hysteresis loop on connection status		// Must check both Max and Min in case they reversed		if (!ManualControlCommandReadOnly(&cmd) &&			cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&			cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {			if (disconnected_count++ > 10) {				connection_state = DISCONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				disconnected_count++;		} else {			if (connected_count++ > 10) {				connection_state = CONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				connected_count++;		}		if (connection_state == DISCONNECTED) {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;			cmd.Throttle = -1;	// Shut down engine with no control			cmd.Roll = 0;			cmd.Yaw = 0;			cmd.Pitch = 0;			//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);			ManualControlCommandSet(&cmd);		} else {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);			ManualControlCommandSet(&cmd);		} 		// Arming and Disarming mechanism		if (cmd.Throttle < 0) {			// Throttle is low, in this condition the arming state could change			uint8_t newCmdArmed = cmd.Armed;			static portTickType armedDisarmStart;			// Look for state changes and write in newArmState			if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) {				// No channel assigned to arming -> arm immediately when throttle is low				newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;			} else {				float armStickLevel;				uint8_t channel = settings.Arming/2;    // 0=Channel1, 1=Channel1_Rev, 2=Channel2, ....				bool reverse = (settings.Arming%2)==1;				bool manualArm = false;				bool manualDisarm = false;				if (connection_state == CONNECTED) {					// Should use RC input only if RX is connected					armStickLevel = scaledChannel[channel];					if (reverse)						armStickLevel =-armStickLevel;					if (armStickLevel <= -0.90)						manualArm = true;					else if (armStickLevel >= +0.90)						manualDisarm = true;				}

//.........这里部分代码省略.........						weak_leveling = -weak_leveling_max;					rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;					axis_lock_accum[i] = 0;					break;				}				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:					rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]);					axis_lock_accum[i] = 0;					break;				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:					if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {						// While getting strong commands act like rate mode						rateDesiredAxis[i] = attitudeDesiredAxis[i];						axis_lock_accum[i] = 0;					} else {						// For weaker commands or no command simply attitude lock (almost) on no gyro change						axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;						if(axis_lock_accum[i] > max_axis_lock)							axis_lock_accum[i] = max_axis_lock;						else if(axis_lock_accum[i] < -max_axis_lock)							axis_lock_accum[i] = -max_axis_lock;						rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]);					}					break;			}		}		uint8_t shouldUpdate = 1;		RateDesiredSet(&rateDesired);		ActuatorDesiredGet(&actuatorDesired);		//Calculate desired command		for(int8_t ct=0; ct< MAX_AXES; ct++)		{			if(rateDesiredAxis[ct] > settings.MaximumRate[ct])				rateDesiredAxis[ct] = settings.MaximumRate[ct];			else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct])				rateDesiredAxis[ct] = -settings.MaximumRate[ct];			switch(stabDesired.StabilizationMode[ct])			{				case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:				case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:				case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:				case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:				{					float command = ApplyPid(&pids[PID_RATE_ROLL + ct],  rateDesiredAxis[ct] - gyro_filtered[ct]);					actuatorDesiredAxis[ct] = bound(command);					break;				}				case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:					switch (ct)				{					case ROLL:						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);						shouldUpdate = 1;						break;					case PITCH:						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);						shouldUpdate = 1;						break;					case YAW:						actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]);						shouldUpdate = 1;						break;				}					break;			}		}		// Save dT		actuatorDesired.UpdateTime = dT * 1000;		if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)			shouldUpdate = 0;		if(shouldUpdate)		{			actuatorDesired.Throttle = stabDesired.Throttle;			if(dT > 15)				actuatorDesired.NumLongUpdates++;			ActuatorDesiredSet(&actuatorDesired);		}		if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||		   (lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||		   !shouldUpdate)		{			ZeroPids();		}		// Clear alarms		AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);	}}

static void onTimer(UAVObjEvent* ev){	static portTickType lastSysTime;	static bool firstRun = true;	static FlightBatteryStateData flightBatteryData;	if (firstRun) {		#ifdef ENABLE_DEBUG_MSG			PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);		#endif		lastSysTime = xTaskGetTickCount();		//FlightBatteryStateGet(&flightBatteryData);		firstRun = false;	}	AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);	portTickType thisSysTime;	FlightBatterySettingsData batterySettings;	static float dT = SAMPLE_PERIOD_MS / 1000;	float Bob;	float energyRemaining;	// Check how long since last update	thisSysTime = xTaskGetTickCount();	if(thisSysTime > lastSysTime) // reuse dt in case of wraparound		dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f);	//lastSysTime = thisSysTime;	FlightBatterySettingsGet(&batterySettings);	//calculate the battery parameters	flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts	flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in AmpsBob =dT; // FIXME: something funky happens if I don't do this... Andrew	flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh	if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps	flightBatteryData.AvgCurrent=(flightBatteryData.AvgCurrent*0.8)+(flightBatteryData.Current*0.2); //in Amps	//sanity checks	if (flightBatteryData.AvgCurrent<0)flightBatteryData.AvgCurrent=0.0;	if (flightBatteryData.PeakCurrent<0)flightBatteryData.PeakCurrent=0.0;	if (flightBatteryData.ConsumedEnergy<0)flightBatteryData.ConsumedEnergy=0.0;	energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh	flightBatteryData.EstimatedFlightTime = ((energyRemaining / (flightBatteryData.AvgCurrent*1000.0))*3600.0);//in Sec	//generate alarms where needed...	if ((flightBatteryData.Voltage<=0)&&(flightBatteryData.Current<=0))	{		AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);		AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR);	}	else	{		if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL);		else if (flightBatteryData.EstimatedFlightTime < 60) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING);		else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME);		// FIXME: should make the battery voltage detection dependent on battery type.		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);	}	lastSysTime = thisSysTime;	FlightBatteryStateSet(&flightBatteryData);}

/** * Main task. It does not return. */static void batteryTask(void * parameters){	const float dT = SAMPLE_PERIOD_MS / 1000.0f;	settingsUpdatedCb(NULL);	// Main task loop	portTickType lastSysTime;	lastSysTime = xTaskGetTickCount();	while (true) {		vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLE_PERIOD_MS));		FlightBatteryStateData flightBatteryData;		FlightBatterySettingsData batterySettings;		float energyRemaining;		if (battery_settings_updated) {			battery_settings_updated = false;			FlightBatterySettingsGet(&batterySettings);			voltageADCPin = batterySettings.VoltagePin;			if (voltageADCPin == FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE)				voltageADCPin = -1;			currentADCPin = batterySettings.CurrentPin;			if (currentADCPin == FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE)				currentADCPin = -1;		}		//calculate the battery parameters		if (voltageADCPin >= 0) {			flightBatteryData.Voltage = ((float) PIOS_ADC_GetChannelVolt(voltageADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_VOLTAGE] * 1000.0f +							batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_VOLTAGE]; //in Volts		} else {			flightBatteryData.Voltage = 0; //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_GetChannelVolt(currentADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_CURRENT] * 1000.0f +							batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_CURRENT]; //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 = -1; //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		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);	}}

//.........这里部分代码省略.........					gps_rx_buffer[rx_count-2] = 0;					// prepare to parse next sentence					start_flag = false;					found_cr = false;					rx_count = 0;					// Our rxBuffer must look like this now:					//   [0]           = '$'					//   ...           = zero or more bytes of sentence payload					//   [end_pos - 1] = '/r'					//   [end_pos]     = '/n'					//					// Prepare to consume the sentence from the buffer									// Validate the checksum over the sentence					if (!NMEA_checksum(&gps_rx_buffer[1]))					{	// Invalid checksum.  May indicate dropped characters on Rx.						PIOS_DEBUG_PinHigh(2);						++numChecksumErrors;						PIOS_DEBUG_PinLow(2);					}					else					{	// Valid checksum, use this packet to update the GPS position						if (!NMEA_update_position(&gps_rx_buffer[1])) {							PIOS_DEBUG_PinHigh(2);							++numParsingErrors;							PIOS_DEBUG_PinLow(2);						}						else							++numUpdates;						timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;						timeOfLastUpdateMs = timeNowMs;						timeOfLastCommandMs = timeNowMs;					}				}			}		#endif		// Check for GPS timeout		timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;		if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)		{	// we have not received any valid GPS sentences for a while.			// either the GPS is not plugged in or a hardware problem or the GPS has locked up.			GPSPositionGet(&GpsData);			GpsData.Status = GPSPOSITION_STATUS_NOGPS;			GPSPositionSet(&GpsData);			AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);			if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS)			{	// resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command				timeOfLastCommandMs = timeNowMs;				#ifdef ENABLE_GPS_BINARY_GTOP					GTOP_BIN_init();					// switch to binary mode					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F/r/n");				#endif				#ifdef ENABLE_GPS_ONESENTENCE_GTOP					// switch to single sentence mode					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C/r/n");				#endif				#ifdef ENABLE_GPS_NMEA					// switch to NMEA mode					PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D/r/n");				#endif				#ifdef DISABLE_GPS_TRESHOLD					PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23/r/n");				#endif			}		}		else		{	// we appear to be receiving GPS sentences OK, we've had an update			HomeLocationData home;			HomeLocationGet(&home);			GPSPositionGet(&GpsData);			if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))				setHomeLocation(&GpsData);			//criteria for GPS-OK taken from this post...			//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220			if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))				AlarmsClear(SYSTEMALARMS_ALARM_GPS);			else			if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);			else				AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);		}		// Block task until next update		vTaskDelay(xDelay);	}}

//.........这里部分代码省略.........					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;			} else {                                     // Position 2				for(int i = 0; i < 3; i++) {					if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE)						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;					else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE)						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;					else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE)						cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;				}				if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL)					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;				else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED)					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;				else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO)					cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;			}			// Update the ManualControlCommand object			ManualControlCommandSet(&cmd);			// This seems silly to set then get, but the reason is if the GCS is			// the control input, the set command will be blocked by the read only			// setting and the get command will pull the right values from telemetry		} else			ManualControlCommandGet(&cmd);	/* Under GCS control */		// Implement hysteresis loop on connection status		// Must check both Max and Min in case they reversed		if (!ManualControlCommandReadOnly(&cmd) &&		    cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&		    cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {			if (disconnected_count++ > 10) {				connection_state = DISCONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				disconnected_count++;		} else {			if (connected_count++ > 10) {				connection_state = CONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				connected_count++;		}		if (connection_state == DISCONNECTED) {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;			cmd.Throttle = -1;	// Shut down engine with no control			cmd.Roll = 0;			cmd.Yaw = 0;			cmd.Pitch = 0;			//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);			ManualControlCommandSet(&cmd);		} else {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);			ManualControlCommandSet(&cmd);		}		/* Look for arm or disarm signal */		if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) {			if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart))	// store when started, deal with rollover				armedDisarmStart = lastSysTime;			else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))				cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;		} else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) {			if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart))	// store when started, deal with rollover				armedDisarmStart = lastSysTime;			else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))				cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;		} else {			armedDisarmStart = 0;		}		// Depending on the mode update the Stabilization or Actuator objects		if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) {			actuator.Roll = cmd.Roll;			actuator.Pitch = cmd.Pitch;			actuator.Yaw = cmd.Yaw;			actuator.Throttle = cmd.Throttle;			ActuatorDesiredSet(&actuator);		} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {			attitude.Roll = cmd.Roll * stabSettings.RollMax;			attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;			attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);			attitude.Throttle =  (cmd.Throttle < 0) ? -1 : cmd.Throttle;			AttitudeDesiredSet(&attitude);		}		if (cmd.Accessory3 < -.5) {	//TODO: Make what happens here depend on GCS			AHRSSettingsData attitudeSettings;			AHRSSettingsGet(&attitudeSettings);			// Hard coding a maximum bias of 15 for now... maybe mistake			attitudeSettings.PitchBias = cmd.Accessory1 * 15;			attitudeSettings.RollBias = cmd.Accessory2 * 15;			AHRSSettingsSet(&attitudeSettings);		}	}}

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

//.........这里部分代码省略.........		// 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;				pathDesired.End[1] = positionActual.East;				pathDesired.End[2] = positionActual.Down;				pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;				pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;				pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;				PathDesiredSet(&pathDesired);				break;			case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:			case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:				state = FW_FOLLOWER_RUNNING;				PathDesiredGet(&pathDesired);				switch(pathDesired.Mode) {					case PATHDESIRED_MODE_ENDPOINT:					case PATHDESIRED_MODE_VECTOR:					case PATHDESIRED_MODE_CIRCLERIGHT:					case PATHDESIRED_MODE_CIRCLELEFT:						break;					default:						state = FW_FOLLOWER_ERR;						pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;						PathStatusSet(&pathStatus);						AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);						break;				}				break;			default:				state = FW_FOLLOWER_IDLE;				break;			}		}		switch(state) {		case FW_FOLLOWER_RUNNING:		{			updatePathVelocity();			uint8_t result = updateFixedDesiredAttitude();			if (result) {				AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);			} else {				AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);			}			PathStatusSet(&pathStatus);			break;		}		case FW_FOLLOWER_IDLE:			// Be cleaner and get rid of global variables			northVelIntegral = 0;			eastVelIntegral = 0;			downVelIntegral = 0;			bearingIntegral = 0;			speedIntegral = 0;			accelIntegral = 0;			powerIntegral = 0;			airspeedErrorInt = 0;			AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);			break;		case FW_FOLLOWER_ERR:		default:			// Leave alarms set above			break;		}	}}

//.........这里部分代码省略.........		// Implement hysteresis loop on connection status		// Must check both Max and Min in case they reversed		if (!ManualControlCommandReadOnly(&cmd) &&			cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&			cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {			if (disconnected_count++ > 10) {				connection_state = DISCONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				disconnected_count++;		} else {			if (connected_count++ > 10) {				connection_state = CONNECTED;				connected_count = 0;				disconnected_count = 0;			} else				connected_count++;		}		if (connection_state == DISCONNECTED) {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;			cmd.Throttle = -1;	// Shut down engine with no control			cmd.Roll = 0;			cmd.Yaw = 0;			cmd.Pitch = 0;			//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning			AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);			ManualControlCommandSet(&cmd);		} else {			cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;			AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);			ManualControlCommandSet(&cmd);		}		//		// Arming and Disarming mechanism		//		// Look for state changes and write in newArmState		uint8_t newCmdArmed = cmd.Armed;	// By default, keep the arming state the same		if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {			// In this configuration we always disarm			newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;		} else {			// In all other cases, we will not change the arm state when disconnected			if (connection_state == CONNECTED)			{				if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {					// In this configuration, we go into armed state as soon as the throttle is low, never disarm					if (cmd.Throttle < 0) {						newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;					}				} else {					// When the configuration is not "Always armed" and no "Always disarmed",					// the state will not be changed when the throttle is not low					if (cmd.Throttle < 0) {						static portTickType armedDisarmStart;						float armingInputLevel = 0;						// Calc channel see assumptions7						switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {						case ARMING_CHANNEL_ROLL: 	armingInputLevel = cmd.Roll; 	break;						case ARMING_CHANNEL_PITCH: 	armingInputLevel = cmd.Pitch; 	break;

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

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);}

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

//.........这里部分代码省略.........        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:            if (multirotor) {                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:            severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:            if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            }            // TODO: put check equivalent to TASK_MONITOR_IsRunning            // here as soon as available for delayed callbacks            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {                // Revo supports Position Hold                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {                // Revo supports AutoLand Mode                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {                // Revo supports POI Mode                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else {                // Revo supports PathPlanner and that must be OK or we are not sane                // PathPlan alarm is uninitialized if not running                // PathPlan alarm is warning or error if the flightplan is invalid                SystemAlarmsAlarmData alarms;                SystemAlarmsAlarmGet(&alarms);                if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {                    severity = SYSTEMALARMS_ALARM_ERROR;                }            }            break;        case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:            if (coptercontrol) {                severity = SYSTEMALARMS_ALARM_ERROR;            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {                // Revo supports ReturnToBase                severity = SYSTEMALARMS_ALARM_ERROR;            }            break;        default:            // Uncovered modes are automatically an error            severity = SYSTEMALARMS_ALARM_ERROR;        }        // mark the first encountered erroneous setting in status and substatus        if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {            alarmstatus    = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;            alarmsubstatus = i;        }    }    // TODO: Check on a multirotor no axis supports "None"    if (severity != SYSTEMALARMS_ALARM_OK) {        ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);    } else {        AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);    }    return 0;}

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