diff --git a/src/model/control/template_controller/lib/Controller.h b/src/model/control/template_controller/lib/Controller.h
index c7cf50769..7db135395 100644
--- a/src/model/control/template_controller/lib/Controller.h
+++ b/src/model/control/template_controller/lib/Controller.h
@@ -156,6 +156,9 @@ typedef struct {
 
   /* enum of PilotMode */
   uint8_T reserved;
+
+  /* home position [x y h yaw], unit [m m m rad] */
+  real32_T home[4];
 } FMS_Out_Bus;
 
 #endif
@@ -185,6 +188,8 @@ typedef struct {
   real_T lat_0;
   real_T lon_0;
   real_T alt_0;
+  real_T dx_dlat;
+  real_T dy_dlon;
   real32_T x_R;
   real32_T y_R;
   real32_T h_R;
diff --git a/src/model/fms/template_fms/lib/FMS.h b/src/model/fms/template_fms/lib/FMS.h
index d0571a175..f6142ead8 100644
--- a/src/model/fms/template_fms/lib/FMS.h
+++ b/src/model/fms/template_fms/lib/FMS.h
@@ -64,6 +64,7 @@ typedef struct {
 
   /* Operation channel 2 */
   uint32_T cmd_2;
+  real32_T param[7];
 } GCS_Cmd_Bus;
 
 #endif
@@ -198,6 +199,8 @@ typedef struct {
   real_T lat_0;
   real_T lon_0;
   real_T alt_0;
+  real_T dx_dlat;
+  real_T dy_dlon;
   real32_T x_R;
   real32_T y_R;
   real32_T h_R;
@@ -231,7 +234,8 @@ typedef enum {
   FMS_Cmd_Land,
   FMS_Cmd_Return,
   FMS_Cmd_Pause,
-  FMS_Cmd_Continue
+  FMS_Cmd_Continue,
+  FMS_Cmd_SetHome = 2000
 } FMS_Cmd;
 
 #endif
@@ -385,6 +389,9 @@ typedef struct {
 
   /* enum of PilotMode */
   uint8_T reserved;
+
+  /* home position [x y h yaw], unit [m m m rad] */
+  real32_T home[4];
 } FMS_Out_Bus;
 
 #endif
diff --git a/src/model/ins/external_ins/lib/INS.h b/src/model/ins/external_ins/lib/INS.h
index d1fb0764b..761978e4b 100644
--- a/src/model/ins/external_ins/lib/INS.h
+++ b/src/model/ins/external_ins/lib/INS.h
@@ -135,6 +135,22 @@ typedef struct {
 
 #endif
 
+#ifndef DEFINED_TYPEDEF_FOR_External_Pos_Bus_
+#define DEFINED_TYPEDEF_FOR_External_Pos_Bus_
+
+typedef struct {
+  uint32_T timestamp;
+  uint32_T field_valid;
+  real32_T x;
+  real32_T y;
+  real32_T z;
+  real32_T phi;
+  real32_T theta;
+  real32_T psi;
+} External_Pos_Bus;
+
+#endif
+
 #ifndef DEFINED_TYPEDEF_FOR_INS_Out_Bus_
 #define DEFINED_TYPEDEF_FOR_INS_Out_Bus_
 
@@ -160,6 +176,8 @@ typedef struct {
   real_T lat_0;
   real_T lon_0;
   real_T alt_0;
+  real_T dx_dlat;
+  real_T dy_dlon;
   real32_T x_R;
   real32_T y_R;
   real32_T h_R;
@@ -169,7 +187,6 @@ typedef struct {
 } INS_Out_Bus;
 
 #endif
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/model/ins/template_ins/ins_interface.c b/src/model/ins/template_ins/ins_interface.c
index e549e4402..b7830ccd2 100644
--- a/src/model/ins/template_ins/ins_interface.c
+++ b/src/model/ins/template_ins/ins_interface.c
@@ -17,8 +17,11 @@
 #include <INS.h>
 #include <firmament.h>
 
+/* External Position */
+MCN_DEFINE(external_pos, sizeof(External_Pos_Bus));
 /* INS output bus */
 MCN_DEFINE(ins_output, sizeof(INS_Out_Bus));
+
 /* Model information */
 fmt_model_info_t ins_model_info;
 
@@ -35,6 +38,7 @@ void ins_interface_init(void)
 
     /* advertise ins_output topic */
     mcn_advertise(MCN_HUB(ins_output), NULL);
+    mcn_advertise(MCN_HUB(external_pos), NULL);
 
     /* Add your code here */
 }
diff --git a/src/model/ins/template_ins/lib/INS.h b/src/model/ins/template_ins/lib/INS.h
index d1fb0764b..5899198d6 100644
--- a/src/model/ins/template_ins/lib/INS.h
+++ b/src/model/ins/template_ins/lib/INS.h
@@ -135,6 +135,22 @@ typedef struct {
 
 #endif
 
+#ifndef DEFINED_TYPEDEF_FOR_External_Pos_Bus_
+#define DEFINED_TYPEDEF_FOR_External_Pos_Bus_
+
+typedef struct {
+  uint32_T timestamp;
+  uint32_T field_valid;
+  real32_T x;
+  real32_T y;
+  real32_T z;
+  real32_T phi;
+  real32_T theta;
+  real32_T psi;
+} External_Pos_Bus;
+
+#endif
+
 #ifndef DEFINED_TYPEDEF_FOR_INS_Out_Bus_
 #define DEFINED_TYPEDEF_FOR_INS_Out_Bus_
 
@@ -160,6 +176,8 @@ typedef struct {
   real_T lat_0;
   real_T lon_0;
   real_T alt_0;
+  real_T dx_dlat;
+  real_T dy_dlon;
   real32_T x_R;
   real32_T y_R;
   real32_T h_R;
diff --git a/src/model/plant/template_plant/lib/Plant.h b/src/model/plant/template_plant/lib/Plant.h
index a7a4ede48..76e0e446d 100644
--- a/src/model/plant/template_plant/lib/Plant.h
+++ b/src/model/plant/template_plant/lib/Plant.h
@@ -24,6 +24,31 @@
 extern "C" {
 #endif
 
+#ifndef DEFINED_TYPEDEF_FOR_Environment_Info_Bus_
+#define DEFINED_TYPEDEF_FOR_Environment_Info_Bus_
+
+/* Collision Information */
+typedef struct {
+  uint32_T timestamp;
+
+  /* Hit point in NED frame */
+  real32_T hit_point[3];
+
+  /* Hit point normal */
+  real32_T hit_normal[3];
+
+  /* Hit location in NED frame */
+  real32_T hit_location[3];
+
+  /* External torque applied at body in body frame */
+  real32_T torque[3];
+
+  /* External force applied at body in body frame. */
+  real32_T force[3];
+} Environment_Info_Bus;
+
+#endif
+
 #ifndef DEFINED_TYPEDEF_FOR_Control_Out_Bus_
     #define DEFINED_TYPEDEF_FOR_Control_Out_Bus_
 
diff --git a/src/task/comm/task_comm.c b/src/task/comm/task_comm.c
index d3eb75921..32a4c1291 100644
--- a/src/task/comm/task_comm.c
+++ b/src/task/comm/task_comm.c
@@ -139,7 +139,7 @@ static uint32_t get_custom_mode(FMS_Out_Bus fms_out)
 bool mavlink_msg_heartbeat_pack_func(mavlink_message_t* msg_t)
 {
     mavlink_heartbeat_t heartbeat = { 0 };
-    FMS_Out_Bus         fms_out;
+    FMS_Out_Bus         fms_out   = { 0 };
 
     heartbeat.type          = MAV_TYPE_QUADROTOR;
     heartbeat.autopilot     = MAV_AUTOPILOT_PX4;
@@ -147,14 +147,13 @@ bool mavlink_msg_heartbeat_pack_func(mavlink_message_t* msg_t)
     heartbeat.custom_mode   = 0;
     heartbeat.system_status = MAV_STATE_STANDBY;
 
-    if (mcn_copy_from_hub(MCN_HUB(fms_output), &fms_out) != FMT_EOK) {
-        return false;
+    if (mcn_copy_from_hub(MCN_HUB(fms_output), &fms_out) == FMT_EOK) {
+        if (fms_out.status == VehicleStatus_Arm || fms_out.status == VehicleStatus_Standby) {
+            heartbeat.base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+            heartbeat.system_status = MAV_STATE_ACTIVE;
+        }
     }
 
-    if (fms_out.status == VehicleStatus_Arm || fms_out.status == VehicleStatus_Standby) {
-        heartbeat.base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-        heartbeat.system_status = MAV_STATE_ACTIVE;
-    }
     /* map fms mode to px4 ctrl mode */
     heartbeat.custom_mode = get_custom_mode(fms_out);