Mavlink improvements