Flight/System Restructure start-up code, now all initialization in done after the RTOS is started

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@541 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/OpenPilot/Modules/Example/examplemodperiodic.c

@@ -49,7 +49,7 @@
 #include "examplesettings.h" // object holding module settings
 
 // Private constants
-#define STACK_SIZE 200
+#define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
 
 // Private types
@@ -80,8 +80,10 @@ static void exampleTask(void* parameters)
 	ExampleSettingsData settings;
 	ExampleObject2Data data;
 	int32_t step;
+	portTickType lastSysTime;
 
 	// Main task loop
+	lastSysTime = xTaskGetTickCount();
 	while (1)
 	{
 		// Update settings with latest value
@@ -124,6 +126,6 @@ static void exampleTask(void* parameters)
 		// block the task until it is time for the next update.
 		// The settings field is in ms, to convert to RTOS ticks we need
 		// to divide by portTICK_RATE_MS.
-		vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
+		vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
 	}
 }

flight/OpenPilot/Modules/Example/examplemodthread.c

@@ -50,7 +50,7 @@
 
 // Private constants
 #define MAX_QUEUE_SIZE 20
-#define STACK_SIZE 200
+#define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
 
 // Private types

flight/OpenPilot/Modules/System/systemmod.c

@@ -29,21 +29,24 @@
 #include "systemstats.h"
 
 // Private constants
-#define STATS_UPDATE_PERIOD_MS 1000
+#define SYSTEM_UPDATE_PERIOD_MS 1000
 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
 											  // must be updated if the FreeRTOS or compiler
 											  // optimisation options are changed.
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
 
 // Private types
 
 // Private variables
 static uint32_t idleCounter;
 static uint32_t idleCounterClear;
-static xSemaphoreHandle mutex;
+static xTaskHandle systemTaskHandle;
 
 // Private functions
-static void ObjectUpdatedCb(UAVObjEvent* ev);
+static void objectUpdatedCb(UAVObjEvent* ev);
-static void StatsUpdateCb(UAVObjEvent* ev);
+static void updateStats();
+static void systemTask(void* parameters);
 
 /**
  * Initialise the module, called on startup.
@@ -51,27 +54,47 @@ static void StatsUpdateCb(UAVObjEvent* ev);
  */
 int32_t SystemModInitialize(void)
 {
-	UAVObjEvent ev;
+	// Create system task
+	xTaskCreate(systemTask, (signed char*)"System", STACK_SIZE, NULL, TASK_PRIORITY, &systemTaskHandle);
+	return 0;
+}
 
-	// Create the mutex
+/**
-	mutex = xSemaphoreCreateRecursiveMutex();
+ * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
+ */
+static void systemTask(void* parameters)
+{
+	portTickType lastSysTime;
 
-	// Listen for ExampleObject1 updates, connect a callback function
+	// System initialization
-	SettingsPersistenceConnectCallback(&ObjectUpdatedCb);
+	OpenPilotInit();
 
-	// Create periodic event that will be used to update the system stats
+	// Initialize vars
 	idleCounter = 0;
 	idleCounterClear = 0;
-	memset(&ev, 0, sizeof(UAVObjEvent));
+	lastSysTime = xTaskGetTickCount();
-	EventPeriodicCallbackCreate(&ev, StatsUpdateCb, STATS_UPDATE_PERIOD_MS);
 
-	return 0;
+	// Listen for SettingPersistance object updates, connect a callback function
+	SettingsPersistenceConnectCallback(&objectUpdatedCb);
+
+	// Main system loop
+	while (1)
+	{
+		// Update the system statistics
+		updateStats();
+
+		// Flash the LED
+		PIOS_LED_Toggle(LED1);
+
+		// Wait until next period
+		vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
+	}
 }
 
 /**
  * Function called in response to object updates
  */
-static void ObjectUpdatedCb(UAVObjEvent* ev)
+static void objectUpdatedCb(UAVObjEvent* ev)
 {
 	SettingsPersistenceData setper;
 
@@ -96,7 +119,7 @@ static void ObjectUpdatedCb(UAVObjEvent* ev)
 /**
  * Called periodically to update the system stats
  */
-static void StatsUpdateCb(UAVObjEvent* ev)
+static void updateStats()
 {
 	SystemStatsData stats;
 
@@ -104,7 +127,7 @@ static void StatsUpdateCb(UAVObjEvent* ev)
 	SystemStatsGet(&stats);
 	stats.FlightTime = xTaskGetTickCount()*portTICK_RATE_MS;
 	stats.HeapRemaining = xPortGetFreeHeapSize();
-	stats.CPULoad = 100 - (uint8_t)round(100.0*((float)idleCounter/(float)(STATS_UPDATE_PERIOD_MS/1000))/(float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
+	stats.CPULoad = 100 - (uint8_t)round(100.0*((float)idleCounter/(float)(SYSTEM_UPDATE_PERIOD_MS/1000))/(float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
 	idleCounterClear = 1;
 	SystemStatsSet(&stats);
 }

flight/OpenPilot/Modules/Telemetry/telemetry.c

@@ -312,8 +312,7 @@ static void telemetryRxTask(void* parameters)
 		//}
 
 		// Block until data are available
-		// TODO: Update once the PIOS_COM is made blocking
+		// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
-		//xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
 		len = PIOS_COM_ReceiveBufferUsed(inputPort);
 		for (int32_t n = 0; n < len; ++n)
 		{

flight/OpenPilot/System/inc/openpilot.h

@@ -44,6 +44,6 @@
 #include "uavtalk.h"
 
 /* Global Functions */
-extern void OpenPilotInit(void);
+void OpenPilotInit(void);
 
 #endif /* OPENPILOT_H */

flight/OpenPilot/System/openpilot.c

@@ -58,9 +58,37 @@ void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
 */
 int main()
 {
+	/* NOTE: Do NOT modify the following start-up sequence */
+	/* Any new initialization functions should be added in */
+	/* OpenPilotInit() */
+
 	/* Brings up System using CMSIS functions, enables the LEDs. */
 	PIOS_SYS_Init();
 
+	/* Initialize the system thread */
+	SystemModInitialize();
+
+	/* Start the FreeRTOS scheduler */
+	vTaskStartScheduler();
+
+	/* If all is well we will never reach here as the scheduler will now be running. */
+	/* If we do get here, it will most likely be because we ran out of heap space. */
+	PIOS_LED_Off(LED1);
+	PIOS_LED_Off(LED2);
+	for(;;) {
+		PIOS_LED_Toggle(LED1);
+		PIOS_LED_Toggle(LED2);
+		PIOS_DELAY_WaitmS(100);
+	}
+
+	return 0;
+}
+
+/**
+ * Initialize the hardware, libraries and modules (called by the System thread in systemmod.c)
+ */
+void OpenPilotInit()
+{
 	/* Delay system */
 	PIOS_DELAY_Init();
 
@@ -74,7 +102,7 @@ int main()
 	for(;;)
 	{
 		/* Check if we have an SD Card with the correct settings files on it */
-		if(!PIOS_SDCARD_MountFS(STARTUP_LOG_ENABLED)) {
+		if(!PIOS_SDCARD_MountFS(0)) {
 			/* Found one without errors */
 			break;
 		}
@@ -115,47 +143,16 @@ int main()
 	PIOS_Servo_SetHz(50, 450);
 
 	/* Create a FreeRTOS task */
-	xTaskCreate(TaskTick, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
 	//xTaskCreate(TaskTesting, (signed portCHAR *)"TaskTesting", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
 	//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
 	//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
 
 	/* Initialize modules */
-	SystemModInitialize();
 	TelemetryInitialize();
-	ExampleModPeriodicInitialize();
+	//ExampleModPeriodicInitialize();
 	//ExampleModThreadInitialize();
-	//ExampleModEventInitialize();
+	ExampleModEventInitialize();
 	GpsInitialize();
-
-	/* Start the FreeRTOS scheduler */
-	vTaskStartScheduler();
-
-	/* If all is well we will never reach here as the scheduler will now be running. */
-	/* If we do get here, it will most likely be because we ran out of heap space. */
-	PIOS_LED_Off(LED1);
-	PIOS_LED_Off(LED2);
-	for(;;) {
-		PIOS_LED_Toggle(LED1);
-		PIOS_LED_Toggle(LED2);
-		PIOS_DELAY_WaitmS(100);
-	}
-
-	return 0;
-}
-
-static void TaskTick(void *pvParameters)
-{
-	portTickType xLastExecutionTime;
-
-	/* Setup the LEDs to Alternate */
-	PIOS_LED_On(LED1);
-
-	for(;;)
-	{
-		PIOS_LED_Toggle(LED1);
-		vTaskDelayUntil(&xLastExecutionTime, 1000 / portTICK_RATE_MS);
-	}
 }
 
 static void TaskTesting(void *pvParameters)