From 11d0bbf5f8590d0a17e3088b6ce52b5ae2c4b2fa Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Sat, 7 Jun 2014 17:43:36 +0200
Subject: [PATCH] OP-1365 Flight code instrumentation API: Use systemMod to
 publish counters

---
 flight/modules/System/systemmod.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c
index 13bf7b264..4f6f0e914 100644
--- a/flight/modules/System/systemmod.c
+++ b/flight/modules/System/systemmod.c
@@ -55,6 +55,12 @@
 #include <callbackinfo.h>
 #include <hwsettings.h>
 #include <pios_flashfs.h>
+
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <instrumentation.h>
+#include <pios_instrumentation.h>
+#endif
+
 #if defined(PIOS_INCLUDE_RFM22B)
 #include <oplinkstatus.h>
 #endif
@@ -124,7 +130,6 @@ int32_t SystemModStart(void)
     // Register task
     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
 
-
     return 0;
 }
 
@@ -148,6 +153,10 @@ int32_t SystemModInitialize(void)
     WatchdogStatusInitialize();
 #endif
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    InstrumentationInit();
+#endif
+
     objectPersistenceQueue = xQueueCreate(1, sizeof(UAVObjEvent));
     if (objectPersistenceQueue == NULL) {
         return -1;
@@ -205,6 +214,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
         updateWDGstats();
 #endif
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+        InstrumentationPublishAllCounters();
+#endif
+
 #ifdef DIAG_TASKS
         // Update the task status object
         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);