diff --git a/core/sys/energest.h b/core/sys/energest.h
index 6d43aaa11..a8f445b65 100644
--- a/core/sys/energest.h
+++ b/core/sys/energest.h
@@ -105,6 +105,21 @@ extern energest_t energest_leveldevice_current_leveltime[ENERGEST_CONF_LEVELDEVI
 										energest_current_time[type]); \
 										energest_current_mode[type] = 0; \
                                        } while(0)
+
+#define ENERGEST_SWITCH(type_off, type_on) do { \
+                                             rtimer_clock_t energest_local_variable_now = RTIMER_NOW(); \
+                                             if(energest_current_mode[type_off] != 0) { \
+                                               if (energest_local_variable_now < energest_current_time[type_off]) { \
+                                                 energest_total_time[type_off].current += RTIMER_ARCH_SECOND; \
+                                               } \
+                                               energest_total_time[type_off].current += (rtimer_clock_t)(energest_local_variable_now - \
+                                                 energest_current_time[type_off]); \
+                                               energest_current_mode[type_off] = 0; \
+                                             } \
+                                             energest_current_time[type_on] = energest_local_variable_now; \
+                                             energest_current_mode[type_on] = 1; \
+                                           } while(0)
+
 #else
 #define ENERGEST_OFF(type) if(energest_current_mode[type] != 0) do {	\
                            energest_total_time[type].current += (rtimer_clock_t)(RTIMER_NOW() - \
@@ -117,13 +132,24 @@ extern energest_t energest_leveldevice_current_leveltime[ENERGEST_CONF_LEVELDEVI
 			                energest_current_time[type]); \
 			   energest_current_mode[type] = 0; \
                                         } while(0)
-#endif
 
+#define ENERGEST_SWITCH(type_off, type_on) do { \
+                                             rtimer_clock_t energest_local_variable_now = RTIMER_NOW(); \
+                                             if(energest_current_mode[type_off] != 0) { \
+                                               energest_total_time[type_off].current += (rtimer_clock_t)(energest_local_variable_now - \
+                                                 energest_current_time[type_off]); \
+                                               energest_current_mode[type_off] = 0; \
+                                             } \
+                                             energest_current_time[type_on] = energest_local_variable_now; \
+                                             energest_current_mode[type_on] = 1; \
+                                           } while(0)
+#endif
 
 #else /* ENERGEST_CONF_ON */
 #define ENERGEST_ON(type) do { } while(0)
 #define ENERGEST_OFF(type) do { } while(0)
 #define ENERGEST_OFF_LEVEL(type,level) do { } while(0)
+#define ENERGEST_SWITCH(type_off, type_on) do { } while(0)
 #endif /* ENERGEST_CONF_ON */
 
 #endif /* ENERGEST_H_ */