示例#1
0
/**
 * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 */
static void systemTask(void *parameters)
{
	/* create all modules thread */
	MODULE_TASKCREATE_ALL;

	if (PIOS_heap_malloc_failed_p()) {
		/* We failed to malloc during task creation,
		 * system behaviour is undefined.  Reset and let
		 * the BootFault code recover for us.
		 */
		PIOS_SYS_Reset();
	}

#if defined(PIOS_INCLUDE_IAP)
	/* Record a successful boot */
	PIOS_IAP_WriteBootCount(0);
#endif

	// Initialize vars
	idleCounter = 0;
	idleCounterClear = 0;

	// Listen for SettingPersistance object updates, connect a callback function
	ObjectPersistenceConnectQueue(objectPersistenceQueue);

#if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
	// Run this initially to make sure the configuration is checked
	configuration_check();

	// Whenever the configuration changes, make sure it is safe to fly
	if (StabilizationSettingsHandle())
		StabilizationSettingsConnectCallback(configurationUpdatedCb);
	if (SystemSettingsHandle())
		SystemSettingsConnectCallback(configurationUpdatedCb);
	if (ManualControlSettingsHandle())
		ManualControlSettingsConnectCallback(configurationUpdatedCb);
	if (FlightStatusHandle())
		FlightStatusConnectCallback(configurationUpdatedCb);
#endif
#if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
	if (StateEstimationHandle())
		StateEstimationConnectCallback(configurationUpdatedCb);
#endif

	// Main system loop
	while (1) {
		// Update the system statistics
		updateStats();

		// Update the system alarms
		updateSystemAlarms();
#if defined(WDG_STATS_DIAGNOSTICS)
		updateWDGstats();
#endif

#if defined(DIAG_TASKS)
		// Update the task status object
		TaskMonitorUpdateAll();
#endif

		// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
		DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif	/* PIOS_LED_HEARTBEAT */

		// Turn on the error LED if an alarm is set
		if (indicateError()) {
#if defined (PIOS_LED_ALARM)
			PIOS_LED_On(PIOS_LED_ALARM);
#endif	/* PIOS_LED_ALARM */
		} else {
#if defined (PIOS_LED_ALARM)
			PIOS_LED_Off(PIOS_LED_ALARM);
#endif	/* PIOS_LED_ALARM */
		}

		FlightStatusData flightStatus;
		FlightStatusGet(&flightStatus);

		UAVObjEvent ev;
		int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
			SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) :
			SYSTEM_UPDATE_PERIOD_MS;

		if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) {
			// If object persistence is updated call the callback
			objectUpdatedCb(&ev);
		}
	}
}
示例#2
0
/**
 * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
 */
static void systemTask(void *parameters)
{
	/* create all modules thread */
	MODULE_TASKCREATE_ALL;

	if (mallocFailed) {
		/* We failed to malloc during task creation,
		 * system behaviour is undefined.  Reset and let
		 * the BootFault code recover for us.
		 */
		PIOS_SYS_Reset();
	}

#if defined(PIOS_INCLUDE_IAP)
	/* Record a successful boot */
	PIOS_IAP_WriteBootCount(0);
#endif

	// Initialize vars
	idleCounter = 0;
	idleCounterClear = 0;

	// Listen for SettingPersistance object updates, connect a callback function
	ObjectPersistenceConnectQueue(objectPersistenceQueue);

	// Main system loop
	while (1) {
		// Update the system statistics
		updateStats();

		// Update the system alarms
		updateSystemAlarms();
#if defined(I2C_WDG_STATS_DIAGNOSTICS)
		updateI2Cstats();
		updateWDGstats();
#endif

#if defined(DIAG_TASKS)
		// Update the task status object
		TaskMonitorUpdateAll();
#endif

		// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
		PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif	/* PIOS_LED_HEARTBEAT */

		// Turn on the error LED if an alarm is set
#if defined (PIOS_LED_ALARM)
		if (AlarmsHasWarnings()) {
			PIOS_LED_On(PIOS_LED_ALARM);
		} else {
			PIOS_LED_Off(PIOS_LED_ALARM);
		}
#endif	/* PIOS_LED_ALARM */

		FlightStatusData flightStatus;
		FlightStatusGet(&flightStatus);

		UAVObjEvent ev;
		int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
			SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) :
			SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS;

		if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) {
			// If object persistence is updated call the callback
			objectUpdatedCb(&ev);
		}
	}
}