Parameter Test

正在提取system的各種資訊

mavone/mavone.cpp

@@ -72,6 +72,9 @@ int main(int argc, char** argv)
 
     // Start MAVSDK Server and connect to it.
     Mavsdk mavsdk;
+    // Set this MAVLINK service has system id 197, component id 254, NOT send heartbeat
+    mavsdk::Mavsdk::Configuration mavserver_configuration(197,254,false);
+    mavsdk.set_configuration(mavserver_configuration);
     ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
     if (connection_result != ConnectionResult::Success) {
         std::cerr << "(mavone.cpp:main) Connection failed: " << connection_result << '\n';
@@ -251,6 +254,10 @@ int main(int argc, char** argv)
                     // 更新 mavsys_connect_status 欄位
                     sqlString = "UPDATE NodeRed_one SET mavsys_connect_status = 'Disconnected' WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
                     stmt->execute(sqlString);
+
+                    // 變更狀態
+                    handlerInfo.handlerState = systemHandlerState::Terminate;
+
                     // reset = true; //debug
                     break;
             }
@@ -259,7 +266,7 @@ int main(int argc, char** argv)
 
         // 刪除 Disconnected 的 handlerInfo
         for (auto it = systemHandlerInfos.begin(); it != systemHandlerInfos.end(); /* no increment here */) {
-            if (it->handlerState == systemHandlerState::Disconnected) {
+            if (it->handlerState == systemHandlerState::Terminate) {
                 // 這個 if 如果不加 則副程序在結束以前就會被主程序放生 執行時會出現 SIGABRT 錯誤
                 if (it->systemThread.joinable()) {
                     it->systemThread.join();

mavone/mavone.h

@@ -47,6 +47,7 @@ enum class systemHandlerState {
     Prologue,                   // 建立了 mysql row 並取得 serialNo
     Ready,                      // 收到第一筆並放到 telemetryInfo 後
     Disconnected,               // mavsdk 判斷該系統斷線了
+    Terminate,                  // 準備要從管理列表中刪除
     // Void
 };
 

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;