diff --git a/modules/mavlink b/modules/mavlink
index 153d474..7d3a041 160000
--- a/modules/mavlink
+++ b/modules/mavlink
@@ -1 +1 @@
-Subproject commit 153d474873a461f1554c2c34aaa864f818509eef
+Subproject commit 7d3a04174105c00eb712b6a6ab16452a372e4a93
diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp
index cdd5aca..28fd1d9 100644
--- a/src/marshconnector.cpp
+++ b/src/marshconnector.cpp
@@ -56,6 +56,10 @@ MarshConnector::MarshConnector() {
   // Initialize member variables
   time_passed = 0.0;
 
+  system_id = 1;
+  component_id =
+      MAV_COMP_ID_USER1 + (MARSH_TYPE_VISUALISATION - MARSH_TYPE_MANAGER);
+
   heartbeat_timer = memnew(Timer);
   add_child(heartbeat_timer);
   heartbeat_timer->set_wait_time(1.0);
@@ -121,15 +125,15 @@ Error MarshConnector::send_heartbeat() {
   // print_line("Sending HEARTBEAT at ", time_passed, " seconds");
 
   mavlink_heartbeat_t heartbeat;
-  heartbeat.type = MAV_TYPE_HELICOPTER;
+  heartbeat.type = MARSH_TYPE_VISUALISATION;
   heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
   heartbeat.base_mode = 0;   // none of the flags applicable
   heartbeat.custom_mode = 0; // not used so far
   heartbeat.system_status = MAV_STATE_ACTIVE;
 
   mavlink_message_t message;
-  mavlink_msg_heartbeat_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
-                                    MAVLINK_COMM_0, &message, &heartbeat);
+  mavlink_msg_heartbeat_encode_chan(system_id, component_id, MAVLINK_COMM_0,
+                                    &message, &heartbeat);
   return send_message(message);
 }
 
@@ -336,16 +340,17 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
   if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT)
     return;
 
-  // mavlink_heartbeat_t heartbeat;
-  // mavlink_msg_heartbeat_decode(&message, &heartbeat);
-  if (message.compid == MARSH_COMP_ID_MANAGER) {
+  mavlink_heartbeat_t heartbeat;
+  mavlink_msg_heartbeat_decode(&message, &heartbeat);
+  if (heartbeat.type == MARSH_TYPE_MANAGER) {
     if (!manager_connected) {
       print_line("Connected to MARSH Manager");
 
       // subscribe to messages not sent to visualisation node by default
       mavlink_command_long_t command;
       command.target_system = 1;
-      command.target_component = MARSH_COMP_ID_MANAGER;
+      command.target_component =
+          MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT
       command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
       command.confirmation = 0;
       command.param1 = static_cast<float>(MAVLINK_MSG_ID_MANUAL_CONTROL);
@@ -357,9 +362,8 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
       command.param7 = 1; // Address of requestor
 
       mavlink_message_t message_sent;
-      mavlink_msg_command_long_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
-                                           MAVLINK_COMM_0, &message_sent,
-                                           &command);
+      mavlink_msg_command_long_encode_chan(
+          system_id, component_id, MAVLINK_COMM_0, &message_sent, &command);
       Error result = send_message(message_sent);
       if (result != OK) {
         print_line("Subscribe send result ", result);
diff --git a/src/marshconnector.h b/src/marshconnector.h
index e1a3e78..5f3279d 100644
--- a/src/marshconnector.h
+++ b/src/marshconnector.h
@@ -103,6 +103,9 @@ private:
       "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON",
   };
 
+  uint8_t system_id;
+  uint8_t component_id;
+
   // TODO: Interpolate with some delay
   Vector3 last_location;
   Quaternion last_rotation;