AP_Networking: improve startup wait

libraries/AP_Networking/AP_Networking.cpp

@@ -7,6 +7,7 @@
 #include "AP_Networking_Backend.h"
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Math/crc.h>
+#include <AP_InternalError/AP_InternalError.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -256,6 +257,25 @@ uint32_t AP_Networking::get_gateway_active() const
     return backend?backend->activeSettings.gw:0;
 }
 
+/*
+  wait for networking to be active
+ */
+void AP_Networking::startup_wait(void) const
+{
+    if (hal.scheduler->in_main_thread()) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+    while (!hal.scheduler->is_system_initialized()) {
+        hal.scheduler->delay(100);
+    }
+#if AP_NETWORKING_BACKEND_CHIBIOS
+    do {
+        hal.scheduler->delay(250);
+    } while (get_ip_active() == 0);
+#endif
+}
+
 AP_Networking *AP_Networking::singleton;
 
 namespace AP

libraries/AP_Networking/AP_Networking.h

@@ -144,6 +144,9 @@ class AP_Networking
         param.gwaddr.set_uint32(gw);
     }
 
+    // wait in a thread for network startup
+    void startup_wait(void) const;
+
     // helper functions to convert between 32bit IP addresses and null terminated strings and back
     static uint32_t convert_str_to_ip(const char* ip_str);
     static const char* convert_ip_to_str(uint32_t ip);
@@ -211,7 +214,6 @@ class AP_Networking
             return false;
         }
 
-        void wait_startup();
         void udp_client_init(void);
         void udp_server_init(void);
         void tcp_server_init(void);

libraries/AP_Networking/AP_Networking_port.cpp

@@ -170,23 +170,12 @@ void AP_Networking::Port::tcp_client_init(void)
     }
 }
 
-/*
-  wait for networking stack to be up
- */
-void AP_Networking::Port::wait_startup(void)
-{
-    while (!hal.scheduler->is_system_initialized()) {
-        hal.scheduler->delay(100);
-    }
-    hal.scheduler->delay(1000);
-}
-
 /*
   update a UDP client
  */
 void AP_Networking::Port::udp_client_loop(void)
 {
-    wait_startup();
+    AP::network().startup_wait();
 
     const char *dest = ip.get_str();
     if (!sock->connect(dest, port.get())) {
@@ -214,7 +203,7 @@ void AP_Networking::Port::udp_client_loop(void)
  */
 void AP_Networking::Port::udp_server_loop(void)
 {
-    wait_startup();
+    AP::network().startup_wait();
 
     const char *addr = ip.get_str();
     if (!sock->bind(addr, port.get())) {
@@ -241,7 +230,7 @@ void AP_Networking::Port::udp_server_loop(void)
  */
 void AP_Networking::Port::tcp_server_loop(void)
 {
-    wait_startup();
+    AP::network().startup_wait();
 
     const char *addr = ip.get_str();
     if (!listen_sock->bind(addr, port.get()) || !listen_sock->listen(1)) {
@@ -285,7 +274,7 @@ void AP_Networking::Port::tcp_server_loop(void)
  */
 void AP_Networking::Port::tcp_client_loop(void)
 {
-    wait_startup();
+    AP::network().startup_wait();
 
     close_on_recv_error = true;
 

libraries/AP_Networking/AP_Networking_tests.cpp

@@ -40,10 +40,7 @@ void AP_Networking::start_tests(void)
  */
 void AP_Networking::test_UDP_client(void)
 {
-    while (!hal.scheduler->is_system_initialized()) {
-        hal.scheduler->delay(100);
-    }
-    hal.scheduler->delay(1000);
+    startup_wait();
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP_client: starting");
     const char *dest = param.test_ipaddr.get_str();
     auto *sock = new SocketAPM(true);
@@ -75,10 +72,7 @@ void AP_Networking::test_UDP_client(void)
  */
 void AP_Networking::test_TCP_client(void)
 {
-    while (!hal.scheduler->is_system_initialized()) {
-        hal.scheduler->delay(100);
-    }
-    hal.scheduler->delay(1000);
+    startup_wait();
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_client: starting");
     const char *dest = param.test_ipaddr.get_str();
     auto *sock = new SocketAPM(false);
@@ -110,10 +104,7 @@ void AP_Networking::test_TCP_client(void)
  */
 void AP_Networking::test_TCP_discard(void)
 {
-    while (!hal.scheduler->is_system_initialized()) {
-        hal.scheduler->delay(100);
-    }
-    hal.scheduler->delay(1000);
+    startup_wait();
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_discard: starting");
     const char *dest = param.test_ipaddr.get_str();
     auto *sock = new SocketAPM(false);