Added drone status debugging output

Added printf() to prvSendError and prvSendStatus for debugging on
UART/semihosting
Refactoring and cleanup
Merge branch 'master' of https://github.com/estei-master/segment_DRONE