From 7c2b558ee942e22961fbd85ffa26bb8a9beb8052 Mon Sep 17 00:00:00 2001
From: ladyada <limor@ladyada.net>
Date: Thu, 22 Oct 2015 13:03:55 -0400
Subject: [PATCH] ping flushes input, tries multiple times

---
 Adafruit_MQTT.cpp | 11 +++++++++--
 Adafruit_MQTT.h   |  4 +++-
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/Adafruit_MQTT.cpp b/Adafruit_MQTT.cpp
index 094af2a..7a87f16 100644
--- a/Adafruit_MQTT.cpp
+++ b/Adafruit_MQTT.cpp
@@ -368,13 +368,20 @@ Adafruit_MQTT_Subscribe *Adafruit_MQTT::readSubscription(int16_t timeout) {
   return subscriptions[i];
 }
 
-bool Adafruit_MQTT::ping(uint8_t num = 1) {
+void Adafruit_MQTT::flushIncoming(uint16_t timeout) {
+  // flush input!
+  DEBUG_PRINTLN(F("Flushing input buffer"));
+  while (readPacket(buffer, MAXBUFFERSIZE, timeout));
+}
+
+bool Adafruit_MQTT::ping(uint8_t num) {
+  flushIncoming(100);
 
   while (num--) {
     // Construct and send ping packet.
     uint8_t len = pingPacket(buffer);
     if (!sendPacket(buffer, len))
-      return false;
+      continue;
     
     // Process ping reply.
     len = readPacket(buffer, 2, PING_TIMEOUT_MS);
diff --git a/Adafruit_MQTT.h b/Adafruit_MQTT.h
index 2b9341a..7bba0a4 100644
--- a/Adafruit_MQTT.h
+++ b/Adafruit_MQTT.h
@@ -178,7 +178,7 @@ class Adafruit_MQTT {
   Adafruit_MQTT_Subscribe *readSubscription(int16_t timeout=0);
 
   // Ping the server to ensure the connection is still alive.
-  bool ping(uint8_t n);
+  bool ping(uint8_t n = 1);
 
  protected:
   // Interface that subclasses need to implement:
@@ -215,6 +215,8 @@ class Adafruit_MQTT {
  private:
   Adafruit_MQTT_Subscribe *subscriptions[MAXSUBSCRIPTIONS];
 
+  void    flushIncoming(uint16_t timeout);
+
   // Functions to generate MQTT packets.
   uint8_t connectPacket(uint8_t *packet);
   uint8_t disconnectPacket(uint8_t *packet);