diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
index 4550633116..01c0945820 100644
--- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h
@@ -55,9 +55,10 @@ extern void sys_tick_irq_handler(void);
 
 
 /** Busy wait, in microseconds */
-/* for now empty shell */
 static inline void sys_time_usleep(uint32_t us) {
-
+  uint32_t start = GET_CUR_TIME_USEC();
+  uint32_t ticks = CPU_TICKS_OF_USEC(us);
+  while ((uint32_t)(GET_CUR_TIME_USEC()-start) < ticks);
 }
 
 #endif /* SYS_TIME_ARCH_H */