Parameter Test

正在提取system的各種資訊

mavone/systemHandler.cpp

@@ -1,7 +1,7 @@
 
-
-
 #include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/info/info.h>
+#include <mavsdk/plugins/param/param.h>
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 #include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
@@ -34,6 +34,7 @@ void systemHandler(System& system) {
     // Initialization
     auto telemetry = Telemetry{system};
     auto action = Action{system};
+    auto param = Param{system};
     int sysid = static_cast<int>(system.get_system_id());
     std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug
 
@@ -48,15 +49,96 @@ void systemHandler(System& system) {
     //
     int _counter;
 
+
+
+    //
+    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';
+
+    // auto mavlink_passthrough = MavlinkPassthrough{system};
+    // mavlink_command_long_t command;
+    // command.target_system = sysid;
+    // command.target_component = MAV_COMP_ID_AUTOPILOT1;
+    // command.command = MAV_CMD_REQUEST_MESSAGE;
+    // command.param1 = 1;
+    // mavlink_message_t message;
+    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
+    //                                 mavlink_passthrough.get_our_compid(),
+    //                                 &message,
+    //                                 &command);
+    // auto result_ackFromSendMessage = mavlink_passthrough.send_message(message);
+
+    // // 演示一下 如何抓到 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;
+
+    // std::pair<Param::Result, float> get_result_f = {Param::Result::Unknown , 0};
+    // get_result_f = param.get_param_float("RNGFND_FILT"); // This function is blocking.
+    // std::cout << "Para Read: " << get_result_f.first << " : " << get_result_f.second << std::endl;
+
+    // auto get_result = param.get_all_params(); // This function is blocking.
+    // auto int_p = get_result.int_params;
+    // for(const auto& item : int_p) {
+    //     std::cout << item << std::endl;
+    // }
+
+    // // 抓一下 health
+    // auto health = telemetry.health();
+    // std::cout << "GYRO: " << (health.is_gyrometer_calibration_ok ? "OK" : "NG") << std::endl;
+    // std::cout << "Accelerometer: " << (health.is_accelerometer_calibration_ok ? "OK" : "NG") << std::endl;
+    // std::cout << "Magnetometer: " << (health.is_magnetometer_calibration_ok ? "OK" : "NG") << std::endl;
+    // std::cout << "Local position NED: " << (health.is_local_position_ok ? "OK" : "NG") << std::endl;
+    // std::cout << "Global position (Int): " << (health.is_global_position_ok ? "OK" : "NG") << std::endl;
+    // std::cout << "Home position: " << (health.is_home_position_ok ? "OK" : "NG") << std::endl;
+
+    
+    // // 如果對接另一邊也是 mavsdk 這幾個 Parameter 要 provide，這邊讀看看
+    // get_result_i = {Param::Result::Unknown , 0};
+    // get_result_i = param.get_param_int("SYS_HITL"); // This function is blocking.
+    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
+    // get_result_i = {Param::Result::Unknown , 0};
+    // get_result_i = param.get_param_int("CAL_GYRO0_ID"); // This function is blocking.
+    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
+    // get_result_i = {Param::Result::Unknown , 0};
+    // get_result_i = param.get_param_int("CAL_ACC0_ID"); // This function is blocking.
+    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
+    // get_result_i = {Param::Result::Unknown , 0};
+    // get_result_i = param.get_param_int("CAL_MAG0_ID"); // This function is blocking.
+    // std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
+
+    // // 看看 has_autopilot() 的值
     // std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
 
+
+    // std::cout << "Dev Test End" << std::endl;
+
     // Put all the subscriber init in this section
     if (system.has_autopilot()) {
 
         auto set_rate_result = telemetry.set_rate_position(1.0);
-        if (set_rate_result != mavsdk::Telemetry::Result::Success) {
-            std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
-        }
+        // if (set_rate_result != mavsdk::Telemetry::Result::Success) {
+        //     std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
+        // }
         telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
             // 轉換數字到字串用的暫存變數
             std::ostringstream num_str_ss;