diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile
index 8687defef..adb3de6ae 100644
--- a/flight/CopterControl/Makefile
+++ b/flight/CopterControl/Makefile
@@ -38,9 +38,15 @@ OUTDIR := $(TOP)/build/$(TARGET)
 DEBUG ?= NO
 
 # Include objects that are just nice information to show
-DIAGNOSTICS ?= NO
+STACK_DIAGNOSTICS ?= NO
+MIXERSTATUS_DIAGNOSTICS ?= NO
+RATEDESIRED_DIAGNOSTICS ?= NO
+I2C_WDG_STATS_DIAGNOSTICS ?= NO
 DIAG_TASKS ?= NO
 
+#Or just turn on all the above diagnostics. WARNING: This consumes massive amounts of memory.
+ALL_DIGNOSTICS ?=NO
+
 # Set to YES to build a FW version that will erase all flash memory
 ERASE_FLASH ?= NO
 # Set to YES to use the Servo output pins for debugging via scope or logic analyser
@@ -485,11 +491,25 @@ ifeq ($(DEBUG),YES)
 CFLAGS += -DDEBUG
 endif
 
-ifeq ($(DIAGNOSTICS),YES)
-CFLAGS += -DDIAGNOSTICS
+#The following Makefile command, ifneq (, $(filter) $(A), $(B) $(C)) is equivalent 
+# to the pseudocode `if(A== B || A==C)`
+ifneq (,$(filter YES,$(STACK_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DSTACK_DIAGNOSTICS
 endif
 
-ifeq ($(DIAG_TASKS),YES)
+ifneq (,$(filter YES,$(MIXERSTATUS_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DMIXERSTATUS_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(RATEDESIRED_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DRATEDESIRED_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(I2C_WDG_STATS_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(DIAG_TASKS) $(ALL_DIGNOSTICS)))
 CFLAGS += -DDIAG_TASKS
 endif
 
diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c
index e59d50957..6bd471666 100644
--- a/flight/Modules/Actuator/actuator.c
+++ b/flight/Modules/Actuator/actuator.c
@@ -126,7 +126,7 @@ int32_t ActuatorInitialize()
 	// Primary output of this module
 	ActuatorCommandInitialize();
 
-#if defined(DIAGNOSTICS)
+#if defined(MIXERSTATUS_DIAGNOSTICS)
 	// UAVO only used for inspecting the internal status of the mixer during debug
 	MixerStatusInitialize();
 #endif
@@ -212,7 +212,7 @@ static void actuatorTask(void* parameters)
 		ActuatorDesiredGet(&desired);
 		ActuatorCommandGet(&command);
 
-#if defined(DIAGNOSTICS)
+#if defined(MIXERSTATUS_DIAGNOSTICS)
 		MixerStatusGet(&mixerStatus);
 #endif
 		int nMixers = 0;
@@ -362,7 +362,7 @@ static void actuatorTask(void* parameters)
 		// Update in case read only (eg. during servo configuration)
 		ActuatorCommandGet(&command);
 
-#if defined(DIAGNOSTICS)
+#if defined(MIXERSTATUS_DIAGNOSTICS)
 		MixerStatusSet(&mixerStatus);
 #endif
 		
diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c
index fc4a5dcc5..a43162e71 100644
--- a/flight/Modules/Stabilization/stabilization.c
+++ b/flight/Modules/Stabilization/stabilization.c
@@ -119,7 +119,7 @@ int32_t StabilizationInitialize()
 	// Initialize variables
 	StabilizationSettingsInitialize();
 	ActuatorDesiredInitialize();
-#if defined(DIAGNOSTICS)
+#if defined(RATEDESIRED_DIAGNOSTICS)
 	RateDesiredInitialize();
 #endif
 
@@ -172,8 +172,7 @@ static void stabilizationTask(void* parameters)
 		StabilizationDesiredGet(&stabDesired);
 		AttitudeActualGet(&attitudeActual);
 		GyrosGet(&gyrosData);
-		
-#if defined(DIAGNOSTICS)
+#if defined(RATEDESIRED_DIAGNOSTICS)
 		RateDesiredGet(&rateDesired);
 #endif
 		
@@ -350,7 +349,7 @@ static void stabilizationTask(void* parameters)
 		if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
 			stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
 
-#if defined(DIAGNOSTICS)
+#if defined(RATEDESIRED_DIAGNOSTICS)
 		RateDesiredSet(&rateDesired);
 #endif
 
diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c
index 715c26d72..870273a1e 100644
--- a/flight/Modules/System/systemmod.c
+++ b/flight/Modules/System/systemmod.c
@@ -82,7 +82,7 @@ static void objectUpdatedCb(UAVObjEvent * ev);
 static void updateStats();
 static void updateSystemAlarms();
 static void systemTask(void *parameters);
-#if defined(DIAGNOSTICS)
+#if defined(I2C_WDG_STATS_DIAGNOSTICS)
 static void updateI2Cstats();
 static void updateWDGstats();
 #endif
@@ -118,7 +118,7 @@ int32_t SystemModInitialize(void)
 #if defined(DIAG_TASKS)
 	TaskInfoInitialize();
 #endif
-#if defined(DIAGNOSTICS)
+#if defined(I2C_WDG_STATS_DIAGNOSTICS)
 	I2CStatsInitialize();
 	WatchdogStatusInitialize();
 #endif
@@ -168,7 +168,7 @@ static void systemTask(void *parameters)
 
 		// Update the system alarms
 		updateSystemAlarms();
-#if defined(DIAGNOSTICS)
+#if defined(I2C_WDG_STATS_DIAGNOSTICS)
 		updateI2Cstats();
 		updateWDGstats();
 #endif
@@ -301,7 +301,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
 /**
  * Called periodically to update the I2C statistics 
  */
-#if defined(DIAGNOSTICS)
+#if defined(I2C_WDG_STATS_DIAGNOSTICS)
 static void updateI2Cstats() 
 {
 #if defined(PIOS_INCLUDE_I2C)
diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile
index 63f2e7907..07c9e05d4 100644
--- a/flight/PipXtreme/Makefile
+++ b/flight/PipXtreme/Makefile
@@ -38,7 +38,14 @@ OUTDIR := $(TOP)/build/$(TARGET)
 DEBUG ?= NO
 
 # Include objects that are just nice information to show
-DIAGNOSTICS ?= NO
+STACK_DIAGNOSTICS ?= NO
+MIXERSTATUS_DIAGNOSTICS ?= NO
+RATEDESIRED_DIAGNOSTICS ?= NO
+I2C_WDG_STATS_DIAGNOSTICS ?= NO
+DIAG_TASKS ?= YES
+
+#Or just turn on all the above diagnostics. WARNING: This consumes massive amounts of memory.
+ALL_DIGNOSTICS ?=NO
 
 # Set to YES to build a FW version that will erase all flash memory
 ERASE_FLASH ?= NO
@@ -378,8 +385,24 @@ ifeq ($(DEBUG),YES)
 CFLAGS = -DDEBUG
 endif
 
-ifeq ($(DIAGNOSTICS),YES)
-CFLAGS = -DDIAGNOSTICS
+ifneq (,$(filter YES,$(STACK_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DSTACK_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(MIXERSTATUS_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DMIXERSTATUS_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(RATEDESIRED_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DRATEDESIRED_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(I2C_WDG_STATS_DIAGNOSTICS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS
+endif
+
+ifneq (,$(filter YES,$(DIAG_TASKS) $(ALL_DIGNOSTICS)))
+CFLAGS += -DDIAG_TASKS
 endif
 
 CFLAGS += -g$(DEBUGF)
diff --git a/flight/PipXtreme/System/inc/FreeRTOSConfig.h b/flight/PipXtreme/System/inc/FreeRTOSConfig.h
index 994956008..0c515b7f7 100755
--- a/flight/PipXtreme/System/inc/FreeRTOSConfig.h
+++ b/flight/PipXtreme/System/inc/FreeRTOSConfig.h
@@ -76,7 +76,7 @@ NVIC value of 255. */
 #endif
 
 /* Enable run time stats collection */
-#if defined(DIAGNOSTICS)
+#if defined(STACK_DIAGNOSTICS)
 #define configCHECK_FOR_STACK_OVERFLOW	2
 
 #define configGENERATE_RUN_TIME_STATS 1
diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile
index ce8a45b6d..ff76ddbfa 100644
--- a/flight/Revolution/Makefile
+++ b/flight/Revolution/Makefile
@@ -255,7 +255,10 @@ endif
 # common architecture-specific flags from the device-specific library makefile
 CFLAGS += $(ARCHFLAGS)
 
-CFLAGS += -DDIAGNOSTICS
+CFLAGS += -DSTACK_DIAGNOSTICS
+CFLAGS += -DMIXERSTATUS_DIAGNOSTICS
+CFLAGS += -DRATEDESIRED_DIAGNOSTICS
+CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS
 CFLAGS += -DDIAG_TASKS
 
 # This is not the best place for these.  Really should abstract out
diff --git a/flight/SimPosix/Makefile b/flight/SimPosix/Makefile
index 622d7f53c..9e24125ee 100644
--- a/flight/SimPosix/Makefile
+++ b/flight/SimPosix/Makefile
@@ -256,7 +256,11 @@ endif
 CFLAGS += $(ARCHFLAGS)
 
 CFLAGS += $(UAVOBJDEFINE)
-CFLAGS += -DDIAGNOSTICS
+
+CFLAGS += -DSTACK_DIAGNOSTICS
+CFLAGS += -DMIXERSTATUS_DIAGNOSTICS
+CFLAGS += -DRATEDESIRED_DIAGNOSTICS
+CFLAGS += -DI2C_WDG_STATS_DIAGNOSTICS
 CFLAGS += -DDIAG_TASKS
 
 # This is not the best place for these.  Really should abstract out