diff --git a/test/mavsdk_tests/run_forwarding_bench.sh b/test/mavsdk_tests/run_forwarding_bench.sh index d137322d72..c834d0bdad 100755 --- a/test/mavsdk_tests/run_forwarding_bench.sh +++ b/test/mavsdk_tests/run_forwarding_bench.sh @@ -91,30 +91,19 @@ echo "" 2>&1 | tee "${RESULT_PREFIX}_bench.log" echo "" -echo "=== Step 5: Collect perf counters ===" - -# Send 'perf' command to PX4 via mavlink shell (or direct if available) -sleep 2 +echo "=== Step 5: Perf counters ===" +echo "The benchmark tool reads perf counters from the board automatically" +echo "via the MAVLink shell protocol. Check the bench log above for BENCH lines." echo "" -echo "=== Step 6: Stop PX4 and extract perf data ===" +echo "=== Step 6: Stop PX4 ===" kill -INT "$PX4_PID" 2>/dev/null || true -sleep 3 +wait "$PX4_PID" 2>/dev/null || true -# PX4 dumps perf counters on exit in some configurations. -# Extract perf lines from the log. -if grep -q "mavlink: fwd_msg" "${RESULT_PREFIX}_px4.log"; then - echo "Perf counters from PX4 log:" - grep -E "mavlink: (fwd_|comp_seen|unsigned_cb)" "${RESULT_PREFIX}_px4.log" | tee "${RESULT_PREFIX}_perf.txt" -else - echo "NOTE: Perf counters not found in PX4 log." - echo "Run 'perf' in the PX4 console before stopping to capture data." - echo "" - echo "Manual workflow:" - echo " 1. Terminal 1: PX4_SIM_MODEL=sihsim_quadx make px4_sitl_sih" - echo " 2. Terminal 2: $BUILD_DIR/mavlink_forwarding_bench --duration $DURATION --rate $RATE --connections $CONNECTIONS" - echo " 3. Terminal 1 (PX4 console): perf" - echo " 4. Copy the perf output to ${RESULT_PREFIX}_perf.txt" +# Extract bench lines from the benchmark log if available +if grep -q "BENCH" "${RESULT_PREFIX}_bench.log" 2>/dev/null; then + echo "Perf counters:" + grep "BENCH" "${RESULT_PREFIX}_bench.log" | tee "${RESULT_PREFIX}_perf.txt" fi echo "" diff --git a/test/mavsdk_tests/test_mavlink_forwarding_bench.cpp b/test/mavsdk_tests/test_mavlink_forwarding_bench.cpp index 7566ed81a2..d9c7a3cd2b 100644 --- a/test/mavsdk_tests/test_mavlink_forwarding_bench.cpp +++ b/test/mavsdk_tests/test_mavlink_forwarding_bench.cpp @@ -52,6 +52,7 @@ #include #include +#include #include #include @@ -127,6 +128,8 @@ static void print_usage(const char *bin) << " --duration Test duration in seconds (default: 60)\n" << " --rate Messages per second per connection (default: 200)\n" << " --connections Number of UDP connections (default: 2)\n" + << " --url MAVSDK connection URL (e.g. serial:///dev/ttyACM0:57600)\n" + << " When set, uses a single connection (ignores --connections)\n" << " --gcs-port GCS UDP port (default: 14550)\n" << " --api-port Offboard/API UDP port (default: 14540)\n" << " --report Write CSV report to file (default: stdout)\n" @@ -141,6 +144,7 @@ int main(int argc, char *argv[]) int gcs_port = 14550; int api_port = 14540; std::string report_file; + std::string connection_url; for (int i = 1; i < argc; ++i) { std::string arg(argv[i]); @@ -154,6 +158,10 @@ int main(int argc, char *argv[]) } else if (arg == "--connections" && i + 1 < argc) { num_connections = std::atoi(argv[++i]); + } else if (arg == "--url" && i + 1 < argc) { + connection_url = argv[++i]; + num_connections = 1; + } else if (arg == "--gcs-port" && i + 1 < argc) { gcs_port = std::atoi(argv[++i]); @@ -174,6 +182,16 @@ int main(int argc, char *argv[]) } } + if (duration_sec <= 0 || rate_hz <= 0 || num_connections <= 0) { + std::cerr << "Error: --duration, --rate, and --connections must be positive" << std::endl; + return 1; + } + + if (num_connections > 10) { + std::cerr << "Error: max 10 connections (sysid 245-254)" << std::endl; + return 1; + } + signal(SIGINT, signal_handler); signal(SIGTERM, signal_handler); @@ -183,27 +201,37 @@ int main(int argc, char *argv[]) std::cout << "Connections: " << num_connections << std::endl; std::cout << std::endl; - // Build list of UDP ports to connect to - std::vector ports; + // Build list of connection URLs + std::vector urls; - if (num_connections >= 1) { - ports.push_back(api_port); // Offboard/API link - } + if (!connection_url.empty()) { + urls.push_back(connection_url); - if (num_connections >= 2) { - ports.push_back(gcs_port); // GCS link - } + } else { + std::vector ports; - // Additional connections use sequential ports after api_port - for (int i = 2; i < num_connections; ++i) { - ports.push_back(api_port + i); + if (num_connections >= 1) { + ports.push_back(api_port); + } + + if (num_connections >= 2) { + ports.push_back(gcs_port); + } + + for (int i = 2; i < num_connections; ++i) { + ports.push_back(api_port + i); + } + + for (int p : ports) { + urls.push_back("udp://:" + std::to_string(p)); + } } // Create MAVSDK instances and connect std::vector> mavsdks; - std::vector passthroughs; + std::vector> passthroughs; - for (size_t i = 0; i < ports.size(); ++i) { + for (size_t i = 0; i < urls.size(); ++i) { auto config = Mavsdk::Configuration( static_cast(245 + i), // unique sysid per connection MAV_COMP_ID_MISSIONPLANNER, @@ -211,13 +239,12 @@ int main(int argc, char *argv[]) ); auto mavsdk = std::make_unique(config); - std::string url = "udp://:" + std::to_string(ports[i]); - std::cout << "Connecting to " << url << " (sysid=" << (245 + i) << ")..." << std::endl; + std::cout << "Connecting to " << urls[i] << " (sysid=" << (245 + i) << ")..." << std::endl; - auto result = mavsdk->add_any_connection(url); + auto result = mavsdk->add_any_connection(urls[i]); if (result != ConnectionResult::Success) { - std::cerr << "Connection failed: " << url << std::endl; + std::cerr << "Connection failed: " << urls[i] << std::endl; return 1; } @@ -225,7 +252,7 @@ int main(int argc, char *argv[]) std::cout << " Waiting for system..." << std::flush; bool found = false; - for (int t = 0; t < 50; ++t) { // 5 second timeout + for (int t = 0; t < 100; ++t) { // 10 second timeout for serial if (!mavsdk->systems().empty()) { found = true; break; @@ -235,15 +262,14 @@ int main(int argc, char *argv[]) } if (!found) { - std::cerr << "\n Timeout waiting for system on port " << ports[i] << std::endl; + std::cerr << "\n Timeout waiting for system on " << urls[i] << std::endl; return 1; } std::cout << " OK" << std::endl; auto system = mavsdk->systems().front(); - auto *passthrough = new MavlinkPassthrough(system); - passthroughs.push_back(passthrough); + passthroughs.push_back(std::make_unique(system)); mavsdks.push_back(std::move(mavsdk)); } @@ -318,7 +344,7 @@ int main(int argc, char *argv[]) uint64_t errors = stats[i].send_errors.load(); double rate = static_cast(sent) / total_elapsed; - std::cout << " Connection " << i << " (port " << ports[i] << "): " + std::cout << " Connection " << i << " (" << urls[i] << "): " << "sent=" << sent << " errors=" << errors << " rate=" << static_cast(rate) << " msg/s" @@ -354,15 +380,15 @@ int main(int argc, char *argv[]) } } - fprintf(out, "connection,port,sysid,messages_sent,send_errors,duration_s,rate_hz_requested,rate_hz_actual\n"); + fprintf(out, "connection,url,sysid,messages_sent,send_errors,duration_s,rate_hz_requested,rate_hz_actual\n"); for (size_t i = 0; i < stats.size(); ++i) { uint64_t sent = stats[i].messages_sent.load(); uint64_t errors = stats[i].send_errors.load(); double rate = static_cast(sent) / total_elapsed; - fprintf(out, "%zu,%d,%d,%" PRIu64 ",%" PRIu64 ",%.1f,%d,%.1f\n", - i, ports[i], static_cast(245 + i), + fprintf(out, "%zu,%s,%d,%" PRIu64 ",%" PRIu64 ",%.1f,%d,%.1f\n", + i, urls[i].c_str(), static_cast(245 + i), sent, errors, total_elapsed, rate_hz, rate); } @@ -371,10 +397,32 @@ int main(int argc, char *argv[]) std::cout << "CSV report written to: " << report_file << std::endl; } - // Cleanup - for (auto *p : passthroughs) { - delete p; + // Read bench counters from the board via MAVLink shell + std::cout << std::endl; + std::cout << "=== Reading board perf counters ===" << std::endl; + + auto system = mavsdks.front()->systems().front(); + Shell shell(system); + + std::string shell_output; + std::atomic got_bench{false}; + + shell.subscribe_receive([&](std::string output) { + shell_output += output; + std::cout << output; + + if (output.find("fwd_msg lock") != std::string::npos) { + got_bench.store(true); + } + }); + + shell.send("mavlink status"); + + for (int i = 0; i < 100 && !got_bench.load(); ++i) { + std::this_thread::sleep_for(100ms); } + std::this_thread::sleep_for(500ms); + return 0; }