MAVLink bugfixes
MAVLink bugfixes