diff --git a/mavone/mavone.cpp b/mavone/mavone.cpp index acaecc0..e748836 100644 --- a/mavone/mavone.cpp +++ b/mavone/mavone.cpp @@ -68,6 +68,9 @@ std::map> decodeMysqlCommand(std::string commandType, std::map> result; switch (commandTypeMap[commandType]) { + case 0: + result[-1] = {0,0}; + break; case 101: // aGuided result[1] = {4,0}; // means change mode to 4 break; @@ -78,13 +81,19 @@ std::map> decodeMysqlCommand(std::string commandType, result[11] = {0,0}; // means action arm break; case 104: // aSetHeight - result[13] = {std::stof(Para1),0}; // means change height to Para1 + try { result[13] = {std::stof(Para1),0}; } // means change height to Para1 + catch (std::invalid_argument &e) { + result[-2] = {0,0}; + } break; case 105: // aTakeoff result[12] = {0,0}; // means action takeoff break; case 106: // aGoto - result[21] = {std::stof(Para1),std::stof(Para2)}; // means goto + try { result[21] = {std::stof(Para1),std::stof(Para2)}; } // means goto + catch (std::invalid_argument &e) { + result[-2] = {0,0}; + } break; } @@ -290,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 29bd7d3..514a1ec 100644 --- a/mavone/systemHandler.cpp +++ b/mavone/systemHandler.cpp @@ -84,7 +84,69 @@ void systemHandler(System& system) { double _target_lon; double _target_lat; - // std::cout << "Dev Test Start" << 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(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; + // // 演示一下 如何抓到 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; @@ -126,7 +188,8 @@ void systemHandler(System& system) { // // 看看 has_autopilot() 的值 // 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()) { @@ -236,8 +299,45 @@ 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) { + // 轉換數字到字串用的暫存變數 + 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(""); + } + } + + statusTextPiece.clear(); + } + }); } + + // std::string testA; + std::ostringstream testA; + // Wait Until telemetryInfo get something. and if nothing get from System for 10 sec will cut off this connection. _counter = 0; while(telemetryInfo.empty()){ @@ -269,6 +369,14 @@ void systemHandler(System& system) { // Vehicle Command for ( auto& oneVehicleCommand : vehicleCommand) { switch (oneVehicleCommand.first) { + case -1 : + std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_type! " << std::endl; + telemetryInfo["StatusText"] = "command_type Error!"; + break; + case -2 : + std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_para! " << std::endl; + break; + // Here deal with the function of Change Mode case 1 : std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Change Mode , " \ @@ -276,13 +384,15 @@ void systemHandler(System& system) { << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE mav_command_long.target_sysid = sysid; - mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1; // 這行指 component id = 1 - mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE - mav_command_long.param1 = 1; // 固定是 1 - mav_command_long.param2 = static_cast(oneVehicleCommand.second[0]); // mode + mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1; // 這行指 component id = 1 + mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE + mav_command_long.param1 = 1; // 固定是 1 + mav_command_long.param2 = static_cast(oneVehicleCommand.second[0]); // mode result_mavpass = mavlink_passthrough.send_command_long(mav_command_long); // 送出指令 + // telemetryInfo["StatusText"] = result_mavpass; std::cout << " ACK REL: " << result_mavpass << std::endl; // dev + // testA << result_mavpass; #endif break; @@ -293,6 +403,7 @@ void systemHandler(System& system) { << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE result_action = action.arm(); + // telemetryInfo["StatusText"] = result_mavpass; std::cout << " ACK REL: " << result_action << std::endl; // dev #endif break; @@ -304,6 +415,7 @@ void systemHandler(System& system) { << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE result_action = action.takeoff(); + // telemetryInfo["StatusText"] = result_mavpass; std::cout << " ACK REL: " << result_action << std::endl; // dev _target_lat = std::stof(telemetryInfo["vehicle_lat"]); _target_lon = std::stof(telemetryInfo["vehicle_lon"]); @@ -316,6 +428,8 @@ void systemHandler(System& system) { << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \ << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE + // 若飛機滯空時 用 goto 方法到指定高度 + // 若飛機在地時 用 set_takeoff_altitude 設定起飛高度 if (telemetryInfo["drone_air"] == "0") { action.set_takeoff_altitude(static_cast(oneVehicleCommand.second[0])); // mavsdk show error but still effect well (?) } else { @@ -330,8 +444,11 @@ void systemHandler(System& system) { mavlink_passthrough.get_our_compid(), &mav_message, &mav_command_int); + // mavlink_passthrough.send_message(mav_message); result_mavpass = mavlink_passthrough.send_message(mav_message); std::cout << " ACK REL: " << result_mavpass << std::endl; // dev + std::cout << "x: " << std::to_string(_target_lon) << std::endl; // dev + std::cout << "y: " << std::to_string(_target_lat) << std::endl; // dev } #endif break; @@ -356,17 +473,22 @@ void systemHandler(System& system) { result_mavpass = mavlink_passthrough.send_message(mav_message); _target_lon = oneVehicleCommand.second[1]; _target_lat = oneVehicleCommand.second[0]; + // telemetryInfo["StatusText"] = result_mavpass; + std::cout << " ACK REL: " << result_mavpass << std::endl; // dev #endif break; } } + // for purpose sleep_for(milliseconds(100)); } + + // Thread Terminate std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug diff --git a/unit function dev/test3_vel_gps_bat_head.py b/unit function dev/test3_vel_gps_bat_head.py new file mode 100644 index 0000000..652d7e5 --- /dev/null +++ b/unit function dev/test3_vel_gps_bat_head.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +import asyncio +from mavsdk import System +from mavsdk.telemetry_server import (StatusText,StatusTextType,Battery,Position,VelocityNed, Heading,RawGps,GpsInfo,FixType) +import random +async def run(): + + # drone = System(port=50053, sysid=96, compid=1) + drone = System(port=50053) + print("456") + + # await drone.connect(system_address="udp://127.0.0.1:14550") + await drone.connect() + print("123") + await drone.telemetry_server.publish_status_text(StatusText(StatusTextType.ALERT,"Check")) + + + while True: + + remaining_percent = random.uniform(0.0, 1.0) + voltage_v = random.uniform(0.0, 20.0) + + latitude_deg = random.uniform(0.0, 50.0) + longitude_deg = random.uniform(0.0, 180.0) + absolute_altitude_m = random.uniform(0.0, 50.0) + relative_altitude_m = random.uniform(0.0, 50.0) + heading_deg = random.uniform(0.0, 360.0) + position = Position(latitude_deg, longitude_deg,absolute_altitude_m,relative_altitude_m) + velocity_ned = VelocityNed(north_m_s=0.0, east_m_s=0.0, down_m_s=0.0) + 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# 是否已定位的标志 +) + gps_info = GpsInfo( + num_satellites=10, # 卫星数量 + fix_type=FixType.RTK_FIXED # GPS 修复类型 + ) + await drone.telemetry_server.publish_battery(Battery(voltage_v,remaining_percent)) + await drone.telemetry_server.publish_position(position,velocity_ned,heading) + await drone.telemetry_server.publish_raw_gps(raw_gps,gps_info) + + vehicle_battery_temperature = random.uniform(20.0,150.0) # 電池溫度 + vehicle_battery_current = random.uniform(0.0,15.0) # 電池電流 + vehicle_battery_CAT = "I" + str(vehicle_battery_current) + "," + "T" + str(vehicle_battery_temperature) + print(vehicle_battery_CAT) + await drone.telemetry_server.publish_status_text(StatusText(StatusTextType.INFO,vehicle_battery_CAT)) + await asyncio.sleep(2) + + + + + #print("remaining_percent: ",remaining_percent) +if __name__ == "__main__": + loop = asyncio.get_event_loop() + loop.run_until_complete(run()) \ No newline at end of file