diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat index d274c070a135f..abf8516ffb6e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat @@ -24,3 +24,5 @@ define HAL_WITH_MCU_MONITORING 1 include ../include/network_PPPGW.inc define HAL_MONITOR_THREAD_ENABLED 1 + +define AP_NETWORKING_TESTS_ENABLED 1 diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 71d323dc8be76..5c7e8ebca18d6 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -289,12 +289,14 @@ class AP_Networking TEST_TCP_CLIENT = (1U<<1), TEST_TCP_DISCARD = (1U<<2), TEST_TCP_REFLECT = (1U<<3), + TEST_CONNECTOR_LOOPBACK = (1U<<4), }; void start_tests(void); void test_UDP_client(void); void test_TCP_client(void); void test_TCP_discard(void); void test_TCP_reflect(void); + void test_connector_loopback(void); #endif // AP_NETWORKING_TESTS_ENABLED #if AP_NETWORKING_REGISTER_PORT_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp index 8a87ed15ba388..48af6322828f0 100644 --- a/libraries/AP_Networking/AP_Networking_tests.cpp +++ b/libraries/AP_Networking/AP_Networking_tests.cpp @@ -38,6 +38,11 @@ void AP_Networking::start_tests(void) "TCP_reflect", 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); } + if (param.tests & TEST_CONNECTOR_LOOPBACK) { + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_connector_loopback, void), + "connector_loopback", + 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); + } } /* @@ -200,4 +205,72 @@ void AP_Networking::test_TCP_reflect(void) } } +void AP_Networking::test_connector_loopback(void) +{ + startup_wait(); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Connector Loopback: starting"); + + // start tcp discard server + auto *listen_sock = new SocketAPM(false); + if (listen_sock == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to create socket"); + return; + } + listen_sock->reuseaddress(); + + // use netif ip address + char ipstr[16]; + SocketAPM::inet_addr_to_str(get_ip_active(), ipstr, sizeof(ipstr)); + if (!listen_sock->bind(ipstr, 9)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to bind"); + return; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: bound"); + + if (!listen_sock->listen(1)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to listen"); + return; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: listening"); + // create discard client + auto *client = new SocketAPM(false); + if (client == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to create client"); + return; + } + while (!client->connect(ipstr, 9)) { + hal.scheduler->delay(10); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback: connected"); + + // accept client + auto *sock = listen_sock->accept(100); + if (sock == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "connector_loopback: failed to accept"); + return; + } + + uint8_t buf[1024]; + uint32_t last_report_ms = AP_HAL::millis(); + uint32_t total_rx = 0; + while (true) { + if ((param.tests & TEST_CONNECTOR_LOOPBACK) == 0) { + hal.scheduler->delay(1); + continue; + } + client->send(buf, 1024); + if (sock->pollin(0)) { + const ssize_t ret = sock->recv(buf, sizeof(buf), 0); + if (ret > 0) { + total_rx += ret; + } + } + if (AP_HAL::millis() - last_report_ms >= 1000) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "connector_loopback throughput %lu kbytes/sec", total_rx/1024); + total_rx = 0; + last_report_ms = AP_HAL::millis(); + } + } +} + #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED