MAVLinkSystem, MAVLinkHILSystem: bugs fixed