Minor fixes to MAVLink

Please review - will increase robustness, results from code review.