diff --git a/flight/Libraries/taskmonitor.c b/flight/Libraries/taskmonitor.c
index 6fc1c4dc3..d8be06264 100644
--- a/flight/Libraries/taskmonitor.c
+++ b/flight/Libraries/taskmonitor.c
@@ -104,6 +104,7 @@ bool TaskMonitorQueryRunning(TaskInfoRunningElem task)
  */
 void TaskMonitorUpdateAll(void)
 {
+#if defined(DIAG_TASKS)
 	TaskInfoData data;
 	int n;
 
@@ -154,4 +155,5 @@ void TaskMonitorUpdateAll(void)
 
 	// Done
 	xSemaphoreGiveRecursive(lock);
+#endif
 }
diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c
index 4a0be78ef..d6fa9aeb2 100644
--- a/flight/Modules/System/systemmod.c
+++ b/flight/Modules/System/systemmod.c
@@ -126,7 +126,9 @@ int32_t SystemModInitialize(void)
 	SystemStatsInitialize();
 	FlightStatusInitialize();
 	ObjectPersistenceInitialize();
+#if defined(DIAG_TASKS)
 	TaskInfoInitialize();
+#endif
 #if defined(I2C_WDG_STATS_DIAGNOSTICS)
 	I2CStatsInitialize();
 	WatchdogStatusInitialize();
@@ -190,8 +192,10 @@ static void systemTask(void *parameters)
 		updateWDGstats();
 #endif
 
+#if defined(DIAG_TASKS)
 		// Update the task status object
 		TaskMonitorUpdateAll();
+#endif
 
 		// Flash the heartbeat LED
 #if defined(PIOS_LED_HEARTBEAT)