diff --git a/Dockerfile b/Dockerfile index 4c027d8..f50e254 100644 --- a/Dockerfile +++ b/Dockerfile @@ -31,8 +31,8 @@ RUN rm -r /home/mavone/pkg CMD ["/bin/sh", "-c", "mavone $CONFIG_FILE"] # 測試語句 -# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -v C:\_Project\MAVSDK_Dev\configurations:/home/mavone mavone_server:0.42 -# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -p 14550:14550 -v C:\_Project\MAVSDK_Dev\configurations:/home/mavone mavone_server:0.421 +# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -v D:\tryRun:/home/mavone mavone_server:0.60 +# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -p 14550:14550 -v D:\tryRun:/home/mavone mavone_server:0.60 # docker run -it --rm -e CONFIG_FILE="/home/mavone/config1.txt" -p 49304:49304 -v /home/webpage/Mavsdk_DEV/mavone:/home/mavone mavone_server:0.421 # 這個方式會是錯的 因為會去執行 host 的環境變數 而不是 container 的環境變數 diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index e748836..b856d9e 100644 --- a/mavone/mavone.cpp +++ b/mavone/mavone.cpp @@ -299,7 +299,7 @@ int main(int argc, char** argv) } sqlString.pop_back(); sqlString += " WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";"; - std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug + // std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug stmt->execute(sqlString); gTelemetryInfo[handlerInfo.systemID] = {}; } diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp index 514a1ec..54fbb14 100644 --- a/mavone/systemHandler.cpp +++ b/mavone/systemHandler.cpp @@ -79,73 +79,38 @@ void systemHandler(System& system) { MavlinkPassthrough::CommandLong mav_command_long; MavlinkPassthrough::Result result_mavpass; Action::Result result_action; + int vehicle_type; int _counter; 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 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'; - - 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; + + // // 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'; + + // sleep_for(milliseconds(500)); + // // 演示一下 如何抓到 parameter // auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking. @@ -189,11 +154,28 @@ void systemHandler(System& system) { // 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()) { + // 用 param 去判斷是什麼種類的 system + std::pair< Param::Result, int32_t > param_int_result; + + vehicle_type = 0; + + // 實體機驗證 + // param_int_result = param.get_param_int("BATT_OPTIONS"); // 都有的 + param_int_result = param.get_param_int("BRD_SBUS_OUT"); // 只有實體機 + std::cout << "BRD_SBUS_OUT Para Read: " << param_int_result.first << " : " << param_int_result.second << std::endl; // debug + if (param_int_result.first == Param::Result::Success) { vehicle_type = 2; } + // 虛擬機驗證 + param_int_result = param.get_param_int("INS_USE3"); // 只有虛擬機 + std::cout << "INS_USE3 Para Read: " << param_int_result.first << " : " << param_int_result.second << std::endl; // debug + if (param_int_result.first == Param::Result::Success) { vehicle_type = 1; } + + + // =======================都用的============================== 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; @@ -227,28 +209,6 @@ void systemHandler(System& system) { num_str_ss.str(""); }); - // set_rate_result = telemetry.set_rate_attitude_euler(1.0); // only in v2.0 - // if (set_rate_result != mavsdk::Telemetry::Result::Success) { - // std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl; - // } - telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) { - // 轉換數字到字串用的暫存變數 - std::ostringstream num_str_ss; - - // Store telemetry information - num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.roll_deg; - telemetryInfo["drone_roll"] = num_str_ss.str(); - num_str_ss.str(""); - - num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg; - telemetryInfo["drone_pitch"] = num_str_ss.str(); - num_str_ss.str(""); - - num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg; - telemetryInfo["drone_yaw"] = num_str_ss.str(); - num_str_ss.str(""); - }); - telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) { // 轉換數字到字串用的暫存變數 std::ostringstream num_str_ss; @@ -259,9 +219,11 @@ void systemHandler(System& system) { num_str_ss.str(""); }); - set_rate_result = telemetry.set_rate_battery(1.0); - if (set_rate_result != mavsdk::Telemetry::Result::Success) { - std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl; + if (vehicle_type == 2) { + set_rate_result = telemetry.set_rate_battery(1.0); + if (set_rate_result != mavsdk::Telemetry::Result::Success) { + std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl; + } } telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) { // 轉換數字到字串用的暫存變數 @@ -276,18 +238,6 @@ void systemHandler(System& system) { num_str_ss.str(""); }); - set_rate_result = telemetry.set_rate_in_air(1.0); - if (set_rate_result != mavsdk::Telemetry::Result::Success) { - std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl; - } - telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) { - if(in_air){ - telemetryInfo["drone_air"] = "1"; - }else{ - telemetryInfo["drone_air"] = "0"; - } - }); - telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) { if(is_armed){ telemetryInfo["drone_arm"] = "1"; @@ -296,42 +246,74 @@ void systemHandler(System& system) { } }); - telemetry.subscribe_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) { - telemetryInfo["vehicle_mode"] = flightModeMap[fightMode]; - }); - - // 為了分析 statusText 字串的兩個暫存變數 - std::vector statusTextPiece; - std::string _token; - telemetry.subscribe_status_text([&system, &telemetryInfo, &statusTextPiece, &_token] - (Telemetry::StatusText statusText) { + // =======================只有無人機============================== + if (vehicle_type >= 1) { + // set_rate_result = telemetry.set_rate_attitude_euler(1.0); // only in v2.0 + // if (set_rate_result != mavsdk::Telemetry::Result::Success) { + // std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl; + // } + telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) { // 轉換數字到字串用的暫存變數 std::ostringstream num_str_ss; - // 為了分析 statusText 字串的另一個暫存變數 - std::istringstream _iss(statusText.text); - std::cout << "(systemHandler.cpp:systemHandler) status_text: " << statusText.type << " ; " << statusText.text << std::endl; // debug - - if(statusText.type == Telemetry::StatusTextType::Info) { - - while (std::getline(_iss, _token, ',')) { - statusTextPiece.push_back(_token); - } - for (const auto& value : statusTextPiece) { - if (value.at(0) == 'T') { - num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1)); - telemetryInfo["vehicle_bat_Temp"] = num_str_ss.str(); - num_str_ss.str(""); - } else if (value.at(0) == 'I') { - num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1)); - telemetryInfo["vehicle_bat_current"] = num_str_ss.str(); - num_str_ss.str(""); - } - } + // Store telemetry information + num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.roll_deg; + telemetryInfo["drone_roll"] = num_str_ss.str(); + num_str_ss.str(""); - statusTextPiece.clear(); + num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg; + telemetryInfo["drone_pitch"] = num_str_ss.str(); + num_str_ss.str(""); + + num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg; + telemetryInfo["drone_yaw"] = num_str_ss.str(); + num_str_ss.str(""); + }); + + set_rate_result = telemetry.set_rate_in_air(1.0); + if (set_rate_result != mavsdk::Telemetry::Result::Success) { + std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl; + } + telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) { + if(in_air){ + telemetryInfo["drone_air"] = "1"; + }else{ + telemetryInfo["drone_air"] = "0"; } - }); + }); + + telemetry.subscribe_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) { + telemetryInfo["vehicle_mode"] = flightModeMap[fightMode]; + }); + } + + // =======================只有車子載具============================== + + if (vehicle_type == 0) { + telemetry.subscribe_status_text([&system, &telemetryInfo] + (Telemetry::StatusText statusText) { + // 轉換數字到字串用的暫存變數 + std::ostringstream num_str_ss; + std::istringstream _iss(statusText.text); + // 為了分析 statusText 字串的兩個暫存變數 + std::string _token; + + // std::cout << "(systemHandler.cpp:systemHandler) status_text: " << statusText.type << " ; " << statusText.text << std::endl; // debug + if(statusText.type == Telemetry::StatusTextType::Info) { + while (std::getline(_iss, _token, ',')) { + if (_token.at(0) == 'T') { + num_str_ss << std::fixed << std::setprecision(2) << std::stof(_token.substr(1)); + telemetryInfo["vehicle_bat_Temp"] = num_str_ss.str(); + num_str_ss.str(""); + } else if (_token.at(0) == 'I') { + num_str_ss << std::fixed << std::setprecision(2) << std::stof(_token.substr(1)); + telemetryInfo["vehicle_bat_current"] = num_str_ss.str(); + num_str_ss.str(""); + } + } + } + }); + } } @@ -348,6 +330,18 @@ void systemHandler(System& system) { break; } } + + switch (vehicle_type) { + case 2: + telemetryInfo["vehicle_type"] = "Real"; + break; + case 1: + telemetryInfo["vehicle_type"] = "SITL"; + break; + case 0: + telemetryInfo["vehicle_type"] = "Self"; + break; + } // Loop while(system.is_connected() & !reset) { @@ -367,7 +361,8 @@ void systemHandler(System& system) { reset = (gHandlerMask[sysid]["cutoff"] != 0); // Vehicle Command - for ( auto& oneVehicleCommand : vehicleCommand) { + for ( auto& oneVehicleCommand : vehicleCommand ) { + if (vehicle_type == 0) { break; } // 不是飛機不吃指令 switch (oneVehicleCommand.first) { case -1 : std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_type! " << std::endl; diff --git a/unit function dev/test3_vel_gps_bat_head.py b/unit function dev/test3_vel_gps_bat_head.py index 652d7e5..ff455b9 100644 --- a/unit function dev/test3_vel_gps_bat_head.py +++ b/unit function dev/test3_vel_gps_bat_head.py @@ -14,6 +14,17 @@ async def run(): print("123") await drone.telemetry_server.publish_status_text(StatusText(StatusTextType.ALERT,"Check")) + # await drone.param_server.provide_param_int("CAL_ACC0_ID",1) + # await drone.param_server.provide_param_int("CAL_GYRO0_ID",1) + # await drone.param_server.provide_param_int("CAL_MAG0_ID",1) + # await drone.param_server.provide_param_int("SYS_HITL",0) + # await drone.param_server.provide_param_int("MIS_TAKEOFF_ALT",0) + + a1 = drone.param_server.IntParam("CAL_ACC0_ID",1) + a2 = drone.param_server.IntParam("CAL_GYRO0_ID",1) + a3 = drone.param_server.IntParam("CAL_MAG0_ID",1) + a4 = drone.param_server.IntParam("SYS_HITL",0) + a5 = drone.param_server.IntParam("MIS_TAKEOFF_ALT",0) while True: @@ -30,21 +41,21 @@ async def run(): heading = Heading(heading_deg) raw_gps = RawGps( - timestamp_us=123456789, # 当前时间戳，以微秒为单位 - latitude_deg=47.3977418, # 纬度 - longitude_deg=8.5455934, # 经度 - absolute_altitude_m=500, # 绝对海拔高度（米） - hdop=1.0, # 水平精度因子 - vdop=1.0, # 垂直精度因子 - velocity_m_s=random.uniform(0.0,50.0), # 速度（米/秒） - cog_deg=90.0, # 航向（度） - altitude_ellipsoid_m=510.0, # 大地水准面上的高度（米） - horizontal_uncertainty_m=1.000000000000, # 水平准确性（米） - vertical_uncertainty_m=1.00000000, - velocity_uncertainty_m_s=0.0000000000000000 ,# 垂直准确性（米） - heading_uncertainty_deg=1.0000000, # 速度准确性（米/秒） - yaw_deg=180.5# 是否已定位的标志 -) + timestamp_us=123456789, # 当前时间戳，以微秒为单位 + latitude_deg=47.3977418, # 纬度 + longitude_deg=8.5455934, # 经度 + absolute_altitude_m=500, # 绝对海拔高度（米） + hdop=1.0, # 水平精度因子 + vdop=1.0, # 垂直精度因子 + velocity_m_s=random.uniform(0.0,50.0), # 速度（米/秒） + cog_deg=90.0, # 航向（度） + altitude_ellipsoid_m=510.0, # 大地水准面上的高度（米） + horizontal_uncertainty_m=1.000000000000, # 水平准确性（米） + vertical_uncertainty_m=1.00000000, + velocity_uncertainty_m_s=0.0000000000000000 ,# 垂直准确性（米） + heading_uncertainty_deg=1.0000000, # 速度准确性（米/秒） + yaw_deg=180.5# 是否已定位的标志 + ) gps_info = GpsInfo( num_satellites=10, # 卫星数量 fix_type=FixType.RTK_FIXED # GPS 修复类型