mavlink: HIL fixes, performance optimization