diff --git a/libraries/AP_Networking/AP_Networking_PPP.cpp b/libraries/AP_Networking/AP_Networking_PPP.cpp index b5437de9e4..13929634e7 100644 --- a/libraries/AP_Networking/AP_Networking_PPP.cpp +++ b/libraries/AP_Networking/AP_Networking_PPP.cpp @@ -53,70 +53,28 @@ void AP_Networking_PPP::ppp_status_callback(struct ppp_pcb_s *pcb, int code, voi { auto &driver = *(AP_Networking_PPP *)ctx; struct netif *pppif = ppp_netif(pcb); - const char *msg = nullptr; switch (code) { - case PPPERR_NONE: { - // got addresses + case PPPERR_NONE: + // got new addresses for the link driver.activeSettings.ip = ntohl(netif_ip4_addr(pppif)->addr); driver.activeSettings.gw = ntohl(netif_ip4_gw(pppif)->addr); driver.activeSettings.nm = ntohl(netif_ip4_netmask(pppif)->addr); driver.activeSettings.last_change_ms = AP_HAL::millis(); + break; + case PPPERR_OPEN: + case PPPERR_CONNECT: + case PPPERR_PEERDEAD: + case PPPERR_IDLETIMEOUT: + case PPPERR_CONNECTTIME: + driver.need_restart = true; break; - } - case PPPERR_PARAM: { /* Invalid parameter. */ - msg = "PPPERR_PARAM"; + + default: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: error %d", code); break; } - case PPPERR_OPEN: { /* Unable to open PPP session. */ - msg = "PPPERR_OPEN"; - break; - } - case PPPERR_DEVICE: { /* Invalid I/O device for PPP. */ - msg = "PPPERR_DEVICE"; - break; - } - case PPPERR_ALLOC: { /* Unable to allocate resources. */ - msg = "PPPERR_ALLOC"; - break; - } - case PPPERR_USER: { /* User interrupt. */ - msg = "PPPERR_USER"; - break; - } - case PPPERR_CONNECT: { /* Connection lost. */ - msg = "PPPERR_CONNECT"; - break; - } - case PPPERR_AUTHFAIL: { /* Failed authentication challenge. */ - msg = "PPPERR_AUTHFAIL"; - break; - } - case PPPERR_PROTOCOL: { /* Failed to meet protocol. */ - msg = "PPPERR_PROTOCOL"; - break; - } - case PPPERR_PEERDEAD: { /* Connection timeout */ - msg = "PPPERR_PEERDEAD"; - break; - } - case PPPERR_IDLETIMEOUT: { /* Idle Timeout */ - msg = "PPPERR_IDLETIMEOUT"; - break; - } - case PPPERR_CONNECTTIME: { /* Max connect time reached */ - msg = "PPPERR_CONNECTTIME"; - break; - } - case PPPERR_LOOPBACK: { /* Loopback detected */ - msg = "PPPERR_LOOPBACK"; - break; - } - } - if (msg != nullptr) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: %s", msg); - } } @@ -169,13 +127,6 @@ void AP_Networking_PPP::ppp_loop(void) while (!hal.scheduler->is_system_initialized()) { hal.scheduler->delay_microseconds(1000); } - - // connect and set as default route - LWIP_TCPIP_LOCK(); - ppp_connect(ppp, 0); - - netif_set_default(pppif); - LWIP_TCPIP_UNLOCK(); // ensure this thread owns the uart uart->begin(0); @@ -183,13 +134,27 @@ void AP_Networking_PPP::ppp_loop(void) while (true) { uint8_t buf[1024]; - auto n = uart->read(buf, sizeof(buf)); - if (n > 0) { - LWIP_TCPIP_LOCK(); - pppos_input(ppp, buf, n); - LWIP_TCPIP_UNLOCK(); - } else { - hal.scheduler->delay_microseconds(200); + + // connect and set as default route + LWIP_TCPIP_LOCK(); + ppp_connect(ppp, 0); + + netif_set_default(pppif); + LWIP_TCPIP_UNLOCK(); + + need_restart = false; + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: connected"); + + while (!need_restart) { + auto n = uart->read(buf, sizeof(buf)); + if (n > 0) { + LWIP_TCPIP_LOCK(); + pppos_input(ppp, buf, n); + LWIP_TCPIP_UNLOCK(); + } else { + hal.scheduler->delay_microseconds(200); + } } } } diff --git a/libraries/AP_Networking/AP_Networking_PPP.h b/libraries/AP_Networking/AP_Networking_PPP.h index e04b9ec234..7e4a16d60a 100644 --- a/libraries/AP_Networking/AP_Networking_PPP.h +++ b/libraries/AP_Networking/AP_Networking_PPP.h @@ -21,6 +21,7 @@ private: AP_HAL::UARTDriver *uart; struct netif *pppif; struct ppp_pcb_s *ppp; + bool need_restart; static void ppp_status_callback(struct ppp_pcb_s *pcb, int code, void *ctx); static uint32_t ppp_output_cb(struct ppp_pcb_s *pcb, const void *data, uint32_t len, void *ctx);