run period better support & system running states api

Author: xiaosanti

flight/modules/ManualControl/armhandler.c

@@ -130,7 +130,7 @@ void armHandler(bool newinit, FrameType_t frameType)
 
     if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) {
         // In this configuration we always disarm
-        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
+        setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); // update flight status msg
         return;
     }
 

flight/modules/Sensors/sensors.c

@@ -132,6 +132,7 @@ PERF_DEFINE_COUNTER(counterMagPeriod);
 PERF_DEFINE_COUNTER(counterBaroPeriod);
 PERF_DEFINE_COUNTER(counterSensorPeriod);
 PERF_DEFINE_COUNTER(counterSensorResets);
+PERF_DEFINE_COUNTER(counterSensorBetween);
 
 #if defined(PIOS_INCLUDE_HMC5X83)
 void aux_hmc5x83_load_settings();
@@ -270,6 +271,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
     PERF_INIT_COUNTER(counterBaroPeriod, 0x53000004);
     PERF_INIT_COUNTER(counterSensorPeriod, 0x53000005);
     PERF_INIT_COUNTER(counterSensorResets, 0x53000006);
+	PERF_INIT_COUNTER(counterSensorBetween, 0x53000007);
 
     // Test sensors
     bool sensors_test = true;
@@ -295,6 +297,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 
     while (1) {
         // TODO: add timeouts to the sensor reads and set an error if the fail
+		PERF_MEASURE_BETWEEN(counterSensorBetween, true);
         if (error) {
             RELOAD_WDG();
             lastSysTime = xTaskGetTickCount();
@@ -343,6 +346,13 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
         }
         PERF_MEASURE_PERIOD(counterSensorPeriod);
         RELOAD_WDG();
+
+		PERF_MEASURE_BETWEEN(counterSensorBetween, false);
+		// char* buffer = ((char*)pios_malloc(500));
+		
+        // PIOS_TASK_MONITOR_GetRunTimeStats(buffer);
+		// free(buffer);
+		// volatile int32_t debug_value = PIOS_Instrumentation_GetCounterValue(counterSensorBetween); // about 500us
         vTaskDelayUntil(&lastSysTime, sensor_period_ticks);
     }
 }
@@ -496,7 +506,6 @@ static void handleMag(float *samples, float temperature)
 
     rot_mult(mag_transform, mags, samples);
 
-	/*
 	// Debug
 	static uint32_t debug_ticks_pre = 0;
 	static uint32_t debug_ticks_now = 0;
@@ -506,7 +515,6 @@ static void handleMag(float *samples, float temperature)
 		debug_ticks_now = 0;
         DEBUG_PRINTF(0, "handle mag: %f,%f,%f\r\n", mags[0],mags[1],mags[2]);
 	}
-	*/
 
     mag.x = samples[0];
     mag.y = samples[1];

flight/modules/Sensors/simulated/Sensors/sensors.c

@@ -96,6 +96,11 @@ static float rand_gauss();
 enum sensor_sim_type { CONSTANT, MODEL_AGNOSTIC, MODEL_QUADCOPTER, MODEL_AIRPLANE } sensor_sim_type;
 
 #define GRAV 9.81
+
+#define PIOS_INSTRUMENT_MODULE
+#include <pios_instrumentation_helper.h>
+PERF_DEFINE_COUNTER(counterSimSensorPeriod);
+
 /**
  * Initialise the module.  Called before the start function
  * \returns 0 on success or -1 if initialisation failed
@@ -159,8 +164,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 
     // Main task loop
     lastSysTime = xTaskGetTickCount();
-    uint32_t last_time = PIOS_DELAY_GetRaw();
+    // uint32_t last_time = PIOS_DELAY_GetRaw();
+	PERF_INIT_COUNTER(counterSimSensorPeriod, 0x53000101);
     while (1) {
+		PERF_MEASURE_BETWEEN(counterSimSensorPeriod, true);
         PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
 
         SystemSettingsData systemSettings;
@@ -183,13 +190,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             sensor_sim_type = MODEL_AGNOSTIC;
         }
 
-        static int i;
-        i++;
-        if (i % 5000 == 0) {
-            // float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
-            // fprintf(stderr, "Sensor relative timing: %f\n", dT);
-            last_time = PIOS_DELAY_GetRaw();
-        }
 
         sensors_count++;
 
@@ -207,6 +207,18 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             simulateModelAirplane();
         }
 
+		PERF_MEASURE_BETWEEN(counterSimSensorPeriod, false); // id = 1392509185 cycle 130us
+        static int i;
+        i++;
+        // if (i % 5000 == 0) {
+            // float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
+            // fprintf(stderr, "Sensor relative timing: %f\n", dT);
+            //last_time = PIOS_DELAY_GetRaw();
+        //}
+		// if (i % 5000 == 0) {
+			// pios_perf_counter_t *counter = (pios_perf_counter_t *)counterSimSensorPeriod;
+			// fprintf(stderr, "simulation sensors perf counter: %d\n", counter->value);
+		// } 
         vTaskDelay(2 / portTICK_RATE_MS);
     }
 }

flight/modules/System/systemmod.c

@@ -249,6 +249,18 @@ static void systemTask(__attribute__((unused)) void *parameters)
 #endif
     // Main system loop
     while (1) {
+		// debug only
+		//static uint16_t i = 0;
+		//const size_t BUFFER_SIZE = 100;
+		//if (i != 0 && i % 20 == 0) {
+		//	char* buffer = (char*)pios_malloc(BUFFER_SIZE);
+		//	PIOS_TASK_MONITOR_GetRunTimeStats(buffer);
+		//	fprintf(stderr, "%s\n", buffer);
+		//	i = 0;
+		//} else {
+		//    i++;
+		//}
+
         NotificationUpdateStatus();
         // Update the system statistics
         updateStats();
@@ -751,6 +763,15 @@ static void updateSystemAlarms()
  */
 void vApplicationIdleHook(void)
 {
+/*
+	static uint16_t c = 0;
+	if (c > 0 && c % 5000 == 0) {
+		fprintf(stderr, "idle hook is running:%d\n", c);
+		c = 0;
+	} else {
+	    c++;
+	}
+*/
     PIOS_TASK_MONITOR_IdleHook();
     NotificationOnboardLedsRun();
 #ifdef PIOS_INCLUDE_WS2811

flight/pios/common/pios_callbackscheduler.c

@@ -375,6 +375,7 @@ void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallbac
 
         for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) {
             struct DelayedCallbackInfoStruct *cbinfo;
+            /* use volatile to avoid opt*/
             LL_FOREACH(task->callbackQueue[prio], cbinfo) {
                 xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
                 info.is_running = true;

flight/pios/common/pios_task_monitor.c

@@ -193,4 +193,69 @@ uint32_t PIOS_TASK_MONITOR_GetZeroLoadTicksCount()
     return zeroLoadIdleCounterPerSec;
 }
 
+/* 
+ * print a human readable table of run time stats
+ * information is generated from raw data provided by uxTaskGetSystemState().
+ * The human readable table is written to pcWriteBuffer.  (see the vTaskList()
+ * API function which actually does just this). 
+ */
+void PIOS_TASK_MONITOR_GetRunTimeStats(/*signed*/ char *pcWriteBuffer) {
+	TaskStatus_t *pxTaskStatusArray;
+	volatile UBaseType_t uxArraySize, x;
+	unsigned long ulTotalRunTime, ulStatsAsPercentage;
+
+   /* Make sure the write buffer does not contain a string. */
+   *pcWriteBuffer = 0x00;
+
+   /* Take a snapshot of the number of tasks in case it changes while this
+   function is executing. */
+   uxArraySize = uxTaskGetNumberOfTasks();
+
+   /* Allocate a TaskStatus_t structure for each task.  An array could be
+   allocated statically at compile time. */
+   pxTaskStatusArray = pvPortMalloc( uxArraySize * sizeof( TaskStatus_t ) );
+
+   if( pxTaskStatusArray != NULL ) {
+      /* Generate raw status information about each task. */
+      uxArraySize = uxTaskGetSystemState( pxTaskStatusArray,
+                                 uxArraySize,
+                                 &ulTotalRunTime );
+
+      /* For percentage calculations. */
+      ulTotalRunTime /= 100UL;
+
+      /* Avoid divide by zero errors. */
+      if( ulTotalRunTime > 0 ) {
+         /* For each populated position in the pxTaskStatusArray array,
+         format the raw data as human readable ASCII data. */
+         for( x = 0; x < uxArraySize; x++ ) {
+            /* What percentage of the total run time has the task used?
+            This will always be rounded down to the nearest integer.
+            ulTotalRunTimeDiv100 has already been divided by 100. */
+            ulStatsAsPercentage =
+                  pxTaskStatusArray[ x ].ulRunTimeCounter / ulTotalRunTime;
+
+            if( ulStatsAsPercentage > 0UL ) {
+               sprintf( pcWriteBuffer, "%s:%d,pct%d \n",
+                                 pxTaskStatusArray[ x ].pcTaskName,
+                                 pxTaskStatusArray[ x ].ulRunTimeCounter,
+                                 ulStatsAsPercentage );
+            }
+            else {
+               /* If the percentage is zero here then the task has
+               consumed less than 1% of the total run time. */
+               sprintf( pcWriteBuffer, "%s:%d,<1% \n",
+                                 pxTaskStatusArray[ x ].pcTaskName,
+                                 pxTaskStatusArray[ x ].ulRunTimeCounter );
+            }
+
+            pcWriteBuffer += strlen( ( char * ) pcWriteBuffer );
+         }
+      }
+
+      /* The array is no longer needed, free the memory it consumes. */
+      vPortFree( pxTaskStatusArray );
+   }
+}
+
 #endif // PIOS_INCLUDE_TASK_MONITOR

flight/pios/inc/pios_instrumentation.h

@@ -65,6 +65,22 @@ static inline void PIOS_Instrumentation_updateCounter(pios_counter_t counter_han
     vPortExitCritical();
 }
 
+/**
+ * Return a counter value
+ * @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ * @param none
+ */
+static inline int32_t PIOS_Instrumentation_GetCounterValue(pios_counter_t counter_handle)
+{
+	int32_t value = 0;
+    PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
+    vPortEnterCritical();
+    pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
+    value = counter->value;
+    vPortExitCritical();
+	return value;
+}
+
 /**
  * Used to determine the time duration of a code block, mark the begin of the block. @see PIOS_Instrumentation_TimeEnd
  * @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
@@ -127,6 +143,34 @@ static inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handl
     counter->lastUpdateTS = PIOS_DELAY_GetRaw();
 }
 
+/**
+ * Used to determine the mean period between start to the end 
+ * @param counter_handle handle of the counter @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
+ */
+static inline void PIOS_Instrumentation_TrackBetween(pios_counter_t counter_handle, bool start)
+{
+    PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
+    pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
+	if (start) {
+        counter->lastUpdateTS = PIOS_DELAY_GetRaw();
+	} else if (counter->lastUpdateTS != 0) {
+        vPortEnterCritical();
+        uint32_t period = PIOS_DELAY_DiffuS(counter->lastUpdateTS);
+        counter->value = (counter->value * 15 + period) / 16;
+        counter->max--;
+        if ((int32_t)period > counter->max) {
+            counter->max = period;
+        }
+        counter->min++;
+        if ((int32_t)period < counter->min) {
+            counter->min = period;
+        }
+        vPortExitCritical();
+    } else {
+	    // Invalid measurement
+	}
+}
+
 /**
  * Increment a counter with a value
  * @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter

flight/pios/inc/pios_instrumentation_helper.h

@@ -104,6 +104,7 @@
 #define PERF_TIMED_SECTION_START(x)   PIOS_Instrumentation_TimeStart(x)
 #define PERF_TIMED_SECTION_END(x)     PIOS_Instrumentation_TimeEnd(x)
 #define PERF_MEASURE_PERIOD(x)        PIOS_Instrumentation_TrackPeriod(x)
+#define PERF_MEASURE_BETWEEN(x, y)    PIOS_Instrumentation_TrackBetween(x, y)
 #define PERF_TRACK_VALUE(x, y)        PIOS_Instrumentation_updateCounter(x, y)
 #define PERF_INCREMENT_VALUE(x)       PIOS_Instrumentation_incrementCounter(x, 1)
 #define PERF_DECREMENT_VALUE(x)       PIOS_Instrumentation_incrementCounter(x, -1)
@@ -115,6 +116,7 @@
 #define PERF_TIMED_SECTION_START(x)
 #define PERF_TIMED_SECTION_END(x)
 #define PERF_MEASURE_PERIOD(x)
+#define PERF_MEASURE_BETWEEN(x, y) (void)y
 #define PERF_TRACK_VALUE(x, y) (void)y
 #define PERF_INCREMENT_VALUE(x)
 #define PERF_DECREMENT_VALUE(x)

flight/pios/inc/pios_task_monitor.h

@@ -134,4 +134,6 @@ extern uint32_t PIOS_TASK_MONITOR_GetIdleTicksCount();
 
 extern uint32_t PIOS_TASK_MONITOR_GetZeroLoadTicksCount();
 
+extern void PIOS_TASK_MONITOR_GetRunTimeStats(/*signed*/ char*);
+
 #endif // PIOS_TASK_MONITOR_H

flight/targets/boards/revolution/firmware/inc/FreeRTOSConfig.h

@@ -32,7 +32,7 @@
 #define configUSE_PREEMPTION                         1
 #define configUSE_IDLE_HOOK                          1
 #define configUSE_TICK_HOOK                          0
-#define configUSE_TRACE_FACILITY                     0
+#define configUSE_TRACE_FACILITY                     1
 #define configUSE_16_BIT_TICKS                       0
 #define configIDLE_SHOULD_YIELD                      0
 #define configUSE_MUTEXES                            1