diff --git a/src/me/drton/jmavsim/MAVLinkHILSystem.java b/src/me/drton/jmavsim/MAVLinkHILSystem.java
index adb29b23..c0cd3057 100644
--- a/src/me/drton/jmavsim/MAVLinkHILSystem.java
+++ b/src/me/drton/jmavsim/MAVLinkHILSystem.java
@@ -126,16 +126,16 @@ public void handleMessage(MAVLinkMessage msg) {
                     if (sysId < 0) {
                         sysId = msg.systemID;
                     }
+
+                    System.out.println("Init MAVLink");
+                    initMavLink();
+
                 } else if (sysId > -1 && sysId != msg.systemID) {
                     System.out.println("WARNING: Got heartbeat from system #" + Integer.toString(msg.systemID) +
                                        " but configured to only accept messages from system #" + Integer.toString(sysId) +
                                        ". Please change the system ID parameter to match in order to use HITL/SITL.");
                 }
             }
-            if (gotHeartBeat) {
-                System.out.println("Init MAVLink");
-                initMavLink();
-            }
             if ((msg.getInt("base_mode") & 128) == 0) {
                 vehicle.setControl(Collections.<Double>emptyList());
             }