diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp
index 4dd0cf6..29bd7d3 100644
--- a/mavone/systemHandler.cpp
+++ b/mavone/systemHandler.cpp
@@ -84,69 +84,7 @@ void systemHandler(System& system) {
     double _target_lon;
     double _target_lat;
 
-    //
-    std::cout << "Dev Test Start" << std::endl;
-
-    // Get the info plugin for this drone
-    auto atvData = system.get_autopilot_version_data();
-    std::cout << "Autopilot Version Data:" << std::endl;
-    std::cout << "  Vendor ID: " << atvData.vendor_id << std::endl;
-    std::cout << "  Product ID: " << atvData.product_id << std::endl;
-    std::cout << "  Capabilities: " << atvData.capabilities << std::endl;
-    std::cout << "  Flight SW Version: " << atvData.flight_sw_version << std::endl;
-    std::cout << "  Middleware SW Version: " << atvData.middleware_sw_version << std::endl;
-    std::cout << "  OS SW Version: " << atvData.os_sw_version << std::endl;
-    std::cout << "  Board Version: " << atvData.board_version << std::endl;
-
-    std::array<uint8_t, 18> uid = atvData.uid2;
-    std::cout << "UID: ";
-    for (const auto& byte : uid) {
-        std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
-    }
-    std::cout << std::endl;
-
-    const Telemetry::FlightMode mode_result = telemetry.flight_mode();
-    std::cerr << "Vehicle Mode: " << mode_result << '\n';
-
-    sleep_for(milliseconds(1500));
-
-
-
-    // action.set_takeoff_altitude(10);
-    // std::cout << "ARM...\n";
-    // action.arm();
-    // sleep_for(seconds(1));
-
-    // std::cout << "Taking off...\n";
-    // action.takeoff();
-    // sleep_for(seconds(10));
-
-    // std::cout << "GoTo...\n";
-    // // auto result_ackFromAction = action.goto_location(24.119289235158828, 120.67216137689361,30,180);
-
-    // 測試GOTO
-    // mavlink_message_t message;
-    // MavlinkPassthrough::CommandLong command;
-    // command.target_sysid = sysid;
-    // command.target_compid = MAV_COMP_ID_AUTOPILOT1;  // 這行指 component id = 1
-    // command.command = MAV_CMD_DO_REPOSITION;
-    // command.param1 = 5; // Speed
-    // command.param2 = 0; // IF change to GUILDED mode ?
-    // command.param3 = 0; // Radius
-    // command.param4 = 0; // Yaw
-    // command.param5 = 24.119289235158828; // Latitude
-    // command.param6 = 120.67216137689361; // Longitude
-    // command.param5 = 24.11928923; // Latitude
-    // command.param6 = 120.67216137; // Longitude
-    // command.param7 = 25; // Altitude
-    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
-    //                                 mavlink_passthrough.get_our_compid(),
-    //                                 &message,
-    //                                 &command);
-    // mavlink_passthrough.send_command_long(command);
-    // auto result_ackFromSendMessage = mavlink_passthrough.send_message(message);
-    // std::cout << "  ACK REL: " << result_ackFromSendMessage << std::endl;
-
+    // std::cout << "Dev Test Start" << std::endl;
     // // 演示一下 如何抓到 parameter
     // auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking.
     // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
@@ -188,8 +126,7 @@ void systemHandler(System& system) {
     // // 看看 has_autopilot() 的值
     // std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
 
-
-    std::cout << "Dev Test End" << std::endl;
+    // std::cout << "Dev Test End" << std::endl;
 
     // Put all the subscriber init in this section
     if (system.has_autopilot()) {
@@ -339,10 +276,10 @@ void systemHandler(System& system) {
                             << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                     #ifdef VEHICLE_CONTROL_ACTIVE
                     mav_command_long.target_sysid = sysid;
-                    mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1;  // 這行指 component id = 1
-                    mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE
-                    mav_command_long.param1 = 1; // 固定是 1
-                    mav_command_long.param2 = static_cast<int>(oneVehicleCommand.second[0]); // mode
+                    mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1;                    // 這行指 component id = 1
+                    mav_command_long.command = 176;                                             // MAV_CMD_DO_SET_MODE
+                    mav_command_long.param1 = 1;                                                // 固定是 1
+                    mav_command_long.param2 = static_cast<int>(oneVehicleCommand.second[0]);    // mode
                     
                     result_mavpass = mavlink_passthrough.send_command_long(mav_command_long); // 送出指令
                     std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
@@ -393,11 +330,8 @@ void systemHandler(System& system) {
                                                         mavlink_passthrough.get_our_compid(),
                                                         &mav_message,
                                                         &mav_command_int);
-                        // mavlink_passthrough.send_message(mav_message);
                         result_mavpass = mavlink_passthrough.send_message(mav_message);
                         std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
-                        std::cout << "x: " << std::to_string(_target_lon) << std::endl; // dev
-                        std::cout << "y: " << std::to_string(_target_lat) << std::endl; // dev
                     }
                     #endif
                     break;
@@ -419,7 +353,6 @@ void systemHandler(System& system) {
                                                     mavlink_passthrough.get_our_compid(),
                                                     &mav_message,
                                                     &mav_command_int);
-                    // mavlink_passthrough.send_message(mav_message);
                     result_mavpass = mavlink_passthrough.send_message(mav_message);
                     _target_lon = oneVehicleCommand.second[1];
                     _target_lat = oneVehicleCommand.second[0];
@@ -429,14 +362,11 @@ void systemHandler(System& system) {
             }
         }
 
-
         // for purpose
         sleep_for(milliseconds(100));
 
     }
 
-
-
     // Thread Terminate
     std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug