diff --git a/code/lib-src/umqtt/robust.py b/code/lib-src/umqtt/robust.py
index 983834f..f648504 100644
--- a/code/lib-src/umqtt/robust.py
+++ b/code/lib-src/umqtt/robust.py
@@ -6,39 +6,68 @@ class MQTTClient(simple.MQTTClient):
 
     DELAY = 2
     DEBUG = False
+    is_connected = False
 
-    def delay(self, i):
-        time.sleep(self.DELAY)
-
-    def log(self, in_reconnect, e):
+    def log(self, in_reconnect, err):
+        """Log error if DEBUG is True"""
         if self.DEBUG:
             if in_reconnect:
-                print("mqtt reconnect: %r" % e)
+                print("mqtt reconnect: %r" % err)
             else:
-                print("mqtt: %r" % e)
+                print("mqtt: %r" % err)
 
     def reconnect(self):
+        """Try to connect or reconnect to MQTT broker"""
         i = 0
         while 1:
             try:
+                self.is_connected = True
                 return super().connect(False)
-            except OSError as e:
-                self.log(True, e)
+            except OSError as err:
+                self.is_connected = False
+                self.log(True, err)
                 i += 1
-                self.delay(i)
+                time.sleep(self.DELAY)
+            if i >= 5:
+                print("Mqtt reconnect : too much retry")
+                self.is_connected = False
+                break
+
 
     def publish(self, topic, msg, retain=False, qos=0):
-        while 1:
-            try:
-                return super().publish(topic, msg, retain, qos)
-            except OSError as e:
-                self.log(False, e)
-            self.reconnect()
+        """Try to publish if connected to MQTT broker"""
+        if self.is_connected:
+            i = 0
+            while 1:
+                try:
+                    return super().publish(topic, msg, retain, qos)
+                except OSError as e:
+                    i += 1
+                    self.log(False, e)
+                if i >= 5:
+                    print("Mqtt publish : too much retry")
+                    self.is_connected = False
+                    break
+                else:
+                    self.reconnect()
+        else:
+            print("Mqtt publish: not connected")
 
     def wait_msg(self):
-        while 1:
-            try:
-                return super().wait_msg()
-            except OSError as e:
-                self.log(False, e)
-            self.reconnect()
+        """Check if any subscribed messages is available"""
+        if self.is_connected:
+            i = 0
+            while 1:
+                try:
+                    return super().wait_msg()
+                except OSError as e:
+                    i += 1
+                    self.log(False, e)
+                if i >= 5:
+                    print("Mqtt wait_msg : too much retry")
+                    self.is_connected = False
+                    break
+                else:
+                    self.reconnect()
+        else:
+            print("Mqtt wait_msg: not connected")
diff --git a/code/lib/umqtt/robust.mpy b/code/lib/umqtt/robust.mpy
index bebc63d..6021a3c 100644
Binary files a/code/lib/umqtt/robust.mpy and b/code/lib/umqtt/robust.mpy differ