Feature accurate cpu load (#2716)

* use more precise STATISTICS module instead of deprecated PROFILING one to get accurate cpu load informations
* enable sys_mon module in drop1 airframe

conf/Makefile.chibios

@@ -69,9 +69,9 @@ $(TARGET).CFLAGS += \
 	-DCH_DBG_ENABLE_TRACE=TRUE \
 	-DCH_DBG_ENABLE_STACK_CHECK=TRUE \
 	-DCH_DBG_FILL_THREADS=TRUE \
-	-DCH_DBG_THREADS_PROFILING=TRUE
+	-DCH_DBG_STATISTICS=TRUE
 else
-$(TARGET).CFLAGS += -DCH_DBG_THREADS_PROFILING=TRUE -DCH_DBG_FILL_THREADS=TRUE
+$(TARGET).CFLAGS += -DCH_DBG_STATISTICS=TRUE -DCH_DBG_FILL_THREADS=TRUE
 endif
 
 #

conf/airframes/ENAC/quadrotor/drop1.xml

@@ -26,6 +26,7 @@
     </target>
 
     <module name="telemetry" type="xbee_api"/>
+    <module name="sys_mon"/>
 
     <module name="motor_mixing"/>
 

sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c

@@ -28,8 +28,8 @@
 #include "modules/core/sys_mon_rtos.h"
 #include <ch.h>
 
-#if !CH_DBG_THREADS_PROFILING
+#if !CH_DBG_STATISTICS
-#error CH_DBG_THREADS_PROFILING should be defined to TRUE to use this monitoring tool
+#error CH_DBG_STATISTICS should be defined to TRUE to use this monitoring tool
 #endif
 
 static uint32_t thread_p_time[RTOS_MON_MAX_THREADS];
@@ -41,12 +41,12 @@ static uint16_t get_stack_free(const thread_t *tp);
 #include "modules/core/shell.h"
 #include "printf.h"
 #include "string.h"
-#define MAX_CPU_INFO_ENTRIES 20
 
 typedef struct _ThreadCpuInfo {
-  float    ticks[MAX_CPU_INFO_ENTRIES];
+  float    ticks[RTOS_MON_MAX_THREADS];
-  float    cpu[MAX_CPU_INFO_ENTRIES];
+  float    cpu[RTOS_MON_MAX_THREADS];
   float    totalTicks;
+  float    totalISRTicks;
 } ThreadCpuInfo ;
 
 
@@ -54,34 +54,39 @@ static void stampThreadCpuInfo (ThreadCpuInfo *ti)
 {
   const thread_t *tp =  chRegFirstThread();
   uint32_t idx=0;
-  float totalTicks =0;
+  
+  ti->totalTicks =0;
   do {
-    totalTicks+= (float) tp->time;
+    ti->ticks[idx] = (float) tp->stats.cumulative;
-    ti->cpu[idx] = (float) tp->time - ti->ticks[idx];;
+    ti->totalTicks += ti->ticks[idx];
-    ti->ticks[idx] = (float) tp->time;
     tp = chRegNextThread ((thread_t *)tp);
     idx++;
-  } while ((tp != NULL) && (idx < MAX_CPU_INFO_ENTRIES));
+  } while ((tp != NULL) && (idx < RTOS_MON_MAX_THREADS));
-
+  ti->totalISRTicks = ch.kernel_stats.m_crit_isr.cumulative;
-  const float diffTotal = totalTicks- ti->totalTicks;
+  ti->totalTicks += ti->totalISRTicks;
-  ti->totalTicks = totalTicks;
   tp =  chRegFirstThread();
   idx=0;
   do {
-    ti->cpu[idx] =  (ti->cpu[idx]*100.f)/diffTotal;
+    ti->cpu[idx] =  (ti->ticks[idx]*100.f) / ti->totalTicks;
     tp = chRegNextThread ((thread_t *)tp);
     idx++;
-  } while ((tp != NULL) && (idx < MAX_CPU_INFO_ENTRIES));
+  } while ((tp != NULL) && (idx < RTOS_MON_MAX_THREADS));
 }
 
 static float stampThreadGetCpuPercent (const ThreadCpuInfo *ti, const uint32_t idx)
 {
-  if (idx >= MAX_CPU_INFO_ENTRIES)
+  if (idx >= RTOS_MON_MAX_THREADS) 
     return -1.f;
+
   return ti->cpu[idx];
 }
 
-static void cmd_threads(shell_stream_t *lchp, int argc,const char* const argv[]) {
+static float stampISRGetCpuPercent (const ThreadCpuInfo *ti)
+{
+  return ti->totalISRTicks * 100.0f / ti->totalTicks;
+}
+
+static void cmd_threads(BaseSequentialStream *lchp, int argc,const char * const argv[]) {
   static const char *states[] = {CH_STATE_NAMES};
   thread_t *tp = chRegFirstThread();
   (void)argv;
@@ -90,30 +95,38 @@ static void cmd_threads(shell_stream_t *lchp, int argc,const char* const argv[])
   float idleTicks=0;
 
   static ThreadCpuInfo threadCpuInfo = {
-    .ticks = {[0 ... MAX_CPU_INFO_ENTRIES-1] = 0.f},
+    .ticks = {[0 ... RTOS_MON_MAX_THREADS-1] = 0.f}, 
-    .cpu =   {[0 ... MAX_CPU_INFO_ENTRIES-1] =-1.f},
+    .cpu =   {[0 ... RTOS_MON_MAX_THREADS-1] =-1.f}, 
-    .totalTicks = 0.f
+    .totalTicks = 0.f,
+    .totalISRTicks = 0.f
   };
+  
   stampThreadCpuInfo (&threadCpuInfo);
-
+  
   chprintf (lchp, "    addr    stack  frestk prio refs  state        time \t percent        name\r\n");
   uint32_t idx=0;
   do {
-    chprintf (lchp, "%.8lx %.8lx %6lu %4lu %4lu %9s %9lu   %.1f    \t%s\r\n",
+    chprintf (lchp, "%.8lx %.8lx %6lu %4lu %4lu %9s %9lu   %.2f%%    \t%s\r\n",
-        (uint32_t)tp, (uint32_t)tp->ctx.sp,
+	      (uint32_t)tp, (uint32_t)tp->ctx.sp,
-        get_stack_free (tp),
+	      get_stack_free (tp),
-        (uint32_t)tp->hdr.pqueue.prio, (uint32_t)(tp->refs - 1),
+	      (uint32_t)tp->hdr.pqueue.prio, (uint32_t)(tp->refs - 1),
-        states[tp->state], (uint32_t)tp->time,
+	      states[tp->state],
-        stampThreadGetCpuPercent (&threadCpuInfo, idx),
+	      (uint32_t)RTC2MS(STM32_SYSCLK, tp->stats.cumulative),
-        chRegGetThreadNameX(tp));
+	      stampThreadGetCpuPercent (&threadCpuInfo, idx),
-    totalTicks+= (float) tp->time;
+	      chRegGetThreadNameX(tp));
-    if (strcmp (chRegGetThreadNameX(tp), "idle") == 0)
+
-      idleTicks =  (float) tp->time;
+    totalTicks+= (float)tp->stats.cumulative;
+    if (strcmp(chRegGetThreadNameX(tp), "idle") == 0)
+    idleTicks = (float)tp->stats.cumulative;
     tp = chRegNextThread ((thread_t *)tp);
     idx++;
   } while (tp != NULL);
+
   const float idlePercent = (idleTicks*100.f)/totalTicks;
   const float cpuPercent = 100.f - idlePercent;
+  chprintf (lchp, "Interrupt Service Routine \t\t     %9lu   %.2f%%    \tISR\r\n",
+	    (uint32_t)RTC2MS(STM32_SYSCLK,threadCpuInfo.totalISRTicks),
+	    stampISRGetCpuPercent(&threadCpuInfo));
   chprintf (lchp, "\r\ncpu load = %.2f%%\r\n", cpuPercent);
 }
 
@@ -182,19 +195,20 @@ void rtos_mon_periodic_arch(void)
     rtos_mon.thread_free_stack[i] = get_stack_free(tp);
 
     // store time spend in thread
-    thread_p_time[rtos_mon.thread_counter] = tp->time;
+    thread_p_time[rtos_mon.thread_counter] = tp->stats.cumulative;
-    sum += (float)(tp->time);
+    sum += (float)(tp->stats.cumulative);
 
     // if current thread is 'idle' thread, store its value separately
     if (tp == chSysGetIdleThreadX()) {
-      idle_counter = (uint32_t)tp->time;
+      idle_counter = (uint32_t)tp->stats.cumulative;
     }
     // get next thread
     tp = chRegNextThread(tp);
     // increment thread counter
     rtos_mon.thread_counter++;
   } while (tp != NULL && rtos_mon.thread_counter < RTOS_MON_MAX_THREADS);
-
+  // sum the time spent in ISR
+  sum += ch.kernel_stats.m_crit_isr.cumulative;
   // store individual thread load (as centi-percent integer, i.e. (th_time/sum)*10*100)
   for (i = 0; i < rtos_mon.thread_counter; i ++) {
     rtos_mon.thread_load[i] = (uint16_t)(1000.f * (float)thread_p_time[i] / sum);