dev v0.61

為了電池子計畫硬加了一些東西不太穩定懶得詳述了
dev v0.60
2024-06-02 16:35:53 +08:00 · 2024-04-17 15:51:08 +08:00 · 2024-03-27 07:15:40 +08:00 · 2024-03-27 07:02:55 +08:00
4 changed files with 280 additions and 129 deletions
@@ -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 的環境變數
@@ -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;
    }

@@ -74,77 +74,43 @@ void systemHandler(System& system) {
    bool reset = false;

    // 程序執行時會用到的變數
-    int _counter;
    mavlink_message_t mav_message;
-    mavlink_set_position_target_global_int_t set_position_target_global_int;
+    mavlink_command_int_t mav_command_int;
    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;
+    // 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;
+    // // 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;
+    // 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';
+    // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
+    // std::cerr << "Vehicle Mode: " << mode_result << '\n';

+    // sleep_for(milliseconds(500));

-    std::cout << "  Set Mode Test " << std::endl;
-    sleep_for(milliseconds(1500));
-
-
-    // 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.
@@ -188,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;
@@ -226,6 +209,45 @@ void systemHandler(System& system) {
            num_str_ss.str("");
        });

+        telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
+            // 轉換數字到字串用的暫存變數
+            std::ostringstream num_str_ss;
+
+            // Store telemetry information
+            num_str_ss << std::fixed << std::setprecision(4) << heading.heading_deg;
+            telemetryInfo["vehicle_head"] = num_str_ss.str();
+            num_str_ss.str("");
+        });
+
+        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) {
+            // 轉換數字到字串用的暫存變數
+            std::ostringstream num_str_ss;
+
+            // Store telemetry information
+            num_str_ss << std::fixed << std::setprecision(4) << battery.remaining_percent;
+            telemetryInfo["vehicle_bat_perc"] = num_str_ss.str();
+            num_str_ss.str("");
+            num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
+            telemetryInfo["vehicle_bat_volt"] = num_str_ss.str();
+            num_str_ss.str("");
+        });
+
+        telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
+            if(is_armed){
+                telemetryInfo["drone_arm"] = "1";
+            }else{
+                telemetryInfo["drone_arm"] = "0";
+            }
+        });
+
+        // =======================只有無人機==============================
+        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;
@@ -248,33 +270,6 @@ void systemHandler(System& system) {
                num_str_ss.str("");        
            });

-        telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
-            // 轉換數字到字串用的暫存變數
-            std::ostringstream num_str_ss;
-
-            // Store telemetry information
-            num_str_ss << std::fixed << std::setprecision(4) << heading.heading_deg;
-            telemetryInfo["vehicle_head"] = num_str_ss.str();
-            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;
-        }
-        telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) {
-            // 轉換數字到字串用的暫存變數
-            std::ostringstream num_str_ss;
-
-            // Store telemetry information
-            num_str_ss << std::fixed << std::setprecision(4) << battery.remaining_percent;
-            telemetryInfo["vehicle_bat_perc"] = num_str_ss.str();
-            num_str_ss.str("");
-            num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
-            telemetryInfo["vehicle_bat_volt"] = 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;
@@ -287,19 +282,44 @@ void systemHandler(System& system) {
                }
            });

-        telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
-            if(is_armed){
-                telemetryInfo["drone_arm"] = "1";
-            }else{
-                telemetryInfo["drone_arm"] = "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("");
+                            }
+                        }
+                    }
+            });
+        }
+    }
+
+
+    // 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()){
@@ -311,6 +331,18 @@ void systemHandler(System& system) {
        }        
    }

+    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) {
        
@@ -329,8 +361,17 @@ 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;
+                    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 , " \
@@ -344,7 +385,9 @@ void systemHandler(System& system) {
                    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;

@@ -355,6 +398,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;
@@ -366,7 +410,10 @@ 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"]);
                    #endif
                    break;

@@ -376,6 +423,28 @@ 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 {
+                        mav_command_int.target_system = sysid;                      // Target system ID
+                        mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
+                        mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
+                        mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
+                        mav_command_int.x = _target_lat * 1E7;                      // Target latitude
+                        mav_command_int.y = _target_lon * 1E7;                      // Target longitude
+                        mav_command_int.z = static_cast<float>(oneVehicleCommand.second[0]);  // Target altitude
+                        mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
+                                                        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;

@@ -385,29 +454,22 @@ 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
-                    set_position_target_global_int.time_boot_ms = 0;
-                    set_position_target_global_int.target_system = sysid;
-                    set_position_target_global_int.target_component = MAV_COMP_ID_AUTOPILOT1;
-                    set_position_target_global_int.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // 對於 home 點指定相對高度
-                    set_position_target_global_int.type_mask = 0b110111111000; // Only latitude, longitude and altitude
-                    set_position_target_global_int.lat_int = oneVehicleCommand.second[0] * 1E7; // Latitude in degrees * 1E7
-                    set_position_target_global_int.lon_int = oneVehicleCommand.second[1] * 1E7; // Longitude in degrees * 1E7
-                    set_position_target_global_int.alt = 30; // Altitude in meters
-
-                    mavlink_msg_set_position_target_global_int_pack(
-                        mavlink_passthrough.get_our_sysid(),
+                    mav_command_int.target_system = sysid;                      // Target system ID
+                    mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
+                    mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
+                    mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
+                    mav_command_int.x = oneVehicleCommand.second[0] * 1E7;      // Target latitude
+                    mav_command_int.y = oneVehicleCommand.second[1] * 1E7;      // Target longitude
+                    mav_command_int.z = std::stof(telemetryInfo["drone_alt"]);  // Target altitude
+                    mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
                                                    mavlink_passthrough.get_our_compid(),
                                                    &mav_message,
-                        set_position_target_global_int.time_boot_ms,
-                        set_position_target_global_int.target_system,
-                        set_position_target_global_int.target_component,
-                        set_position_target_global_int.coordinate_frame,
-                        set_position_target_global_int.type_mask,
-                        set_position_target_global_int.lat_int,
-                        set_position_target_global_int.lon_int,
-                        set_position_target_global_int.alt,
-                        0, 0, 0, 0, 0, 0, 0, 0);  // vx, vy, vz, afx, afy, afz, yaw, yaw_rate are all 0
+                                                    &mav_command_int);
                    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;
@@ -0,0 +1,80 @@
+#!/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"))
+
+    # 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:
+
+        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())
Author	SHA1	Message	Date
chiyu1468	3f9b2ab8e5	dev v0.61 為了電池子計畫硬加了一些東西不太穩定懶得詳述了	2024-06-02 16:35:53 +08:00
chiyu1468	f937f89e56	dev v0.60 前面的版本是增加控制部分這個版本是增加從 statusText 弄成電池的電流與溫度值該功能可以搭配 test3_vel_gps_bat_head.py 程式測試	2024-04-17 15:51:08 +08:00
chiyu1468	0f532701f2	nothing change 只是整理一下排版跟刪掉多餘註解	2024-03-27 07:15:40 +08:00
chiyu1468	06b9df2c0f	basic command complete 使用另一個方式做了 goto 完成了 set height	2024-03-27 07:02:55 +08:00