dev v0.60

前面的版本是增加控制部分
這個版本是增加從 statusText 弄成電池的 電流與溫度值
該功能可以搭配 test3_vel_gps_bat_head.py 程式測試

mavone/mavone.cpp

@@ -68,6 +68,9 @@ std::map<int, std::array<double, 2>> decodeMysqlCommand(std::string commandType,
     std::map<int, std::array<double, 2>> 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<int, std::array<double, 2>> 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] = {};
                     }

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<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';
+
+    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<std::string> 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<int>(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<int>(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<int>(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
 

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())