Fix memory issues on low free memory

* fix memory issues on low free memory
* add queue size

docs/index.md

@@ -341,6 +341,12 @@ const char* getClientId() const
 
 Retuns the client ID.
 
+```cpp
+size_t queueSize();
+```
+
+Returns the amount of elements, regardless of type, in the queue.
+
 # Compile time configuration
 
 A number of constants which influence the behaviour of the client can be set at compile time. You can set these options in the `Config.h` file or pass the values as compiler flags. Because these options are compile-time constants, they are used for all instances of `espMqttClient` you create in your program.

src/MqttClient.cpp

@@ -194,6 +194,14 @@ const char* MqttClient::getClientId() const {
   return _clientId;
 }
 
+size_t MqttClient::queueSize() {
+  size_t ret = 0;
+  EMC_SEMAPHORE_TAKE();
+  ret = _outbox.size();
+  EMC_SEMAPHORE_GIVE();
+  return ret;
+}
+
 void MqttClient::loop() {
   switch (_state) {
     case State::disconnected:

src/MqttClient.h

@@ -65,6 +65,7 @@ class MqttClient {
   uint16_t publish(const char* topic, uint8_t qos, bool retain, espMqttClientTypes::PayloadCallback callback, size_t length);
   void clearQueue(bool deleteSessionData = false);  // Not MQTT compliant and may cause unpredictable results when `deleteSessionData` = true!
   const char* getClientId() const;
+  size_t queueSize();  // No const because of mutex
   void loop();
 
  protected:
@@ -130,7 +131,7 @@ class MqttClient {
     uint32_t timeSent;
     espMqttClientInternals::Packet packet;
     template <typename... Args>
-    OutgoingPacket(uint32_t t, espMqttClientTypes::Error error, Args&&... args) :
+    OutgoingPacket(uint32_t t, espMqttClientTypes::Error& error, Args&&... args) :  // NOLINT(runtime/references)
       timeSent(t),
       packet(error, std::forward<Args>(args) ...) {}
   };
@@ -148,16 +149,24 @@ class MqttClient {
   bool _addPacket(Args&&... args) {
     espMqttClientTypes::Error error(espMqttClientTypes::Error::SUCCESS);
     espMqttClientInternals::Outbox<OutgoingPacket>::Iterator it = _outbox.emplace(0, error, std::forward<Args>(args) ...);
-    if (it && error == espMqttClientTypes::Error::SUCCESS) return true;
-    return false;
+    if (it && error == espMqttClientTypes::Error::SUCCESS) {
+      return true;
+    } else {
+      if (it) _outbox.remove(it);
+      return false;
+    }
   }
 
   template <typename... Args>
   bool _addPacketFront(Args&&... args) {
     espMqttClientTypes::Error error(espMqttClientTypes::Error::SUCCESS);
     espMqttClientInternals::Outbox<OutgoingPacket>::Iterator it = _outbox.emplaceFront(0, error, std::forward<Args>(args) ...);
-    if (it && error == espMqttClientTypes::Error::SUCCESS) return true;
-    return false;
+    if (it && error == espMqttClientTypes::Error::SUCCESS) {
+      return true;
+    } else {
+      if (it) _outbox.remove(it);
+      return false;
+    }
   }
 
   void _checkOutbox();

src/Outbox.h

@@ -163,6 +163,16 @@ class Outbox {
     return false;
   }
 
+  size_t size() const {
+    Node* n = _first;
+    size_t count = 0;
+    while (n) {
+      n = n->next;
+      ++count;
+    }
+    return count;
+  }
+
  private:
   Node* _first;
   Node* _last;

src/Packets/Packet.cpp

@@ -100,7 +100,7 @@ Packet::Packet(espMqttClientTypes::Error& error,
   (password ? 2 + strlen(password) : 0);
 
   // allocate memory
-  if (!_allocate(remainingLength)) {
+  if (!_allocate(remainingLength, false)) {
     error = espMqttClientTypes::Error::OUT_OF_MEMORY;
     return;
   }
@@ -300,8 +300,8 @@ Packet::Packet(espMqttClientTypes::Error& error, MQTTPacketType type)
 }
 
 
-bool Packet::_allocate(size_t remainingLength) {
-  if (EMC_GET_FREE_MEMORY() < EMC_MIN_FREE_MEMORY) {
+bool Packet::_allocate(size_t remainingLength, bool check) {
+  if (check && EMC_GET_FREE_MEMORY() < EMC_MIN_FREE_MEMORY) {
     emc_log_w("Packet buffer not allocated: low memory");
     return false;
   }

src/Packets/Packet.h

@@ -133,7 +133,7 @@ class Packet {
 
  private:
   // pass remainingLength = total size - header - remainingLengthLength!
-  bool _allocate(size_t remainingLength);
+  bool _allocate(size_t remainingLength, bool check = true);
 
   // fills header and returns index of next available byte in buffer
   size_t _fillPublishHeader(uint16_t packetId,