From 2dbfc6158db9a73f2b22091d6923a60e71aa389f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 27 Sep 2014 11:44:17 +0900
Subject: [PATCH] Rover: check target of set-mode request from GCS

Issue discovered and fix contributed by Deadolous
---
 APMrover2/GCS_Mavlink.pde | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 65fcdca8dd..4d4d85d722 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -918,6 +918,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_set_mode_t packet;
             mavlink_msg_set_mode_decode(msg, &packet);
 
+            // exit immediately if this command is not meant for this vehicle
+            if (mavlink_check_target(packet.target_system, 0)) {
+                break;
+            }
+
             if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
                 // we ignore base_mode as there is no sane way to map
                 // from that bitmap to a APM flight mode. We rely on
@@ -1025,6 +1030,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         int16_t v[8];
         mavlink_msg_rc_channels_override_decode(msg, &packet);
 
+        // exit immediately if this command is not meant for this vehicle
         if (mavlink_check_target(packet.target_system,packet.target_component))
             break;