dev v0.61

為了電池子計畫 硬加了一些東西 不太穩定 懶得詳述了

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" -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 C:\_Project\MAVSDK_Dev\configurations:/home/mavone mavone_server:0.421
+# 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 的環境變數

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] = {};
                     }

mavone/systemHandler.cpp

@@ -79,74 +79,39 @@ 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;
+    // std::cout << "Dev Test Start" << std::endl;
 
-    // Get the info plugin for this drone
+    // // Get the info plugin for this drone
-    auto atvData = system.get_autopilot_version_data();
+    // auto atvData = system.get_autopilot_version_data();
-    std::cout << "Autopilot Version Data:" << std::endl;
+    // std::cout << "Autopilot Version Data:" << std::endl;
-    std::cout << "  Vendor ID: " << atvData.vendor_id << std::endl;
+    // std::cout << "  Vendor ID: " << atvData.vendor_id << std::endl;
-    std::cout << "  Product ID: " << atvData.product_id << std::endl;
+    // std::cout << "  Product ID: " << atvData.product_id << std::endl;
-    std::cout << "  Capabilities: " << atvData.capabilities << std::endl;
+    // std::cout << "  Capabilities: " << atvData.capabilities << std::endl;
-    std::cout << "  Flight SW Version: " << atvData.flight_sw_version << 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 << "  Middleware SW Version: " << atvData.middleware_sw_version << std::endl;
-    std::cout << "  OS SW Version: " << atvData.os_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::cout << "  Board Version: " << atvData.board_version << std::endl;
 
-    std::array<uint8_t, 18> uid = atvData.uid2;
+    // std::array<uint8_t, 18> uid = atvData.uid2;
-    std::cout << "UID: ";
+    // std::cout << "UID: ";
-    for (const auto& byte : uid) {
+    // for (const auto& byte : uid) {
-        std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
+    //     std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
-    }
+    // }
-    std::cout << std::endl;
+    // std::cout << std::endl;
 
-    const Telemetry::FlightMode mode_result = telemetry.flight_mode();
+    // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
-    std::cerr << "Vehicle Mode: " << mode_result << '\n';
+    // std::cerr << "Vehicle Mode: " << mode_result << '\n';
 
-    sleep_for(milliseconds(1500));
+    // sleep_for(milliseconds(500));
 
 
-
-    // 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;
@@ -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,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;
@@ -249,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;
@@ -288,51 +282,39 @@ 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];
             });
+        }
 
-        // 為了分析 statusText 字串的兩個暫存變數
+        // =======================只有車子載具==============================
-        std::vector<std::string> statusTextPiece;
+
-        std::string _token;
+        if (vehicle_type == 0) {
-        telemetry.subscribe_status_text([&system, &telemetryInfo, &statusTextPiece, &_token]
+            telemetry.subscribe_status_text([&system, &telemetryInfo]
                 (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
+                    // 為了分析 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, ',')) {
-                        statusTextPiece.push_back(_token);
+                            if (_token.at(0) == 'T') {
-                    }
+                                num_str_ss << std::fixed << std::setprecision(2) << std::stof(_token.substr(1));
-
-                    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') {
+                            } else if (_token.at(0) == 'I') {
-                            num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1));
+                                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("");
                             }
                         }
-
-                    statusTextPiece.clear();
                     }
             });
         }
+    }
 
 
     // std::string testA;
@@ -349,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) {
         
@@ -368,6 +362,7 @@ void systemHandler(System& system) {
 
         // Vehicle Command
         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;

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: