Add error handling to ESP-NOW

Author: nseidle

Firmware/RTK_Surveyor/Begin.ino

@@ -432,7 +432,7 @@ void beginGNSS()
       return;
     }
   }
-  
+
   //Increase transactions to reduce transfer time
   i2cGNSS.i2cTransactionSize = 128;
 
@@ -769,9 +769,7 @@ void radioStart()
     //Nothing to start. UART2 of ZED is connected to external Radio port and is configured at configureUbloxModule()
   }
   else if (settings.radioType == RADIO_ESPNOW)
-  {
     espnowStart();
-  }
 }
 
 //Start task to determine SD card size

Firmware/RTK_Surveyor/ESPNOW.ino

@@ -90,23 +90,33 @@ void promiscuous_rx_cb(void *buf, wifi_promiscuous_pkt_type_t type)
 void espnowStart()
 {
 #ifdef COMPILE_ESPNOW
+
+  esp_err_t response;
+
   if (wifiState == WIFI_OFF && espnowState == ESPNOW_OFF)
   {
-    //Radio is off, turn it on
-    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_LR); //Stops WiFi Station.
+    if (WiFi.getMode() == WIFI_OFF)
+      WiFi.mode(WIFI_STA);
 
-    WiFi.mode(WIFI_STA);
-
-    log_d("WiFi off, ESP-Now added to protocols");
+    //Radio is off, turn it on
+    //esp_wifi_set_protocol requires WiFi to be started
+    response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_LR); //Stops WiFi Station.
+    if (response != ESP_OK)
+      systemPrintf("espnowStart: Error setting ESP-Now lone protocol: %s\r\n", esp_err_to_name(response));
+    else
+      log_d("WiFi off, ESP-Now added to protocols");
   }
   //If WiFi is on but ESP NOW is off, then enable LR protocol
   else if (wifiState > WIFI_OFF && espnowState == ESPNOW_OFF)
   {
     //Enable WiFi + ESP-Now
     //Enable long range, PHY rate of ESP32 will be 512Kbps or 256Kbps
-    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR); //Stops WiFi Station.
-
-    log_d("WiFi on, ESP-Now added to protocols");
+    //esp_wifi_set_protocol requires WiFi to be started
+    response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR);
+    if (response != ESP_OK)
+      systemPrintf("espnowStart: Error setting ESP-Now + WiFi protocols: %s\r\n", esp_err_to_name(response));
+    else
+      log_d("WiFi on, ESP-Now added to protocols");
   }
 
   //If ESP-Now is already active, do nothing
@@ -115,14 +125,18 @@ void espnowStart()
     log_d("ESP-Now already on");
   }
 
+
   // Init ESP-NOW
   if (esp_now_init() != ESP_OK) {
-    systemPrintln("Error starting ESP-Now");
+    systemPrintln("espnowStart: Error starting ESP-Now");
     return;
   }
 
   // Use promiscuous callback to capture RSSI of packet
-  esp_wifi_set_promiscuous(true);
+  response = esp_wifi_set_promiscuous(true);
+  if (response != ESP_OK)
+    systemPrintf("espnowStart: Error setting promiscuous mode: %s\r\n", esp_err_to_name(response));
+
   esp_wifi_set_promiscuous_rx_cb(&promiscuous_rx_cb);
 
   // Register callbacks
@@ -179,19 +193,29 @@ void espnowStop()
   if (espnowState == ESPNOW_OFF) return;
 
   //Turn off promiscuous WiFi mode
-  esp_wifi_set_promiscuous(false);
+  esp_err_t response = esp_wifi_set_promiscuous(false);
+  if (response != ESP_OK)
+    systemPrintf("espnowStop: Failed to set promiscuous mode: %s\r\n", esp_err_to_name(response));
+
   esp_wifi_set_promiscuous_rx_cb(NULL);
 
   //Deregister callbacks
   //esp_now_unregister_send_cb();
-  esp_now_unregister_recv_cb();
+  response = esp_now_unregister_recv_cb();
+  if (response != ESP_OK)
+    systemPrintf("espnowStop: Failed to unregister receive callback: %s\r\n", esp_err_to_name(response));
 
   //Forget all ESP-Now Peers
   for (int x = 0 ; x < settings.espnowPeerCount ; x++)
     espnowRemovePeer(settings.espnowPeers[x]);
 
   //Leave WiFi with default settings (no WIFI_PROTOCOL_LR for ESP NOW)
-  esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N); //Stops WiFi Station.
+  //esp_wifi_set_protocol requires WiFi to be started
+  response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
+  if (response != ESP_OK)
+    systemPrintf("espnowStop: Error setting WiFi protocols: %s\r\n", esp_err_to_name(response));
+  else
+    log_d("WiFi on, ESP-Now added to protocols");
 
   //Deinit ESP-NOW
   if (esp_now_deinit() != ESP_OK) {
@@ -325,10 +349,11 @@ esp_err_t espnowAddPeer(uint8_t *peerMac, bool encrypt)
 esp_err_t espnowRemovePeer(uint8_t *peerMac)
 {
 #ifdef COMPILE_ESPNOW
-  esp_err_t result = esp_now_del_peer(peerMac);
-  if (result != ESP_OK)
-    log_d("Failed to remove peer");
-  return (result);
+  esp_err_t response = esp_now_del_peer(peerMac);
+  if (response != ESP_OK)
+    log_d("Failed to remove peer: %s", esp_err_to_name(response));
+
+  return (response);
 #else
   return (ESP_OK);
 #endif
@@ -417,7 +442,7 @@ void espnowStaticPairing()
   bool exitPair = false;
   while (exitPair == false)
   {
-    if (Serial.available())
+    if (systemAvailable())
     {
       systemPrintln("User pressed button. Pairing canceled.");
       break;

Firmware/RTK_Surveyor/States.ino

@@ -613,7 +613,7 @@ void updateSystemState()
             {
               systemPrint("Parsing: ");
               for (int x = 0 ; x < incomingSettingsSpot ; x++)
-                Serial.write(incomingSettings[x]);
+                systemWrite(incomingSettings[x]);
               systemPrintln();
 
               parseIncomingSettings();

Firmware/RTK_Surveyor/System.ino

@@ -585,7 +585,7 @@ bool setConstellations(bool sendCompleteBatch)
 //Periodically print position if enabled
 void printPosition()
 {
-    //Periodically print the position
+  //Periodically print the position
   if (settings.enablePrintPosition && ((millis() - lastPrintPosition) > 15000))
   {
     printCurrentConditions();

Firmware/RTK_Surveyor/WiFi.ino

@@ -135,13 +135,17 @@ bool wifiStartAP()
   {
     //Stop any current WiFi activity
     wifiStop();
-    
+
     //Start in AP mode
     WiFi.mode(WIFI_AP);
 
-    //Before attempting WiFiMulti connect, be sure we have default WiFi protocols enabled.
-    //This must come after WiFi.mode
-    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
+    //Before starting AP mode, be sure we have default WiFi protocols enabled.
+    //esp_wifi_set_protocol requires WiFi to be started
+    esp_err_t response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
+    if (response != ESP_OK)
+      systemPrintf("wifiStartAP: Error setting WiFi protocols: %s\r\n", esp_err_to_name(response));
+    else
+      log_d("WiFi protocols set");
 
     IPAddress local_IP(192, 168, 4, 1);
     IPAddress gateway(192, 168, 4, 1);
@@ -159,8 +163,6 @@ bool wifiStartAP()
   else
   {
     //Start webserver on local WiFi instead of AP
-    log_d("WiFi Config starting WiFi");
-    wifiStart(); //Makes sure any ESP-Now settings have been cleared
 
     //Attempt to connect to local WiFi with increasing timeouts
     int timeout = 0;
@@ -179,11 +181,11 @@ bool wifiStartAP()
     {
       displayNoWiFi(2000);
       requestChangeState(STATE_ROVER_NOT_STARTED);
-      return(false);
+      return (false);
     }
   }
 
-  return(true);
+  return (true);
 }
 
 #endif  //COMPILE_WIFI
@@ -281,7 +283,7 @@ void wifiStart()
   if (wifiNetworkCount() == 0)
   {
     systemPrintln("Error: Please enter at least one SSID before using WiFi");
-    //paintWiFiNoNetworksFail(4000, true); //TODO
+    //paintNoWiFi(2000); //TODO
     wifiStop();
     return;
   }
@@ -292,16 +294,6 @@ void wifiStart()
 
   log_d("Starting WiFi");
 
-  WiFi.mode(WIFI_STA);
-
-  //Before attempting WiFiMulti connect, be sure we have default WiFi protocols enabled.
-  //This must come after WiFi.mode
-  esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N);
-
-  //If ESP-NOW is running, blend in ESP-NOW protocol.
-  if (espnowState > ESPNOW_OFF)
-    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR); //Enable WiFi + ESP-Now. Stops WiFi Station.
-
   wifiSetState(WIFI_CONNECTING); //This starts the state machine running
 
   //Display the heap state
@@ -341,19 +333,25 @@ void wifiStop()
 
   wifiConnectionAttempts = 0; //Reset the timeout
 
-  //No matter what, turn off WiFi to stop rolling STA_DISCONNECTED and AUTH_EXPIRE notifications
-  WiFi.mode(WIFI_OFF);
-  log_d("WiFi Stopped");
 
   //If ESP-Now is active, change protocol to only Long Range and re-start WiFi
   if (espnowState > ESPNOW_OFF)
   {
-    // Enable long range, PHY rate of ESP32 will be 512Kbps or 256Kbps
-    esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_LR); //Stops WiFi Station.
-
-    WiFi.mode(WIFI_STA);
-
-    log_d("WiFi disabled, ESP-Now left in place");
+    if (WiFi.getMode() == WIFI_OFF)
+      WiFi.mode(WIFI_STA);
+
+    //Enable long range, PHY rate of ESP32 will be 512Kbps or 256Kbps
+    //esp_wifi_set_protocol requires WiFi to be started
+    esp_err_t response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_LR);
+    if (response != ESP_OK)
+      systemPrintf("wifiStop: Error setting ESP-Now lone protocol: %s\r\n", esp_err_to_name(response));
+    else
+      log_d("WiFi disabled, ESP-Now left in place");
+  }
+  else
+  {
+    WiFi.mode(WIFI_OFF);
+    log_d("WiFi Stopped");
   }
 
   //Display the heap state
@@ -381,8 +379,38 @@ bool wifiConnect(unsigned long timeout)
 {
 #ifdef COMPILE_WIFI
 
-  if (wifiIsConnected())
-    return (true);
+  if (wifiIsConnected()) return (true); //Nothing to do
+
+  //Before we can issue esp_wifi_() commands WiFi must be started
+  if (WiFi.getMode() == WIFI_OFF)
+    WiFi.mode(WIFI_STA);
+
+  //Verify that the necessary protocols are set
+  uint8_t protocols = 0;
+  esp_err_t response = esp_wifi_get_protocol(WIFI_IF_STA, &protocols);
+  if (response != ESP_OK)
+    systemPrintf("wifiConnect: Failed to get protocols: %s\r\n", esp_err_to_name(response));
+
+  //If ESP-NOW is running, blend in ESP-NOW protocol.
+  if (espnowState > ESPNOW_OFF)
+  {
+    if (protocols != (WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR))
+    {
+      esp_err_t response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N | WIFI_PROTOCOL_LR); //Enable WiFi + ESP-Now.
+      if (response != ESP_OK)
+        systemPrintf("wifiConnect: Error setting WiFi + ESP-NOW protocols: %s\r\n", esp_err_to_name(response));
+    }
+  }
+  else
+  {
+    //Make sure default WiFi protocols are in place
+    if (protocols != (WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N))
+    {
+      esp_err_t response = esp_wifi_set_protocol(WIFI_IF_STA, WIFI_PROTOCOL_11B | WIFI_PROTOCOL_11G | WIFI_PROTOCOL_11N); //Enable WiFi.
+      if (response != ESP_OK)
+        systemPrintf("wifiConnect: Error setting WiFi + ESP-NOW protocols: %s\r\n", esp_err_to_name(response));
+    }
+  }
 
   WiFiMulti wifiMulti;
 
@@ -395,16 +423,16 @@ bool wifiConnect(unsigned long timeout)
 
   systemPrint("Connecting WiFi... ");
 
-  int response = wifiMulti.run(timeout);
-  if (response == WL_CONNECTED)
+  int wifiResponse = wifiMulti.run(timeout);
+  if (wifiResponse == WL_CONNECTED)
   {
     systemPrintln();
     return true;
   }
-  else if (response == WL_DISCONNECTED)
+  else if (wifiResponse == WL_DISCONNECTED)
     systemPrint("No friendly WiFi networks detected. ");
   else
-    systemPrintf("WiFi failed to connect: error #%d. ", response);
+    systemPrintf("WiFi failed to connect: error #%d. ", wifiResponse);
 
 #endif
 
@@ -600,6 +628,8 @@ void tcpUpdate()
 
   if (settings.enableTcpClient == false && settings.enableTcpServer == false) return; //Nothing to do
 
+  if (wifiInConfigMode()) return; //Do not service TCP during WiFi config
+
   //If TCP is enabled, but WiFi is not connected, start WiFi
   if (wifiIsConnected() == false && (settings.enableTcpClient == true || settings.enableTcpServer == true))
   {

Firmware/RTK_Surveyor/menuBase.ino

@@ -31,10 +31,10 @@ void menuBase()
       {
         systemPrint("3) Set Lat/Long/Altitude coordinates: ");
         systemPrint(settings.fixedLat, haeNumberOfDecimals);
-        Serial.write(167); //°
+        systemWrite(167); //°
         systemPrint(", ");
         systemPrint(settings.fixedLong, haeNumberOfDecimals);
-        Serial.write(167); //°
+        systemWrite(167); //°
         systemPrint(", ");
         systemPrint(settings.fixedAltitude, 4);
         systemPrintln("m");

Firmware/RTK_Surveyor/menuFirmware.ino

@@ -47,9 +47,6 @@ void menuFirmware()
     {
       bool previouslyConnected = wifiIsConnected();
 
-      log_d("Firmware version Check starting WiFi");
-      wifiStart(); //Makes sure any ESP-Now settings have been cleared
-
       //Attempt to connect to local WiFi
       if (wifiConnect(10000) == true)
       {
@@ -349,9 +346,6 @@ bool otaCheckVersion(char *versionAvailable, uint8_t versionAvailableLength)
 #ifdef COMPILE_WIFI
   bool previouslyConnected = wifiIsConnected();
 
-  log_d("OTA Check Version starting WiFi");
-  wifiStart(); //Makes sure any ESP-Now settings have been cleared
-
   if (wifiConnect(10000) == true)
   {
     char versionString[20];
@@ -395,7 +389,9 @@ bool otaCheckVersion(char *versionAvailable, uint8_t versionAvailableLength)
     }
   }
   else
-    systemPrintln("WiFi not available");
+  {
+    systemPrintln("WiFi not available.");
+  }
 
   if (systemState != STATE_WIFI_CONFIG)
   {
@@ -422,9 +418,6 @@ void otaUpdate()
 #ifdef COMPILE_WIFI
   bool previouslyConnected = wifiIsConnected();
 
-  log_d("OTA Update starting WiFi");
-  wifiStart(); //Makes sure any ESP-Now settings have been cleared
-
   if (wifiConnect(10000) == true)
   {
     char versionString[20];
@@ -470,7 +463,9 @@ void otaUpdate()
     }
   }
   else
-    systemPrintln("WiFi not available");
+  {
+    systemPrintln("WiFi not available.");
+  }
 
   //Update failed. If WiFi was originally off, turn it off again
   if (previouslyConnected == false)