diff --git a/flight/OpenPilot/Modules/Example/examplemodperiodic.c b/flight/OpenPilot/Modules/Example/examplemodperiodic.c index 432db22b4..611e49876 100644 --- a/flight/OpenPilot/Modules/Example/examplemodperiodic.c +++ b/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 ); } } diff --git a/flight/OpenPilot/Modules/Example/examplemodthread.c b/flight/OpenPilot/Modules/Example/examplemodthread.c index dfe8e6050..5f601452a 100644 --- a/flight/OpenPilot/Modules/Example/examplemodthread.c +++ b/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 diff --git a/flight/OpenPilot/Modules/System/systemmod.c b/flight/OpenPilot/Modules/System/systemmod.c index 4fd2544ae..d46a958e7 100644 --- a/flight/OpenPilot/Modules/System/systemmod.c +++ b/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. + // 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 StatsUpdateCb(UAVObjEvent* ev); +static void objectUpdatedCb(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 - SettingsPersistenceConnectCallback(&ObjectUpdatedCb); + // System initialization + OpenPilotInit(); - // Create periodic event that will be used to update the system stats + // Initialize vars idleCounter = 0; idleCounterClear = 0; - memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicCallbackCreate(&ev, StatsUpdateCb, STATS_UPDATE_PERIOD_MS); + lastSysTime = xTaskGetTickCount(); - 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); } diff --git a/flight/OpenPilot/Modules/Telemetry/telemetry.c b/flight/OpenPilot/Modules/Telemetry/telemetry.c index a8a94d4c5..b512f0aca 100644 --- a/flight/OpenPilot/Modules/Telemetry/telemetry.c +++ b/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 - //xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY); + // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking len = PIOS_COM_ReceiveBufferUsed(inputPort); for (int32_t n = 0; n < len; ++n) { diff --git a/flight/OpenPilot/System/inc/openpilot.h b/flight/OpenPilot/System/inc/openpilot.h index e7ecc0f7f..075273690 100644 --- a/flight/OpenPilot/System/inc/openpilot.h +++ b/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 */ diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index 6ae56336a..b1ce57c22 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/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)