Updates and bug fixes 0.3a

Fixes to course change formula
Fixes to speed detection / Tx interval formula
Added timestamp to debugging messages