結構調整

結構調整以及增加telemetry控制

CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(mavone)
+
+add_executable(mavone
+    mavone.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(mavone
+    MAVSDK::mavsdk
+    pthread
+)
+
+if(NOT MSVC)
+    add_compile_options(mavone PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(mavone PRIVATE -WX -W2)
+endif()

mavone.cpp

@@ -16,6 +16,10 @@
 #include <thread>
 #include <vector>
 
+using namespace mavsdk;
+using std::chrono::seconds;
+using std::this_thread::sleep_for;
+
 struct MavInitParameter {
     std::string connectPort;
     std::string connectPort2;
@@ -23,15 +27,18 @@ struct MavInitParameter {
     float C;
 };
 
-using namespace mavsdk;
+struct systemHandlerInfo {
-using std::chrono::seconds;
+    int systemID;
-using std::this_thread::sleep_for;
+    std::thread systemThread;
     
-void initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
+
+};
+
+
+int initializeParameters(const std::string& filePath, MavInitParameter& initSetting) {
     std::ifstream configFile(filePath);
     if (!configFile) {
-        std::cerr << "Failed to open config file: " << filePath << std::endl;
+        return 1;
-        return;
     }
     
     std::vector<std::string> parameters; // Use a vector to store the parameters
@@ -65,191 +72,354 @@ void initializeParameters(const std::string& filePath, MavInitParameter& initSet
     for (const auto& parameter : parameters) {
         std::cout << "Line: " << parameter << std::endl;
     }
+    return 0;
 }
 
-std::shared_ptr<System> get_system(Mavsdk& mavsdk)
+/*
-{
+ * Important multi-threaded functions, 
-    std::cout << "Waiting to discover system...\n";
+ * handling system initialization, receiving messages and sending messages.
-    auto prom = std::promise<std::shared_ptr<System>>{};
+ */
-    auto fut = prom.get_future();
  
-    // We wait for new systems to be discovered, once we find one that has an
-    // autopilot, we decide to use it.
-    mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
-        auto system = mavsdk.systems().back();
 
-        if (system->has_autopilot()) {
-            std::cout << "Discovered autopilot\n";
 
-            // Unsubscribe again as we only want to find one system.
+void systemHandler(System& system){
-            // mavsdk.subscribe_on_new_system(nullptr);
+
-            prom.set_value(system);
+    // Initialization
-        }
+    auto telemetry = Telemetry{system};
+    auto action = Action{system};
+    std::cout << "System " << static_cast<int>(system.get_system_id()) << " get in Thread.(debug)" << std::endl; // debug
+    telemetry.subscribe_position([&system](Telemetry::Position position) {
+        std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
+        // Feed To mysql
     });
 
-    // We usually receive heartbeats at 1Hz, therefore we should find a
-    // system after around 3 seconds max, surely.
-    if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
-        std::cerr << "No autopilot found.\n";
-        return {};
-    }
 
-    // Get discovered system now.
+    sleep_for(seconds(5)); // debug
-    return fut.get();
+
+    std::cout << "Thread Out " << static_cast<int>(system.get_system_id()) << std::endl;
 }
 
+
 int main(int argc, char** argv)
 {
+
     if (argc != 2) {
-        std::cout << "Config File Needed!" << std::endl;
+        std::cout << "Config File Needed! ex. config.txt" << std::endl;
         return 1;
     }
 
-    // std::string configFilePath = "config.txt";
+    // Program Initial Read Setting from config file
     struct MavInitParameter initSetting;
-    initializeParameters(argv[1], initSetting);
+    auto i = initializeParameters(argv[1], initSetting);
+    if (i == 1){
+        std::cerr << "Failed to open config file: " << argv[1] << std::endl;
+    }
 
+    // Start MAVSDK Server and connect to it.
     Mavsdk mavsdk;
     ConnectionResult connection_result = mavsdk.add_any_connection(initSetting.connectPort);
-
-    // mavsdk.subscribe_on_new_system([]() {
-    //     std::cout << "Discovered new system\n";
-    // });
-
-    // sleep_for(seconds(3));
-
     if (connection_result != ConnectionResult::Success) {
         std::cerr << "Connection failed: " << connection_result << '\n';
         return 1;
     }
 
-    auto system = get_system(mavsdk);
+    // For store Discover systems in a list
-    if (!system) {
+    std::vector<systemHandlerInfo> systemHandlerInfos;
-        return 1;
-    }
 
-    std::cout << "MarkA \n";
+    // Subscribe to new system discovery
-
+    mavsdk.subscribe_on_new_system([&mavsdk, &systemHandlerInfos]() {
-    for(int i = 0; i < 10; ++i) {
+        // Get the last subscribed system 
-        sleep_for(seconds(5));
+        auto comingSystems = mavsdk.systems();
-
+        std::shared_ptr<System> sys = comingSystems.back();
-        for (auto system : mavsdk.systems()) {
+        // Avoid Duplicate System ID (Component ID is careless, maybe determind by UUID will be better?)
-            std::cout << "Found system with system ID: " << static_cast<int>(system->get_system_id())
+        for(int i = 0;i < systemHandlerInfos.size();i++) {
-              << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
+            if(sys->get_system_id() == systemHandlerInfos[i].systemID){
+                comingSystems.pop_back();
+                if(comingSystems.size() != 0){
+                    sys = comingSystems.back();
+                } else {
+                    std::cout << "Duplicate System ID Collision : " << static_cast<int>(sys->get_system_id()) << std::endl;
+                    return;
+                }
+            }
         }
+        // make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
+        System& new_system = *sys;
+        std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug
 
-        std::cout << "link check " << i << " time. \n";
+        // 
-        }
+        // std::map<string, float> telemetryInfo;
+        auto telemetryInfo_pro = std::promise<std::map<string, float>>{};
+        auto telemetryInfo_fut = telemetryInfo_pro.get_future();
 
+        // Let handler progrem deal with System  
+        std::thread systemHandleThread([&new_system]() {systemHandler(new_system);});
 
-    return 1;
+        systemHandlerInfos.push_back(systemHandlerInfo{
-
+            static_cast<int>(new_system.get_system_id()),
-
+            std::move(systemHandleThread)
-
+        });
-    // Instantiate plugins.
-    auto telemetry = Telemetry{system};
-    auto action = Action{system};
-
-    // Instantiate mavlink_passthrough plugin.
-    auto mavlink_passthrough = MavlinkPassthrough{system};
-
-    auto x = static_cast<int>(system->get_system_id());
-    std::cout << "System ID : " << x << '\n';
-
-    System::AutopilotVersion y = system->get_autopilot_version_data();
-    std::cout << "MAV_PROTOCOL_CAPABILITY : " << y.capabilities << '\n';
-    std::cout << "Flight Stack Version : " << y.flight_sw_version << '\n';
-
-    // Check vehicle mode
-    const Telemetry::FlightMode mode_result = telemetry.flight_mode();
-    std::cerr << "Vehicle Mode: " << mode_result << '\n';
-    // return 1; // debug
-
-    // Set the flight mode to "AUTO". use SET_MODE (#11) in MAVLink Messages.
-    mavlink_message_t message;
-
-
-    // mavlink_msg_set_mode_pack(mavlink_passthrough.get_our_sysid(),
-    //                           mavlink_passthrough.get_our_compid(),
-    //                           &message,
-    //                           system->get_system_id(),
-    //                           MAV_COMP_ID_AUTOPILOT1,
-    //                           4);
-
-    mavlink_command_long_t command;
-    command.target_system = system->get_system_id();
-    command.target_component = MAV_COMP_ID_AUTOPILOT1;
-    command.command = MAV_CMD_DO_SET_MODE;
-    command.param1 = MAV_MODE_AUTO_ARMED;
-    mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
-                                    mavlink_passthrough.get_our_compid(),
-                                    &message,
-                                    &command);
-
-    mavlink_passthrough.send_message(message);
-
-
-    return 1; // debug
-
-    // We want to listen to the altitude of the drone at 1 Hz.
-    const auto set_rate_result = telemetry.set_rate_position(0.2);
-    if (set_rate_result != Telemetry::Result::Success) {
-        std::cerr << "Setting rate failed: " << set_rate_result << '\n';
-        return 1;
-    }
-
-    // Set up callback to monitor altitude while the vehicle is in flight
-    telemetry.subscribe_position([](Telemetry::Position position) {
-        std::cout << "Altitude: " << position.relative_altitude_m << " m\n";
     });
 
-    // Check until vehicle is ready to arm
+    // MySQL connection is here
-    while (telemetry.health_all_ok() != true) {
-        std::cout << "Vehicle is getting ready to arm\n";
-        sleep_for(seconds(1));
-    }
 
-    // Arm vehicle
-    std::cout << "Arming...\n";
-    const Action::Result arm_result = action.arm();
 
-    if (arm_result != Action::Result::Success) {
+    // Main Loop is here
-        std::cerr << "Arming failed: " << arm_result << '\n';
+    //   -- 將這個服務目前的狀態 更新到 mysql 中
-        return 1;
+    //   -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
-    }
+    //   -- 從 mysql 抓到指令 丟到 systemHandlerInfo
-
-    return 1; // debug
 
 
 
-    // Take off
+
-    std::cout << "Taking off...\n";
+
-    const Action::Result takeoff_result = action.takeoff();
+
-    if (takeoff_result != Action::Result::Success) {
+// ==============================================================================
-        std::cerr << "Takeoff failed: " << takeoff_result << '\n';
+
-        return 1;
+    // mavsdk.subscribe_on_new_system([&mavsdk]() {
-    }
+    //     auto system = mavsdk.systems().back();
+
+
+    //     if (system->has_autopilot()) {
+    //         std::cout << "Discovered autopilot\n";
+    //     }
+    // });
+
+
+
+    
+
+    // if (connection_result != ConnectionResult::Success) {
+    //     std::cerr << "Connection failed: " << connection_result << '\n';
+    //     return 1;
+    // }
+
+    // auto system = get_system(mavsdk);
+    // if (!system) {
+    //     return 1;
+    // }
+
+
+
+    // for (auto system : mavsdk.systems()) {
+    //     std::cout << "(mavsdk.systems) get id : " << static_cast<int>(system->get_system_id()) << std::endl;
+    //     for (auto systemHandler : systemHandlers) {
+    //         std::cout << systemHandler.systemID << " Mark B" << std::endl;
+    //         if (systemHandler.systemID == static_cast<int>(system->get_system_id())) {
+    //             std::cout << "Mark C" << std::endl;
+    //             // telemetry = Telemetry{system};
+    //             const auto set_rate_result = telemetry.set_rate_position(1);
+    //             if (set_rate_result != Telemetry::Result::Success) {
+    //                 std::cerr << "Setting rate failed: " << set_rate_result << '\n';
+    //                 return 1;
+    //             }
+    //             telemetry.subscribe_position([systemHandler](Telemetry::Position position) {
+    //                 std::cout << "System ID: " << systemHandler.systemID << " Altitude: " << position.relative_altitude_m << " m\n";
+    //             });
+
+    //             // // Check until vehicle is ready to arm
+    //             // while (telemetry.health_all_ok() != true) {
+    //             //     std::cout << "System ID: " << systemHandler.systemID << " Vehicle is getting ready to arm\n";
+    //             //     sleep_for(seconds(1));
+    //             // }
+
+    //             // // Arm vehicle
+    //             // std::cout << "System ID: " << systemHandler.systemID << " Arming...\n";
+    //             // const Action::Result arm_result = action.arm();
+
+    //             // if (arm_result != Action::Result::Success) {
+    //             //     std::cerr << "System ID: " << systemHandler.systemID << " Arming failed: " << arm_result << '\n';
+    //             //     return 1;
+    //             // }
+    //             // telemetrys.push_back(telemetry);
+    //             sleep_for(seconds(5));
+    //         } else {
+    //             std::cout << " Mark D " << std::endl;
+    //         }
+    //     }
+    // }
+    
+
+
+
+    // std::cout << typeid().name() << std::endl;
+
+
+    // return 1;
+
+    // Instantiate plugins.
+    // auto telemetry = Telemetry{systemsList[0]};
+    // auto telemetry1 = Telemetry{systemsList[1]};
+
+    // // Instantiate plugins.
+    // auto telemetry = Telemetry{system};
+    // auto action = Action{system};
+
+    // // Instantiate mavlink_passthrough plugin.
+    // auto mavlink_passthrough = MavlinkPassthrough{system};
+
+    // auto x = static_cast<int>(system->get_system_id());
+    // std::cout << "System ID : " << x << '\n';
+
+    // System::AutopilotVersion y = system->get_autopilot_version_data();
+    // std::cout << "MAV_PROTOCOL_CAPABILITY : " << y.capabilities << '\n';
+    // std::cout << "Flight Stack Version : " << y.flight_sw_version << '\n';
+
+    // // Check vehicle mode
+    // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
+    // std::cerr << "Vehicle Mode: " << mode_result << '\n';
+    // // return 1; // debug
+
+    // // Set the flight mode to "AUTO". use SET_MODE (#11) in MAVLink Messages.
+    // mavlink_message_t message;
+
+
+    // // mavlink_msg_set_mode_pack(mavlink_passthrough.get_our_sysid(),
+    // //                           mavlink_passthrough.get_our_compid(),
+    // //                           &message,
+    // //                           system->get_system_id(),
+    // //                           MAV_COMP_ID_AUTOPILOT1,
+    // //                           4);
+
+    // mavlink_command_long_t command;
+    // command.target_system = system->get_system_id();
+    // command.target_component = MAV_COMP_ID_AUTOPILOT1;
+    // command.command = MAV_CMD_DO_SET_MODE;
+    // command.param1 = MAV_MODE_AUTO_ARMED;
+    // mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
+    //                                 mavlink_passthrough.get_our_compid(),
+    //                                 &message,
+    //                                 &command);
+
+    // mavlink_passthrough.send_message(message);
+
+
+    // return 1; // debug
+
+    // We want to listen to the altitude of the drone at 1 Hz.
+    // const auto set_rate_result = telemetry0.set_rate_position(1);
+    // if (set_rate_result != Telemetry::Result::Success) {
+    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
+    //     return 1;
+    // }
+    // const auto set_rate_result = telemetry1.set_rate_position(1);
+    // if (set_rate_result != Telemetry::Result::Success) {
+    //     std::cerr << "Setting rate failed: " << set_rate_result << '\n';
+    //     return 1;
+    // }
+
+    
+
+    // Set up callback to monitor altitude while the vehicle is in flight
+    // auto x = systemsList[0]->get_system_id();
+    // telemetry.subscribe_position([x](Telemetry::Position position) {
+    //     std::cout << "System ID: " << x << "Altitude: " << position.relative_altitude_m << " m\n";
+    // });
+
+    // std::cout << "Mark C" << std::endl;
+
+    // for(int i = 0; i < 5; ++i) {
+    //     sleep_for(seconds(5));
+
+    //     std::cout << "Number of discovered systems: " << systemsList.size() << std::endl;
+
+    //     for (auto system : mavsdk.systems()) {
+    //         std::cout << "Found system with system ID: " << static_cast<int>(system->get_system_id())
+    //           << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
+    //     }
+
+    //     std::cout << "link check " << i << " time. \n";
+    // }
+
+    // // Check until vehicle is ready to arm
+    // while (telemetry.health_all_ok() != true) {
+    //     std::cout << "Vehicle is getting ready to arm\n";
+    //     sleep_for(seconds(1));
+    // }
+
+    // // Arm vehicle
+    // std::cout << "Arming...\n";
+    // const Action::Result arm_result = action.arm();
+
+    // if (arm_result != Action::Result::Success) {
+    //     std::cerr << "Arming failed: " << arm_result << '\n';
+    //     return 1;
+    // }
+
+    // return 1; // debug
+
+
+
+    // // Take off
+    // std::cout << "Taking off...\n";
+    // const Action::Result takeoff_result = action.takeoff();
+    // if (takeoff_result != Action::Result::Success) {
+    //     std::cerr << "Takeoff failed: " << takeoff_result << '\n';
+    //     return 1;
+    // }
 
     // Let it hover for a bit before landing again.
-    sleep_for(seconds(10));
+    sleep_for(seconds(15));
 
-    std::cout << "Landing...\n";
+    // std::cout << "Landing...\n";
-    const Action::Result land_result = action.land();
+    // const Action::Result land_result = action.land();
-    if (land_result != Action::Result::Success) {
+    // if (land_result != Action::Result::Success) {
-        std::cerr << "Land failed: " << land_result << '\n';
+    //     std::cerr << "Land failed: " << land_result << '\n';
-        return 1;
+    //     return 1;
-    }
+    // }
 
-    // Check if vehicle is still in air
+    // // Check if vehicle is still in air
-    while (telemetry.in_air()) {
+    // while (telemetry.in_air()) {
-        std::cout << "Vehicle is landing...\n";
+    //     std::cout << "Vehicle is landing...\n";
-        sleep_for(seconds(1));
+    //     sleep_for(seconds(1));
-    }
+    // }
-    std::cout << "Landed!\n";
+    // std::cout << "Landed!\n";
 
-    // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
+    // // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
-    sleep_for(seconds(3));
+    // sleep_for(seconds(3));
-    std::cout << "Finished...\n";
+    // std::cout << "Finished...\n";
 
     return 0;
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ===================================================
+// 1.
+
+    // std::cout << "Mark A" << std::endl;
+    // sleep_for(seconds(3));
+
+    // System& s0 = *mavsdk.systems()[0];
+    // System& s1 = *mavsdk.systems()[1];
+    // std::cout << static_cast<int>(s0.get_system_id()) << std::endl;
+
+
+    // auto t0 = Telemetry{s0};
+    // auto t1 = Telemetry{s1};
+    // t0.subscribe_position([](Telemetry::Position position) {
+    //     std::cout << "System ID: " << "0" << " Altitude: " << position.relative_altitude_m << " m\n";
+    // });
+
+    // t1.subscribe_position([](Telemetry::Position position) {
+    //     std::cout << "System ID: " << "1" << " Altitude: " << position.relative_altitude_m << " m\n";
+    // });