OP-4 Flight/System Created CPU utilization measurement and created object to report system information (load, heap and time)

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

flight/OpenPilot/Makefile

@@ -127,6 +127,7 @@ SRC += $(OPUAVOBJ)/eventdispatcher.c
 #SRC += $(OPTESTS)/test_I2c_PCF8570.c
 #SRC += $(OPTESTS)/test_BMP085.c
 #SRC += $(OPTESTS)/test_uavobjects.c
+#SRC += $(OPTESTS)/test_cpuload.c
 
 
 ## UAVOBJECTS
@@ -137,6 +138,8 @@ SRC += $(OPUAVOBJ)/settingspersistence.c
 SRC += $(OPUAVOBJ)/gpsobject.c
 SRC += $(OPUAVOBJ)/gcstelemetrystats.c
 SRC += $(OPUAVOBJ)/flighttelemetrystats.c
+SRC += $(OPUAVOBJ)/systemstats.c
+SRC += $(OPUAVOBJ)/systemalarms.c
 
 ## PIOS Hardware (STM32F10x)
 SRC += $(PIOSSTM32F10X)/pios_sys.c

flight/OpenPilot/Modules/GPS/GPS.c

@@ -73,7 +73,7 @@ char NmeaPacket[NMEA_BUFFERSIZE];
 // Private constants
 //#define MAX_QUEUE_SIZE 20
 #define STACK_SIZE configMINIMAL_STACK_SIZE
-#define TASK_PRIORITY (tskIDLE_PRIORITY + 10)
+#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
 //#define REQ_TIMEOUT_MS 500
 //#define MAX_RETRIES 3
 

flight/OpenPilot/Modules/System/systemmod.c

@@ -26,15 +26,24 @@
 
 #include "systemmod.h"
 #include "settingspersistence.h"
+#include "systemstats.h"
 
 // Private constants
+#define STATS_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.
 
 // Private types
 
 // Private variables
+static uint32_t idleCounter;
+static uint32_t idleCounterClear;
+static xSemaphoreHandle mutex;
 
 // Private functions
 static void ObjectUpdatedCb(UAVObjEvent* ev);
+static void StatsUpdateCb(UAVObjEvent* ev);
 
 /**
  * Initialise the module, called on startup.
@@ -42,9 +51,20 @@ static void ObjectUpdatedCb(UAVObjEvent* ev);
  */
 int32_t SystemModInitialize(void)
 {
+	UAVObjEvent ev;
+
+	// Create the mutex
+	mutex = xSemaphoreCreateRecursiveMutex();
+
 	// Listen for ExampleObject1 updates, connect a callback function
 	SettingsPersistenceConnectCallback(&ObjectUpdatedCb);
 
+	// Create periodic event that will be used to update the system stats
+	idleCounter = 0;
+	idleCounterClear = 0;
+	memset(&ev, 0, sizeof(UAVObjEvent));
+	EventPeriodicCallbackCreate(&ev, StatsUpdateCb, STATS_UPDATE_PERIOD_MS);
+
 	return 0;
 }
 
@@ -73,4 +93,36 @@ static void ObjectUpdatedCb(UAVObjEvent* ev)
 	}
 }
 
+/**
+ * Called periodically to update the system stats
+ */
+static void StatsUpdateCb(UAVObjEvent* ev)
+{
+	SystemStatsData stats;
+
+	// Get stats and update
+	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);
+	idleCounterClear = 1;
+	SystemStatsSet(&stats);
+}
+
+/**
+ * Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
+ */
+void vApplicationIdleHook(void)
+{
+	// Called when the scheduler has no tasks to run
+	if (idleCounterClear == 0)
+	{
+		++idleCounter;
+	}
+	else
+	{
+		idleCounter = 0;
+		idleCounterClear = 0;
+	}
+}
 

flight/OpenPilot/Modules/Telemetry/telemetry.c

@@ -283,6 +283,8 @@ static void telemetryRxTask(void* parameters)
 			//PIOS_LED_Toggle(LED1);
 			UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort));
 		}
+		vTaskDelay(1); // <- remove when blocking calls are implemented
+
 	}
 }
 

flight/OpenPilot/System/openpilot.c

@@ -322,12 +322,4 @@ static void TaskSDCard(void *pvParameters)
 	}
 }
 
-/**
-* Idle hook function
-*/
-void vApplicationIdleHook(void)
-{
-	/* Called when the scheduler has no tasks to run */
 
-
-}

flight/OpenPilot/Tests/test_cpuload.c

@@ -0,0 +1,89 @@
+/**
+ ******************************************************************************
+ *
+ * @file       test_cpuload.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Calibration for the CPU load calculation
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * This file is used to calculate the number of counts we get when
+ * the CPU is fully idle (no load). This is used by systemmod.c to
+ * calculate the CPU load. This calibration should be run whenever
+ * changes are made in the FreeRTOS configuration or on the compiler
+ * optimisation parameters.
+ */
+
+#include "openpilot.h"
+
+// Local constants
+#define BENCHMARK_DURATION_MS 10000
+
+// Local functions
+static void testTask(void *pvParameters);
+
+// Variables
+static uint32_t idleCounter = 0;
+static uint32_t idleCounterClear = 0;
+
+int main()
+{
+	PIOS_SYS_Init();
+
+	// Create test task
+	xTaskCreate(testTask, (signed portCHAR *)"Test", 1000 , NULL, 1, NULL);
+
+	// Start the FreeRTOS scheduler
+	vTaskStartScheduler();
+	return 0;
+}
+
+static void testTask(void *pvParameters)
+{
+	uint32_t countsPerSecond = 0;
+
+	while (1)
+	{
+		// Delay until enough required test duration
+		vTaskDelay( BENCHMARK_DURATION_MS / portTICK_RATE_MS );
+
+		// Calculate counts per second, set breakpoint here
+		countsPerSecond = idleCounter / (BENCHMARK_DURATION_MS/1000);
+
+		// Reset and start again - do not clear idleCounter directly!
+		// SET BREAKPOINT HERE and read the countsPerSecond variable
+		// this should be used to update IDLE_COUNTS_PER_SEC_AT_NO_LOAD in systemmod.c
+		idleCounterClear = 1;
+	}
+}
+
+void vApplicationIdleHook(void)
+{
+	/* Called when the scheduler has no tasks to run */
+	if (idleCounterClear == 0)
+	{
+		++idleCounter;
+	}
+	else
+	{
+		idleCounter = 0;
+		idleCounterClear = 0;
+	}
+}

flight/OpenPilot/UAVObjects/inc/systemalarms.h

@@ -0,0 +1,78 @@
+/**
+ ******************************************************************************
+ *
+ * @file       systemalarms.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the SystemAlarms object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: systemalarms.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef SYSTEMALARMS_H
+#define SYSTEMALARMS_H
+
+#include "openpilot.h"
+
+// Object constants
+#define SYSTEMALARMS_OBJID 1784134234U
+#define SYSTEMALARMS_NAME "SystemAlarms"
+#define SYSTEMALARMS_ISSINGLEINST 1
+#define SYSTEMALARMS_ISSETTINGS 0
+#define SYSTEMALARMS_NUMBYTES sizeof(SystemAlarmsData)
+
+// Object access macros
+#define SystemAlarmsGet(dataOut) UAVObjGetData(SystemAlarmsHandle(), dataOut)
+#define SystemAlarmsSet(dataIn) UAVObjSetData(SystemAlarmsHandle(), dataIn)
+#define SystemAlarmsInstGet(instId, dataOut) UAVObjGetInstanceData(SystemAlarmsHandle(), instId, dataOut)
+#define SystemAlarmsInstSet(instId, dataIn) UAVObjSetInstanceData(SystemAlarmsHandle(), instId, dataIn)
+#define SystemAlarmsConnectQueue(queue) UAVObjConnectQueue(SystemAlarmsHandle(), queue, EV_MASK_ALL_UPDATES)
+#define SystemAlarmsConnectCallback(cb) UAVObjConnectCallback(SystemAlarmsHandle(), cb, EV_MASK_ALL_UPDATES)
+#define SystemAlarmsCreateInstance() UAVObjCreateInstance(SystemAlarmsHandle())
+#define SystemAlarmsRequestUpdate() UAVObjRequestUpdate(SystemAlarmsHandle())
+#define SystemAlarmsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(SystemAlarmsHandle(), instId)
+#define SystemAlarmsUpdated() UAVObjUpdated(SystemAlarmsHandle())
+#define SystemAlarmsInstUpdated(instId) UAVObjUpdated(SystemAlarmsHandle(), instId)
+#define SystemAlarmsGetMetadata(dataOut) UAVObjGetMetadata(SystemAlarmsHandle(), dataOut)
+#define SystemAlarmsSetMetadata(dataIn) UAVObjSetMetadata(SystemAlarmsHandle(), dataIn)
+
+// Object data
+typedef struct {
+    uint8_t Type;
+    uint8_t Severity;
+    uint8_t Active;
+    uint32_t Info;
+
+} __attribute__((packed)) SystemAlarmsData;
+
+// Enumeration types
+typedef enum { SYSTEMALARMS_TYPE_NONE=0, SYSTEMALARMS_TYPE_STACKOVERFLOW=1, SYSTEMALARMS_TYPE_OUTOFMEMORY=2,  } SYSTEMALARMSTYPEEnum;
+typedef enum { SYSTEMALARMS_SEVERITY_INFO=0, SYSTEMALARMS_SEVERITY_WARNING=1, SYSTEMALARMS_SEVERITY_ERROR=2, SYSTEMALARMS_SEVERITY_CRITICAL=3,  } SYSTEMALARMSSEVERITYEnum;
+typedef enum { SYSTEMALARMS_ACTIVE_TRUE=0, SYSTEMALARMS_ACTIVE_FALSE=1,  } SYSTEMALARMSACTIVEEnum;
+
+
+// Generic interface functions
+int32_t SystemAlarmsInitialize();
+UAVObjHandle SystemAlarmsHandle();
+
+#endif // SYSTEMALARMS_H

flight/OpenPilot/UAVObjects/inc/systemstats.h

@@ -0,0 +1,74 @@
+/**
+ ******************************************************************************
+ *
+ * @file       systemstats.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the SystemStats object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: systemstats.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef SYSTEMSTATS_H
+#define SYSTEMSTATS_H
+
+#include "openpilot.h"
+
+// Object constants
+#define SYSTEMSTATS_OBJID 680908530U
+#define SYSTEMSTATS_NAME "SystemStats"
+#define SYSTEMSTATS_ISSINGLEINST 1
+#define SYSTEMSTATS_ISSETTINGS 0
+#define SYSTEMSTATS_NUMBYTES sizeof(SystemStatsData)
+
+// Object access macros
+#define SystemStatsGet(dataOut) UAVObjGetData(SystemStatsHandle(), dataOut)
+#define SystemStatsSet(dataIn) UAVObjSetData(SystemStatsHandle(), dataIn)
+#define SystemStatsInstGet(instId, dataOut) UAVObjGetInstanceData(SystemStatsHandle(), instId, dataOut)
+#define SystemStatsInstSet(instId, dataIn) UAVObjSetInstanceData(SystemStatsHandle(), instId, dataIn)
+#define SystemStatsConnectQueue(queue) UAVObjConnectQueue(SystemStatsHandle(), queue, EV_MASK_ALL_UPDATES)
+#define SystemStatsConnectCallback(cb) UAVObjConnectCallback(SystemStatsHandle(), cb, EV_MASK_ALL_UPDATES)
+#define SystemStatsCreateInstance() UAVObjCreateInstance(SystemStatsHandle())
+#define SystemStatsRequestUpdate() UAVObjRequestUpdate(SystemStatsHandle())
+#define SystemStatsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(SystemStatsHandle(), instId)
+#define SystemStatsUpdated() UAVObjUpdated(SystemStatsHandle())
+#define SystemStatsInstUpdated(instId) UAVObjUpdated(SystemStatsHandle(), instId)
+#define SystemStatsGetMetadata(dataOut) UAVObjGetMetadata(SystemStatsHandle(), dataOut)
+#define SystemStatsSetMetadata(dataIn) UAVObjSetMetadata(SystemStatsHandle(), dataIn)
+
+// Object data
+typedef struct {
+    uint32_t FlightTime;
+    uint16_t HeapRemaining;
+    uint8_t CPULoad;
+
+} __attribute__((packed)) SystemStatsData;
+
+// Enumeration types
+
+
+// Generic interface functions
+int32_t SystemStatsInitialize();
+UAVObjHandle SystemStatsHandle();
+
+#endif // SYSTEMSTATS_H

flight/OpenPilot/UAVObjects/systemalarms.c

@@ -0,0 +1,73 @@
+/**
+ ******************************************************************************
+ *
+ * @file       systemalarms.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the SystemAlarms object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: systemalarms.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "systemalarms.h"
+
+// Private variables
+static UAVObjHandle handle;
+
+/**
+ * Initialize object.
+ * \return 0 Success
+ * \return -1 Failure
+ */
+int32_t SystemAlarmsInitialize()
+{
+	UAVObjMetadata metadata;
+
+	// Register object with the object manager
+	handle = UAVObjRegister(SYSTEMALARMS_OBJID, SYSTEMALARMS_NAME, 0, SYSTEMALARMS_ISSINGLEINST, SYSTEMALARMS_ISSETTINGS, SYSTEMALARMS_NUMBYTES);
+	if (handle == 0) return -1;
+
+	// Initialize metadata
+	metadata.telemetryAcked = 1;
+	metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
+	metadata.telemetryUpdatePeriod = 0;
+	metadata.gcsTelemetryAcked = 1;
+	metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
+	metadata.gcsTelemetryUpdatePeriod = 0;
+	metadata.loggingUpdateMode = UPDATEMODE_ONCHANGE;
+	metadata.loggingUpdatePeriod = 0;
+	UAVObjSetMetadata(handle, &metadata);
+
+	// Done
+	return 0;
+}
+
+/**
+ * Get object handle
+ */
+UAVObjHandle SystemAlarmsHandle()
+{
+	return handle;
+}
+
+

flight/OpenPilot/UAVObjects/systemstats.c

@@ -0,0 +1,73 @@
+/**
+ ******************************************************************************
+ *
+ * @file       systemstats.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Implementation of the SystemStats object. This file has been 
+ *             automatically generated by the UAVObjectGenerator.
+ * 
+ * @note       Object definition file: systemstats.xml. 
+ *             This is an automatically generated file.
+ *             DO NOT modify manually.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "systemstats.h"
+
+// Private variables
+static UAVObjHandle handle;
+
+/**
+ * Initialize object.
+ * \return 0 Success
+ * \return -1 Failure
+ */
+int32_t SystemStatsInitialize()
+{
+	UAVObjMetadata metadata;
+
+	// Register object with the object manager
+	handle = UAVObjRegister(SYSTEMSTATS_OBJID, SYSTEMSTATS_NAME, 0, SYSTEMSTATS_ISSINGLEINST, SYSTEMSTATS_ISSETTINGS, SYSTEMSTATS_NUMBYTES);
+	if (handle == 0) return -1;
+
+	// Initialize metadata
+	metadata.telemetryAcked = 1;
+	metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
+	metadata.telemetryUpdatePeriod = 1000;
+	metadata.gcsTelemetryAcked = 1;
+	metadata.gcsTelemetryUpdateMode = UPDATEMODE_NEVER;
+	metadata.gcsTelemetryUpdatePeriod = 0;
+	metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
+	metadata.loggingUpdatePeriod = 1000;
+	UAVObjSetMetadata(handle, &metadata);
+
+	// Done
+	return 0;
+}
+
+/**
+ * Get object handle
+ */
+UAVObjHandle SystemStatsHandle()
+{
+	return handle;
+}
+
+

flight/OpenPilot/UAVObjects/uavobjectsinit.c

@@ -35,6 +35,8 @@
 #include "gcstelemetrystats.h"
 #include "gpsobject.h"
 #include "settingspersistence.h"
+#include "systemalarms.h"
+#include "systemstats.h"
 
 
 /**
@@ -50,5 +52,7 @@ void UAVObjectsInitializeAll()
     GCSTelemetryStatsInitialize();
     GpsObjectInitialize();
     SettingsPersistenceInitialize();
+    SystemAlarmsInitialize();
+    SystemStatsInitialize();
 
 }

flight/PiOS/inc/FreeRTOSConfig.h

@@ -23,11 +23,11 @@
 #define configTICK_RATE_HZ		( ( portTickType ) 1000 )
 #define configMAX_PRIORITIES		( ( unsigned portBASE_TYPE ) 5 )
 #define configMINIMAL_STACK_SIZE	( ( unsigned short ) 256 )
-#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 20 * 1024 ) )
+#define configTOTAL_HEAP_SIZE		( ( size_t ) ( 45 * 1024 ) )
 #define configMAX_TASK_NAME_LEN		( 16 )
 #define configUSE_TRACE_FACILITY	0
 #define configUSE_16_BIT_TICKS		0
-#define configIDLE_SHOULD_YIELD		1
+#define configIDLE_SHOULD_YIELD		0
 #define configUSE_MUTEXES		1
 #define configUSE_RECURSIVE_MUTEXES	1
 #define configUSE_COUNTING_SEMAPHORES	0