#include <sys/systick.h>
#include <sys/atomic.h>
#include <sys/threads.h>
void wait_ms(uint32_t ms)
{
current_thread()->timeout = _systick_ticks + (systick_t)((systick_t)1000L * (systick_t)ms);
thread_sleep( current_thread() );
};