Fix debug output
Update time measurement to use timer from RIOT