diff --git a/Mavlink/MAVLinkInterface.cs b/Mavlink/MAVLinkInterface.cs
index be3ab63141..fa42b00eb9 100644
--- a/Mavlink/MAVLinkInterface.cs
+++ b/Mavlink/MAVLinkInterface.cs
@@ -3039,18 +3039,23 @@ public void setGuidedModeWP(byte sysid, byte compid, Locationwp gotohere, bool s
                 if (setguidedmode)
                 {
                     // fix for followme change
-                    if (MAVlist[sysid,compid].cs.mode.ToUpper() != "GUIDED")
+                    if (MAVlist[sysid, compid].cs.mode.ToUpper() != "GUIDED")
                         setMode(sysid, compid, "GUIDED");
                 }
 
-                MainV2.comPort.setPositionTargetGlobalInt((byte)sysidcurrent, (byte)compidcurrent,
-                    true, false, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT,
-                    gotohere.lat, gotohere.lng, MainV2.comPort.MAV.GuidedMode.z, 0, 0, 0, 0, 0);
-
-                //MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, MAV_FRAME.GLOBAL_RELATIVE_ALT,(byte) 2);
+                if (MAVlist[sysid,compid].cs.firmware == MainV2.Firmwares.ArduPlane)
+                {
+                    MAV_MISSION_RESULT ans = setWP(sysid, compid, gotohere, 0, MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2);
 
-                //if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
-                    //throw new Exception("Guided Mode Failed");
+                    if (ans != MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
+                        throw new Exception("Guided Mode Failed");
+                }
+                else
+                {
+                    setPositionTargetGlobalInt((byte)sysid, (byte)compid,
+                        true, false, false, false, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT_INT,
+                        gotohere.lat, gotohere.lng, MainV2.comPort.MAV.GuidedMode.z, 0, 0, 0, 0, 0);
+                }
             }
             catch (Exception ex)
             {
diff --git a/MissionPlanner.csproj b/MissionPlanner.csproj
index 3e6357a0a4..7b66c42839 100644
--- a/MissionPlanner.csproj
+++ b/MissionPlanner.csproj
@@ -578,6 +578,12 @@
     <Content Include="Scripts\example1.py">
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>
+    <None Update="Drivers\cube.cat">
+      <CopyToOutputDirectory>Always</CopyToOutputDirectory>
+    </None>
+    <None Update="Drivers\Cube.inf">
+      <CopyToOutputDirectory>Always</CopyToOutputDirectory>
+    </None>
     <None Update="Drivers\signed.cer">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>
     </None>