diff --git a/mavone/config.txt b/configurations/mavone_config.txt similarity index 100% rename from mavone/config.txt rename to configurations/mavone_config.txt diff --git a/mavone/NodeRed_one.sql b/configurations/mavone_mysql_schema.sql similarity index 100% rename from mavone/NodeRed_one.sql rename to configurations/mavone_mysql_schema.sql diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp index ae84248..8cb49d3 100644 --- a/mavone/systemHandler.cpp +++ b/mavone/systemHandler.cpp @@ -1,7 +1,7 @@ - - #include +#include +#include #include #include #include @@ -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(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 uid = atvData.uid2; + std::cout << "UID: "; + for (const auto& byte : uid) { + std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast(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 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;