Mavlink: bug fixed