Fixed MavLink message bugs