Hitl fix v2

Attempt to refix HITL without the segfault :)  Patch from Guilhermito included https://github.com/peabody124/TauLabs/pull/3