Merge branch 'bugfix-flight'

flight/CopterControl/System/inc/FreeRTOSConfig.h

@@ -71,6 +71,18 @@ configKERNEL_INTERRUPT_PRIORITY setting.  Here 15 corresponds to the lowest
 NVIC value of 255. */
 #define configLIBRARY_KERNEL_INTERRUPT_PRIORITY	15
 
+/* Enable run time stats collection */
+//#if defined(DEBUG)
+#define configGENERATE_RUN_TIME_STATS 1
+#define INCLUDE_uxTaskGetRunTime 1
+#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
+do {\
+(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\
+(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
+} while(0)
+#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
+//#endif
+
 /**
   * @}
   */

flight/CopterControl/System/taskmonitor.c

@@ -35,21 +35,23 @@
 // Private variables
 static xSemaphoreHandle lock;
 static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
+static uint32_t lastMonitorTime;
 
 // Private functions
 
 /**
- * Initialize library
+ * Initialize library
  */
 int32_t TaskMonitorInitialize(void)
 {
 	lock = xSemaphoreCreateRecursiveMutex();
 	memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
+	lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
 	return 0;
 }
 
 /**
- * Register a task handle with the library
+ * Register a task handle with the library
  */
 int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
 {
@@ -67,16 +69,30 @@ int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
 }
 
 /**
- * Update the status of all tasks
+ * Update the status of all tasks
  */
 void TaskMonitorUpdateAll(void)
 {
 	TaskInfoData data;
 	int n;
-
+	
 	// Lock
 	xSemaphoreTakeRecursive(lock, portMAX_DELAY);
-
+	
+#if ( configGENERATE_RUN_TIME_STATS == 1 )
+	uint32_t currentTime;
+	uint32_t deltaTime;
+	
+	/*
+	 * Calculate the amount of elapsed run time between the last time we
+	 * measured and now. Scale so that we can convert task run times
+	 * directly to percentages.
+	 */
+	currentTime = portGET_RUN_TIME_COUNTER_VALUE();
+	deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
+	lastMonitorTime = currentTime;			
+#endif
+	
 	// Update all task information
 	for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
 	{
@@ -87,18 +103,24 @@ void TaskMonitorUpdateAll(void)
 			data.StackRemaining[n] = 10000;
 #else
 			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
+#if ( configGENERATE_RUN_TIME_STATS == 1 )
+			/* Generate run time stats */
+			data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
 #endif
+#endif
+			
 		}
 		else
 		{
 			data.Running[n] = TASKINFO_RUNNING_FALSE;
 			data.StackRemaining[n] = 0;
+			data.RunningTime[n] = 0;
 		}
 	}
-
+	
 	// Update object
 	TaskInfoSet(&data);
-
+	
 	// Done
 	xSemaphoreGiveRecursive(lock);
 }

flight/OpenPilot/System/inc/FreeRTOSConfig.h

@@ -75,9 +75,12 @@ NVIC value of 255. */
 #if defined(DEBUG)
 #define configGENERATE_RUN_TIME_STATS 1
 #define INCLUDE_uxTaskGetRunTime 1
-#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() PIOS_RTC_Init()
+#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
-// Note: Using the tick count defeats the purpose here, need some timer on the scale of 10khz
+do {\
-#define portGET_RUN_TIME_COUNTER_VALUE()  PIOS_RTC_Counter()
+(*(unsigned long *)0xe000edfc) |= (1<<24);/* DEMCR |= DEMCR_TRCENA */\
+(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
+} while(0)
+#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
 #endif
 
 

flight/OpenPilot/System/taskmonitor.c

@@ -35,6 +35,7 @@
 // Private variables
 static xSemaphoreHandle lock;
 static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM];
+static uint32_t lastMonitorTime;
 
 // Private functions
 
@@ -45,6 +46,7 @@ int32_t TaskMonitorInitialize(void)
 {
 	lock = xSemaphoreCreateRecursiveMutex();
 	memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
+	lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
 	return 0;
 }
 
@@ -95,6 +97,20 @@ void TaskMonitorUpdateAll(void)
 	// Lock
 	xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 
+#if ( configGENERATE_RUN_TIME_STATS == 1 )
+	uint32_t currentTime;
+	uint32_t deltaTime;
+	
+	/*
+	 * Calculate the amount of elapsed run time between the last time we
+	 * measured and now. Scale so that we can convert task run times
+	 * directly to percentages.
+	 */
+	currentTime = portGET_RUN_TIME_COUNTER_VALUE();
+	deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */
+	lastMonitorTime = currentTime;			
+#endif
+	
 	// Update all task information
 	for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n)
 	{
@@ -107,7 +123,8 @@ void TaskMonitorUpdateAll(void)
 			data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4;
 #if ( configGENERATE_RUN_TIME_STATS == 1 )
 			/* Generate run time stats */
-			data.RunningTime[n] = 100 * (float) uxTaskGetRunTime(handles[n]) / portGET_RUN_TIME_COUNTER_VALUE();
+			data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime;
+			
 #endif
 #endif
 			

flight/PiOS/Common/pios_flashfs_objlist.c

@@ -105,7 +105,7 @@ int32_t PIOS_FLASHFS_Init()
  */
 static int32_t PIOS_FLASHFS_CleabObjectTableHeader() 
 {
-	if(PIOS_Flash_W25X_EraseSector(OBJECT_TABLE_START) != 0)
+	if(PIOS_Flash_W25X_EraseSector(0) != 0)
 		return -1;
 
 	uint32_t object_table_magic = OBJECT_TABLE_MAGIC;

flight/PiOS/STM32F10x/Libraries/FreeRTOS/Source/tasks.c

@@ -2315,9 +2315,13 @@ tskTCB *pxNewTCB;
 #if ( INCLUDE_uxTaskGetRunTime == 1 )
 unsigned portBASE_TYPE uxTaskGetRunTime( xTaskHandle xTask )
 {
+	unsigned long runTime;
+	
 	tskTCB *pxTCB;
 	pxTCB = prvGetTCBFromHandle( xTask );
-	return pxTCB->ulRunTimeCounter;
+	runTime = pxTCB->ulRunTimeCounter;
+	pxTCB->ulRunTimeCounter = 0;
+	return runTime;
 }
 
 #endif