WiFi connection is now part of the main loop and don't block other functions, sensors can work unconnected for debug purposes

src/defines.h

@@ -21,7 +21,6 @@
     THE SOFTWARE.
 */
 
-//#define FULL_DEBUG
 
 #define IMU_MPU9250 1
 #define IMU_MPU6500 2
@@ -64,8 +63,11 @@
 #endif
 
 //Debug information
+//#define FULL_DEBUG
 #define serialDebug false // Set to true to get Serial output for debugging
 #define serialBaudRate 115200
+#define UPDATE_IMU_UNCONNECTED 1
+#define SEND_UPDATES_UNCONNECTED 1
 
 // Determines how often we sample and send data
 #define samplingRateInMillis 10

src/main.cpp

@@ -84,7 +84,7 @@ void setup()
     // join I2C bus
     Wire.flush();
     Wire.begin(D2, D1);
-    Wire.setClockStretchLimit(1000);
+    Wire.setClockStretchLimit(4000);
     Serial.begin(serialBaudRate);
     while (!Serial)
         ; // wait for connection
@@ -127,30 +127,40 @@ void setup()
 
 void loop()
 {
+    wifiUpkeep();
     otaUpdate();
     clientUpdate();
-    if(isConnected())
+    if (isCalibrating)
     {
-        if (isCalibrating)
-        {
-            sensor.startCalibration(0);
-            isCalibrating = false;
+        sensor.startCalibration(0);
+        isCalibrating = false;
+    }
+    #ifndef UPDATE_IMU_UNCONNECTED
+        if(isConnected()) {
+    #endif
+    sensor.motionLoop();
+    #ifndef UPDATE_IMU_UNCONNECTED
         }
-        sensor.motionLoop();
-        // Send updates
-        now_ms = millis();
-        if (now_ms - last_ms >= samplingRateInMillis)
-        {
-            last_ms = now_ms;
-            processBlinking();
+    #endif
+    // Send updates
+    now_ms = millis();
+    if (now_ms - last_ms >= samplingRateInMillis)
+    {
+        last_ms = now_ms;
+        processBlinking();
 
-            sensor.sendData();
-        }
-        if(now_ms - last_battery_sample >= batterySampleRate) {
-            last_battery_sample = now_ms;
-            float battery = ((float) analogRead(A0)) * batteryADCMultiplier;
-            sendFloat(battery, PACKET_BATTERY_LEVEL);
-        }
+        #ifndef SEND_UPDATES_UNCONNECTED
+            if(isConnected()) {
+        #endif
+        sensor.sendData();
+        #ifndef SEND_UPDATES_UNCONNECTED
+            }
+        #endif
+    }
+    if(now_ms - last_battery_sample >= batterySampleRate) {
+        last_battery_sample = now_ms;
+        float battery = ((float) analogRead(A0)) * batteryADCMultiplier;
+        sendFloat(battery, PACKET_BATTERY_LEVEL);
     }
 }
 

src/udpclient.cpp

@@ -42,6 +42,8 @@ IPAddress host;
 bool connected = false;
 unsigned long lastConnectionAttemptMs;
 unsigned long lastPacketMs;
+unsigned long lastWifiReportTime = 0;
+bool isWifiConnected = false;
 
 uint8_t serialBuffer[128];
 size_t serialLength = 0;
@@ -316,7 +318,7 @@ void setCommandRecievedCallback(commandRecievedCallback callback)
 
 void clientUpdate()
 {
-    if (WiFi.status() == WL_CONNECTED)
+    if (isWifiConnected)
     {
         if(connected) {
             int packetSize = Udp.parsePacket();
@@ -415,33 +417,48 @@ void setUpWiFi(DeviceConfig * const config) {
     WiFi.mode(WIFI_STA);
 
     WiFi.begin(WiFi.SSID().c_str(), WiFi.psk().c_str());
-    while (WiFi.status() == WL_DISCONNECTED)
+}
+
+namespace {
+    void reportWifiError() {
+        if(lastWifiReportTime + 1000 < millis()) {
+            lastWifiReportTime = millis();
+            Serial.print(".");
+        }
+    }
+}
+
+void wifiUpkeep() {
+    if(isWifiConnected)
+        return;
+
+    if(WiFi.status() == WL_DISCONNECTED)
     {
-        delay(500);
-        Serial.print(".");
+        // WiFi is in disconnected state, wait until it connects or erros out
+        reportWifiError();
+        return;
     }
     if(WiFi.status() != WL_CONNECTED) {
-        Serial.printf("\nCould not connect to WiFi. state='%d'\n", WiFi.status());
-        Serial.println("Please press WPS button on your router, until mode is indicated.");
+        // Wifi is not connected, try connecting with WPS
+        //Serial.printf("\nCould not connect to WiFi. state='%d'\n", WiFi.status());
+        //Serial.println("Please press WPS button on your router, until mode is indicated.");
 
-        while(true) {
+        //while(true) {
             if(!startWPSPBC()) {
                 Serial.println("Failed to connect with WPS");
             } else {
                 WiFi.begin(WiFi.SSID().c_str(), WiFi.psk().c_str()); // reading data from EPROM, 
-                while (WiFi.status() == WL_DISCONNECTED) {          // last saved credentials
-                    delay(500);
-                    Serial.print("."); // show wait for connect to AP
-                }
+                // WiFi will start connecting here until errors out or connects
             }
-            if(WiFi.status() == WL_CONNECTED)
-                break;
-            delay(1000);
-        }
+        //}
     }
-    Serial.printf("\nConnected successfully to SSID '%s', ip address %s\n", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
 
-    Udp.begin(port);
+    if(WiFi.status() == WL_CONNECTED && !isWifiConnected) {
+        // Wasn't connected, but now connected, stop waiting
+        isWifiConnected = true;
+        Serial.printf("\nConnected successfully to SSID '%s', ip address %s\n", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
+        Udp.begin(port);
+    }
 }
 
 bool isConnected() {

src/udpclient.h

@@ -70,6 +70,7 @@ void setConfigRecievedCallback(configRecievedCallback);
 void setCommandRecievedCallback(commandRecievedCallback);
 void sendSerial(uint8_t *const data, int length, int type);
 void setUpWiFi(DeviceConfig * const config);
+void wifiUpkeep();
 bool isConnected();
 
 template<typename T> T convert_chars(unsigned char* src);