MAVLinkProtocol Unit Test plus other fixes