Iniital commit

Software/USER_SETTINGS.cpp

@@ -15,6 +15,13 @@ volatile uint16_t MAXCHARGEAMP =
 volatile uint16_t MAXDISCHARGEAMP =
     300;  //30.0A , BYD CAN specific setting, Max discharge speed in Amp (Some inverters needs to be artificially limited)
 
+// MQTT
+// For more detailed settings, see mqtt.h
+#ifdef MQTT
+const char* mqtt_user = "REDACTED";
+const char* mqtt_password = "REDACTED";
+#endif  // USE_MQTT
+
 #ifdef WEBSERVER
 volatile uint8_t AccessPointEnabled =
     true;  //Set to either true or false incase you want the board to enable a direct wifi access point

Software/USER_SETTINGS.h

@@ -12,7 +12,7 @@
 //#define CHADEMO_BATTERY
 //#define IMIEV_CZERO_ION_BATTERY
 //#define KIA_HYUNDAI_64_BATTERY
-//#define NISSAN_LEAF_BATTERY
+#define NISSAN_LEAF_BATTERY
 //#define RENAULT_KANGOO_BATTERY
 //#define RENAULT_ZOE_BATTERY
 //#define SANTA_FE_PHEV_BATTERY
@@ -21,7 +21,7 @@
 
 /* Select inverter communication protocol. See Wiki for which to use with your inverter: https://github.com/dalathegreat/BYD-Battery-Emulator-For-Gen24/wiki */
 //#define BYD_CAN          //Enable this line to emulate a "BYD Battery-Box Premium HVS" over CAN Bus
-//#define BYD_MODBUS       //Enable this line to emulate a "BYD 11kWh HVM battery" over Modbus RTU
+#define BYD_MODBUS       //Enable this line to emulate a "BYD 11kWh HVM battery" over Modbus RTU
 //#define LUNA2000_MODBUS  //Enable this line to emulate a "Luna2000 battery" over Modbus RTU
 //#define PYLON_CAN        //Enable this line to emulate a "Pylontech battery" over CAN bus
 //#define SMA_CAN          //Enable this line to emulate a "BYD Battery-Box H 8.9kWh, 7 mod" over CAN bus
@@ -30,13 +30,14 @@
 
 /* Other options */
 #define DEBUG_VIA_USB  //Enable this line to have the USB port output serial diagnostic data while program runs
-//define INTERLOCK_REQUIRED //Nissan LEAF specific setting, if enabled requires both high voltage conenctors to be seated before starting
+#define INTERLOCK_REQUIRED //Nissan LEAF specific setting, if enabled requires both high voltage conenctors to be seated before starting
 //#define CONTACTOR_CONTROL     //Enable this line to have pins 25,32,33 handle automatic precharge/contactor+/contactor- closing sequence
 //#define PWM_CONTACTOR_CONTROL //Enable this line to use PWM logic for contactors, which lower power consumption and heat generation
 //#define DUAL_CAN              //Enable this line to activate an isolated secondary CAN Bus using add-on MCP2515 controller (Needed for FoxESS inverters)
 //#define SERIAL_LINK_RECEIVER  //Enable this line to receive battery data over RS485 pins from another Lilygo (This LilyGo interfaces with inverter)
 //#define SERIAL_LINK_TRANSMITTER  //Enable this line to send battery data over RS485 pins to another Lilygo (This LilyGo interfaces with battery)
-//#define WEBSERVER  //Enable this line to enable WiFi, and to run the webserver. See USER_SETTINGS.cpp for the Wifi settings.
+#define WEBSERVER  //Enable this line to enable WiFi, and to run the webserver. See USER_SETTINGS.cpp for the Wifi settings.
+#define MQTT  // Enable this line to enable MQTT
 
 /* Battery limits: These are set in the USER_SETTINGS.cpp file, or later on via the Webserver */
 extern volatile uint16_t BATTERY_WH_MAX;

Software/src/devboard/mqtt/mqtt.cpp

@@ -0,0 +1,94 @@
+#include "mqtt.h"
+#include <Arduino.h>
+#include <WiFi.h>
+#include <freertos/FreeRTOS.h>
+#include "../../../USER_SETTINGS.h"
+#include "../../lib/knolleary-pubsubclient/PubSubClient.h"
+
+const char* mqtt_subscriptions[] = MQTT_SUBSCRIPTIONS;
+const size_t mqtt_nof_subscriptions = sizeof(mqtt_subscriptions) / sizeof(mqtt_subscriptions[0]);
+
+WiFiClient espClient;
+PubSubClient client(espClient);
+char msg[MSG_BUFFER_SIZE];
+int value = 0;
+static unsigned long previousMillisUpdateVal;
+
+/** Publish global values and call callbacks for specific modules */
+static void publish_values(void) {
+
+  snprintf(msg, sizeof(msg),
+           "{\n"
+           "  \"SOC\": %.3f,\n"
+           "  \"StateOfHealth\": %.3f,\n"
+           "  \"temperature_min\": %.3f,\n"
+           "  \"temperature_max\": %.3f,\n"
+           "  \"cell_max_voltage\": %d,\n"
+           "  \"cell_min_voltage\": %d\n"
+           "}\n",
+           ((float)SOC) / 100.0, ((float)StateOfHealth) / 100.0, ((float)((int16_t)temperature_min)) / 10.0,
+           ((float)((int16_t)temperature_max)) / 10.0, cell_max_voltage, cell_min_voltage);
+  bool result = client.publish("battery/info", msg, true);
+  Serial.println(msg);  // Uncomment to print the payload on serial
+}
+
+/* This is called whenever a subscribed topic changes (hopefully) */
+static void callback(char* topic, byte* payload, unsigned int length) {
+  Serial.print("Message arrived [");
+  Serial.print(topic);
+  Serial.print("] ");
+  for (unsigned int i = 0; i < length; i++) {
+    Serial.print((char)payload[i]);
+  }
+  Serial.println();
+}
+
+/* If we lose the connection, get it back and re-sub */
+static void reconnect() {
+  // attempt one reconnection
+  Serial.print("Attempting MQTT connection... ");
+  // Create a random client ID
+  String clientId = "LilyGoClient-";
+  clientId += String(random(0xffff), HEX);
+  // Attempt to connect
+  if (client.connect(clientId.c_str(), mqtt_user, mqtt_password)) {
+    Serial.println("connected");
+
+    for (int i = 0; i < mqtt_nof_subscriptions; i++) {
+      client.subscribe(mqtt_subscriptions[i]);
+      Serial.print("Subscribed to: ");
+      Serial.println(mqtt_subscriptions[i]);
+    }
+  } else {
+    Serial.print("failed, rc=");
+    Serial.print(client.state());
+    Serial.println(" try again in 5 seconds");
+    // Wait 5 seconds before retrying
+  }
+}
+
+void init_mqtt(void) {
+  client.setServer(MQTT_SERVER, MQTT_PORT);
+  client.setCallback(callback);
+  Serial.println("MQTT initialized");
+
+  previousMillisUpdateVal = millis();
+  reconnect();
+}
+
+void mqtt_loop(void) {
+  if (client.connected()) {
+    client.loop();
+    if (millis() - previousMillisUpdateVal >= 5000)  // Every 5s
+    {
+      previousMillisUpdateVal = millis();
+      publish_values();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
+    }
+  } else {
+    if (millis() - previousMillisUpdateVal >= 5000)  // Every 5s
+    {
+      previousMillisUpdateVal = millis();
+      reconnect();  // Update values heading towards inverter. Prepare for sending on CAN, or write directly to Modbus.
+    }
+  }
+}

Software/src/devboard/mqtt/mqtt.h

@@ -0,0 +1,59 @@
+/**
+ * MQTT add-on for the battery emulator
+ * 
+ * Usage:
+ * 
+ * Subscription - Add topics to MQTT_SUBSCRIPTIONS in USER_SETTINGS.h and handle the messages in mqtt.cpp:callback()
+ * 
+ * Publishing - See example in mqtt.cpp:publish_values() for constructing the payload
+ * 
+ * Home assistant - See below for an example, and the official documentation is quite good (https://www.home-assistant.io/integrations/sensor.mqtt/)
+ * in configuration.yaml:
+ * mqtt: !include mqtt.yaml
+ * 
+ * in mqtt.yaml:
+ * sensor:
+ *   - name: "Cell max"
+ *       state_topic: "battery/info"
+ *       unit_of_measurement: "mV"
+ *       value_template: "{{ value_json.cell_max_voltage | int }}"
+ *   - name: "Cell min"
+ *       state_topic: "battery/info"
+ *       unit_of_measurement: "mV"
+ *       value_template: "{{ value_json.cell_min_voltage | int }}"
+ *   - name: "Temperature max"
+ *       state_topic: "battery/info"
+ *       unit_of_measurement: "C"
+ *       value_template: "{{ value_json.temperature_max | float }}"
+ *   - name: "Temperature min"
+ *       state_topic: "battery/info"
+ *       unit_of_measurement: "C"
+ *       value_template: "{{ value_json.temperature_min | float }}"
+ */
+
+#ifndef __MQTT_H__
+#define __MQTT_H__
+
+#include <Arduino.h>
+#include "../../../USER_SETTINGS.h"
+
+#define MSG_BUFFER_SIZE (256)
+#define MQTT_SUBSCRIPTIONS \
+  { "my/topic/abc", "my/other/topic" }
+#define MQTT_SERVER "192.168.xxx.yyy"
+#define MQTT_PORT 1883
+
+extern uint16_t SOC;
+extern uint16_t StateOfHealth;
+extern uint16_t temperature_min;   //C+1,  Goes thru convert2unsignedint16 function (15.0C = 150, -15.0C =  65385)
+extern uint16_t temperature_max;   //C+1,  Goes thru convert2unsignedint16 function (15.0C = 150, -15.0C =  65385)
+extern uint16_t cell_max_voltage;  //mV,   0-4350
+extern uint16_t cell_min_voltage;  //mV,   0-4350
+
+extern const char* mqtt_user;
+extern const char* mqtt_password;
+
+void init_mqtt(void);
+void mqtt_loop(void);
+
+#endif

Software/src/devboard/webserver/webserver.cpp

@@ -132,6 +132,11 @@ void init_webserver() {
 
   // Start server
   server.begin();
+
+  #ifdef MQTT
+  // Init MQTT
+  init_mqtt();
+  #endif
 }
 
 void init_WiFi_AP() {

Software/src/devboard/webserver/webserver.h

@@ -5,10 +5,14 @@
 #include <WiFi.h>
 #include "../../../USER_SETTINGS.h"  // Needed for WiFi ssid and password
 #include "../../lib/ayushsharma82-ElegantOTA/src/ElegantOTA.h"
+#ifdef MQTT
+#include "../../lib/knolleary-pubsubclient/PubSubClient.h"
+#endif
 #include "../../lib/me-no-dev-AsyncTCP/src/AsyncTCP.h"
 #include "../../lib/me-no-dev-ESPAsyncWebServer/src/ESPAsyncWebServer.h"
 #include "../../lib/miwagner-ESP32-Arduino-CAN/ESP32CAN.h"
 #include "../config.h"  // Needed for LED defines
+#include "../mqtt/mqtt.h"  // Needed for LED defines
 
 extern uint16_t SOC;                         //SOC%, 0-100.00 (0-10000)
 extern uint16_t StateOfHealth;               //SOH%, 0-100.00 (0-10000)

Software/src/lib/knolleary-pubsubclient/PubSubClient.cpp

@@ -0,0 +1,769 @@
+/*
+
+  PubSubClient.cpp - A simple client for MQTT.
+  Nick O'Leary
+  http://knolleary.net
+*/
+
+#include "PubSubClient.h"
+#include "Arduino.h"
+
+PubSubClient::PubSubClient() {
+    this->_state = MQTT_DISCONNECTED;
+    this->_client = NULL;
+    this->stream = NULL;
+    setCallback(NULL);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+
+PubSubClient::PubSubClient(Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+
+PubSubClient::PubSubClient(IPAddress addr, uint16_t port, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(addr, port);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(IPAddress addr, uint16_t port, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(addr,port);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(IPAddress addr, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(addr, port);
+    setCallback(callback);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(IPAddress addr, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(addr,port);
+    setCallback(callback);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+
+PubSubClient::PubSubClient(uint8_t *ip, uint16_t port, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(ip, port);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(uint8_t *ip, uint16_t port, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(ip,port);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(uint8_t *ip, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(ip, port);
+    setCallback(callback);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(uint8_t *ip, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(ip,port);
+    setCallback(callback);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+
+PubSubClient::PubSubClient(const char* domain, uint16_t port, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(domain,port);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(const char* domain, uint16_t port, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(domain,port);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(const char* domain, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(domain,port);
+    setCallback(callback);
+    setClient(client);
+    this->stream = NULL;
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+PubSubClient::PubSubClient(const char* domain, uint16_t port, MQTT_CALLBACK_SIGNATURE, Client& client, Stream& stream) {
+    this->_state = MQTT_DISCONNECTED;
+    setServer(domain,port);
+    setCallback(callback);
+    setClient(client);
+    setStream(stream);
+    this->bufferSize = 0;
+    setBufferSize(MQTT_MAX_PACKET_SIZE);
+    setKeepAlive(MQTT_KEEPALIVE);
+    setSocketTimeout(MQTT_SOCKET_TIMEOUT);
+}
+
+PubSubClient::~PubSubClient() {
+  free(this->buffer);
+}
+
+boolean PubSubClient::connect(const char *id) {
+    return connect(id,NULL,NULL,0,0,0,0,1);
+}
+
+boolean PubSubClient::connect(const char *id, const char *user, const char *pass) {
+    return connect(id,user,pass,0,0,0,0,1);
+}
+
+boolean PubSubClient::connect(const char *id, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage) {
+    return connect(id,NULL,NULL,willTopic,willQos,willRetain,willMessage,1);
+}
+
+boolean PubSubClient::connect(const char *id, const char *user, const char *pass, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage) {
+    return connect(id,user,pass,willTopic,willQos,willRetain,willMessage,1);
+}
+
+boolean PubSubClient::connect(const char *id, const char *user, const char *pass, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage, boolean cleanSession) {
+    if (!connected()) {
+        int result = 0;
+
+
+        if(_client->connected()) {
+            result = 1;
+        } else {
+            if (domain != NULL) {
+                result = _client->connect(this->domain, this->port);
+            } else {
+                result = _client->connect(this->ip, this->port);
+            }
+        }
+
+        if (result == 1) {
+            nextMsgId = 1;
+            // Leave room in the buffer for header and variable length field
+            uint16_t length = MQTT_MAX_HEADER_SIZE;
+            unsigned int j;
+
+#if MQTT_VERSION == MQTT_VERSION_3_1
+            uint8_t d[9] = {0x00,0x06,'M','Q','I','s','d','p', MQTT_VERSION};
+#define MQTT_HEADER_VERSION_LENGTH 9
+#elif MQTT_VERSION == MQTT_VERSION_3_1_1
+            uint8_t d[7] = {0x00,0x04,'M','Q','T','T',MQTT_VERSION};
+#define MQTT_HEADER_VERSION_LENGTH 7
+#endif
+            for (j = 0;j<MQTT_HEADER_VERSION_LENGTH;j++) {
+                this->buffer[length++] = d[j];
+            }
+
+            uint8_t v;
+            if (willTopic) {
+                v = 0x04|(willQos<<3)|(willRetain<<5);
+            } else {
+                v = 0x00;
+            }
+            if (cleanSession) {
+                v = v|0x02;
+            }
+
+            if(user != NULL) {
+                v = v|0x80;
+
+                if(pass != NULL) {
+                    v = v|(0x80>>1);
+                }
+            }
+            this->buffer[length++] = v;
+
+            this->buffer[length++] = ((this->keepAlive) >> 8);
+            this->buffer[length++] = ((this->keepAlive) & 0xFF);
+
+            CHECK_STRING_LENGTH(length,id)
+            length = writeString(id,this->buffer,length);
+            if (willTopic) {
+                CHECK_STRING_LENGTH(length,willTopic)
+                length = writeString(willTopic,this->buffer,length);
+                CHECK_STRING_LENGTH(length,willMessage)
+                length = writeString(willMessage,this->buffer,length);
+            }
+
+            if(user != NULL) {
+                CHECK_STRING_LENGTH(length,user)
+                length = writeString(user,this->buffer,length);
+                if(pass != NULL) {
+                    CHECK_STRING_LENGTH(length,pass)
+                    length = writeString(pass,this->buffer,length);
+                }
+            }
+
+            write(MQTTCONNECT,this->buffer,length-MQTT_MAX_HEADER_SIZE);
+
+            lastInActivity = lastOutActivity = millis();
+
+            while (!_client->available()) {
+                unsigned long t = millis();
+                if (t-lastInActivity >= ((int32_t) this->socketTimeout*1000UL)) {
+                    _state = MQTT_CONNECTION_TIMEOUT;
+                    _client->stop();
+                    return false;
+                }
+            }
+            uint8_t llen;
+            uint32_t len = readPacket(&llen);
+
+            if (len == 4) {
+                if (buffer[3] == 0) {
+                    lastInActivity = millis();
+                    pingOutstanding = false;
+                    _state = MQTT_CONNECTED;
+                    return true;
+                } else {
+                    _state = buffer[3];
+                }
+            }
+            _client->stop();
+        } else {
+            _state = MQTT_CONNECT_FAILED;
+        }
+        return false;
+    }
+    return true;
+}
+
+// reads a byte into result
+boolean PubSubClient::readByte(uint8_t * result) {
+   uint32_t previousMillis = millis();
+   while(!_client->available()) {
+     yield();
+     uint32_t currentMillis = millis();
+     if(currentMillis - previousMillis >= ((int32_t) this->socketTimeout * 1000)){
+       return false;
+     }
+   }
+   *result = _client->read();
+   return true;
+}
+
+// reads a byte into result[*index] and increments index
+boolean PubSubClient::readByte(uint8_t * result, uint16_t * index){
+  uint16_t current_index = *index;
+  uint8_t * write_address = &(result[current_index]);
+  if(readByte(write_address)){
+    *index = current_index + 1;
+    return true;
+  }
+  return false;
+}
+
+uint32_t PubSubClient::readPacket(uint8_t* lengthLength) {
+    uint16_t len = 0;
+    if(!readByte(this->buffer, &len)) return 0;
+    bool isPublish = (this->buffer[0]&0xF0) == MQTTPUBLISH;
+    uint32_t multiplier = 1;
+    uint32_t length = 0;
+    uint8_t digit = 0;
+    uint16_t skip = 0;
+    uint32_t start = 0;
+
+    do {
+        if (len == 5) {
+            // Invalid remaining length encoding - kill the connection
+            _state = MQTT_DISCONNECTED;
+            _client->stop();
+            return 0;
+        }
+        if(!readByte(&digit)) return 0;
+        this->buffer[len++] = digit;
+        length += (digit & 127) * multiplier;
+        multiplier <<=7; //multiplier *= 128
+    } while ((digit & 128) != 0);
+    *lengthLength = len-1;
+
+    if (isPublish) {
+        // Read in topic length to calculate bytes to skip over for Stream writing
+        if(!readByte(this->buffer, &len)) return 0;
+        if(!readByte(this->buffer, &len)) return 0;
+        skip = (this->buffer[*lengthLength+1]<<8)+this->buffer[*lengthLength+2];
+        start = 2;
+        if (this->buffer[0]&MQTTQOS1) {
+            // skip message id
+            skip += 2;
+        }
+    }
+    uint32_t idx = len;
+
+    for (uint32_t i = start;i<length;i++) {
+        if(!readByte(&digit)) return 0;
+        if (this->stream) {
+            if (isPublish && idx-*lengthLength-2>skip) {
+                this->stream->write(digit);
+            }
+        }
+
+        if (len < this->bufferSize) {
+            this->buffer[len] = digit;
+            len++;
+        }
+        idx++;
+    }
+
+    if (!this->stream && idx > this->bufferSize) {
+        len = 0; // This will cause the packet to be ignored.
+    }
+    return len;
+}
+
+boolean PubSubClient::loop() {
+    if (connected()) {
+        unsigned long t = millis();
+        if ((t - lastInActivity > this->keepAlive*1000UL) || (t - lastOutActivity > this->keepAlive*1000UL)) {
+            if (pingOutstanding) {
+                this->_state = MQTT_CONNECTION_TIMEOUT;
+                _client->stop();
+                return false;
+            } else {
+                this->buffer[0] = MQTTPINGREQ;
+                this->buffer[1] = 0;
+                _client->write(this->buffer,2);
+                lastOutActivity = t;
+                lastInActivity = t;
+                pingOutstanding = true;
+            }
+        }
+        if (_client->available()) {
+            uint8_t llen;
+            uint16_t len = readPacket(&llen);
+            uint16_t msgId = 0;
+            uint8_t *payload;
+            if (len > 0) {
+                lastInActivity = t;
+                uint8_t type = this->buffer[0]&0xF0;
+                if (type == MQTTPUBLISH) {
+                    if (callback) {
+                        uint16_t tl = (this->buffer[llen+1]<<8)+this->buffer[llen+2]; /* topic length in bytes */
+                        memmove(this->buffer+llen+2,this->buffer+llen+3,tl); /* move topic inside buffer 1 byte to front */
+                        this->buffer[llen+2+tl] = 0; /* end the topic as a 'C' string with \x00 */
+                        char *topic = (char*) this->buffer+llen+2;
+                        // msgId only present for QOS>0
+                        if ((this->buffer[0]&0x06) == MQTTQOS1) {
+                            msgId = (this->buffer[llen+3+tl]<<8)+this->buffer[llen+3+tl+1];
+                            payload = this->buffer+llen+3+tl+2;
+                            callback(topic,payload,len-llen-3-tl-2);
+
+                            this->buffer[0] = MQTTPUBACK;
+                            this->buffer[1] = 2;
+                            this->buffer[2] = (msgId >> 8);
+                            this->buffer[3] = (msgId & 0xFF);
+                            _client->write(this->buffer,4);
+                            lastOutActivity = t;
+
+                        } else {
+                            payload = this->buffer+llen+3+tl;
+                            callback(topic,payload,len-llen-3-tl);
+                        }
+                    }
+                } else if (type == MQTTPINGREQ) {
+                    this->buffer[0] = MQTTPINGRESP;
+                    this->buffer[1] = 0;
+                    _client->write(this->buffer,2);
+                } else if (type == MQTTPINGRESP) {
+                    pingOutstanding = false;
+                }
+            } else if (!connected()) {
+                // readPacket has closed the connection
+                return false;
+            }
+        }
+        return true;
+    }
+    return false;
+}
+
+boolean PubSubClient::publish(const char* topic, const char* payload) {
+    return publish(topic,(const uint8_t*)payload, payload ? strnlen(payload, this->bufferSize) : 0,false);
+}
+
+boolean PubSubClient::publish(const char* topic, const char* payload, boolean retained) {
+    return publish(topic,(const uint8_t*)payload, payload ? strnlen(payload, this->bufferSize) : 0,retained);
+}
+
+boolean PubSubClient::publish(const char* topic, const uint8_t* payload, unsigned int plength) {
+    return publish(topic, payload, plength, false);
+}
+
+boolean PubSubClient::publish(const char* topic, const uint8_t* payload, unsigned int plength, boolean retained) {
+    if (connected()) {
+        if (this->bufferSize < MQTT_MAX_HEADER_SIZE + 2+strnlen(topic, this->bufferSize) + plength) {
+            // Too long
+            return false;
+        }
+        // Leave room in the buffer for header and variable length field
+        uint16_t length = MQTT_MAX_HEADER_SIZE;
+        length = writeString(topic,this->buffer,length);
+
+        // Add payload
+        uint16_t i;
+        for (i=0;i<plength;i++) {
+            this->buffer[length++] = payload[i];
+        }
+
+        // Write the header
+        uint8_t header = MQTTPUBLISH;
+        if (retained) {
+            header |= 1;
+        }
+        return write(header,this->buffer,length-MQTT_MAX_HEADER_SIZE);
+    }
+    return false;
+}
+
+boolean PubSubClient::publish_P(const char* topic, const char* payload, boolean retained) {
+    return publish_P(topic, (const uint8_t*)payload, payload ? strnlen(payload, this->bufferSize) : 0, retained);
+}
+
+boolean PubSubClient::publish_P(const char* topic, const uint8_t* payload, unsigned int plength, boolean retained) {
+    uint8_t llen = 0;
+    uint8_t digit;
+    unsigned int rc = 0;
+    uint16_t tlen;
+    unsigned int pos = 0;
+    unsigned int i;
+    uint8_t header;
+    unsigned int len;
+    int expectedLength;
+
+    if (!connected()) {
+        return false;
+    }
+
+    tlen = strnlen(topic, this->bufferSize);
+
+    header = MQTTPUBLISH;
+    if (retained) {
+        header |= 1;
+    }
+    this->buffer[pos++] = header;
+    len = plength + 2 + tlen;
+    do {
+        digit = len  & 127; //digit = len %128
+        len >>= 7; //len = len / 128
+        if (len > 0) {
+            digit |= 0x80;
+        }
+        this->buffer[pos++] = digit;
+        llen++;
+    } while(len>0);
+
+    pos = writeString(topic,this->buffer,pos);
+
+    rc += _client->write(this->buffer,pos);
+
+    for (i=0;i<plength;i++) {
+        rc += _client->write((char)pgm_read_byte_near(payload + i));
+    }
+
+    lastOutActivity = millis();
+
+    expectedLength = 1 + llen + 2 + tlen + plength;
+
+    return (rc == expectedLength);
+}
+
+boolean PubSubClient::beginPublish(const char* topic, unsigned int plength, boolean retained) {
+    if (connected()) {
+        // Send the header and variable length field
+        uint16_t length = MQTT_MAX_HEADER_SIZE;
+        length = writeString(topic,this->buffer,length);
+        uint8_t header = MQTTPUBLISH;
+        if (retained) {
+            header |= 1;
+        }
+        size_t hlen = buildHeader(header, this->buffer, plength+length-MQTT_MAX_HEADER_SIZE);
+        uint16_t rc = _client->write(this->buffer+(MQTT_MAX_HEADER_SIZE-hlen),length-(MQTT_MAX_HEADER_SIZE-hlen));
+        lastOutActivity = millis();
+        return (rc == (length-(MQTT_MAX_HEADER_SIZE-hlen)));
+    }
+    return false;
+}
+
+int PubSubClient::endPublish() {
+ return 1;
+}
+
+size_t PubSubClient::write(uint8_t data) {
+    lastOutActivity = millis();
+    return _client->write(data);
+}
+
+size_t PubSubClient::write(const uint8_t *buffer, size_t size) {
+    lastOutActivity = millis();
+    return _client->write(buffer,size);
+}
+
+size_t PubSubClient::buildHeader(uint8_t header, uint8_t* buf, uint16_t length) {
+    uint8_t lenBuf[4];
+    uint8_t llen = 0;
+    uint8_t digit;
+    uint8_t pos = 0;
+    uint16_t len = length;
+    do {
+
+        digit = len  & 127; //digit = len %128
+        len >>= 7; //len = len / 128
+        if (len > 0) {
+            digit |= 0x80;
+        }
+        lenBuf[pos++] = digit;
+        llen++;
+    } while(len>0);
+
+    buf[4-llen] = header;
+    for (int i=0;i<llen;i++) {
+        buf[MQTT_MAX_HEADER_SIZE-llen+i] = lenBuf[i];
+    }
+    return llen+1; // Full header size is variable length bit plus the 1-byte fixed header
+}
+
+boolean PubSubClient::write(uint8_t header, uint8_t* buf, uint16_t length) {
+    uint16_t rc;
+    uint8_t hlen = buildHeader(header, buf, length);
+
+#ifdef MQTT_MAX_TRANSFER_SIZE
+    uint8_t* writeBuf = buf+(MQTT_MAX_HEADER_SIZE-hlen);
+    uint16_t bytesRemaining = length+hlen;  //Match the length type
+    uint8_t bytesToWrite;
+    boolean result = true;
+    while((bytesRemaining > 0) && result) {
+        bytesToWrite = (bytesRemaining > MQTT_MAX_TRANSFER_SIZE)?MQTT_MAX_TRANSFER_SIZE:bytesRemaining;
+        rc = _client->write(writeBuf,bytesToWrite);
+        result = (rc == bytesToWrite);
+        bytesRemaining -= rc;
+        writeBuf += rc;
+    }
+    return result;
+#else
+    rc = _client->write(buf+(MQTT_MAX_HEADER_SIZE-hlen),length+hlen);
+    lastOutActivity = millis();
+    return (rc == hlen+length);
+#endif
+}
+
+boolean PubSubClient::subscribe(const char* topic) {
+    return subscribe(topic, 0);
+}
+
+boolean PubSubClient::subscribe(const char* topic, uint8_t qos) {
+    size_t topicLength = strnlen(topic, this->bufferSize);
+    if (topic == 0) {
+        return false;
+    }
+    if (qos > 1) {
+        return false;
+    }
+    if (this->bufferSize < 9 + topicLength) {
+        // Too long
+        return false;
+    }
+    if (connected()) {
+        // Leave room in the buffer for header and variable length field
+        uint16_t length = MQTT_MAX_HEADER_SIZE;
+        nextMsgId++;
+        if (nextMsgId == 0) {
+            nextMsgId = 1;
+        }
+        this->buffer[length++] = (nextMsgId >> 8);
+        this->buffer[length++] = (nextMsgId & 0xFF);
+        length = writeString((char*)topic, this->buffer,length);
+        this->buffer[length++] = qos;
+        return write(MQTTSUBSCRIBE|MQTTQOS1,this->buffer,length-MQTT_MAX_HEADER_SIZE);
+    }
+    return false;
+}
+
+boolean PubSubClient::unsubscribe(const char* topic) {
+	size_t topicLength = strnlen(topic, this->bufferSize);
+    if (topic == 0) {
+        return false;
+    }
+    if (this->bufferSize < 9 + topicLength) {
+        // Too long
+        return false;
+    }
+    if (connected()) {
+        uint16_t length = MQTT_MAX_HEADER_SIZE;
+        nextMsgId++;
+        if (nextMsgId == 0) {
+            nextMsgId = 1;
+        }
+        this->buffer[length++] = (nextMsgId >> 8);
+        this->buffer[length++] = (nextMsgId & 0xFF);
+        length = writeString(topic, this->buffer,length);
+        return write(MQTTUNSUBSCRIBE|MQTTQOS1,this->buffer,length-MQTT_MAX_HEADER_SIZE);
+    }
+    return false;
+}
+
+void PubSubClient::disconnect() {
+    this->buffer[0] = MQTTDISCONNECT;
+    this->buffer[1] = 0;
+    _client->write(this->buffer,2);
+    _state = MQTT_DISCONNECTED;
+    _client->flush();
+    _client->stop();
+    lastInActivity = lastOutActivity = millis();
+}
+
+uint16_t PubSubClient::writeString(const char* string, uint8_t* buf, uint16_t pos) {
+    const char* idp = string;
+    uint16_t i = 0;
+    pos += 2;
+    while (*idp) {
+        buf[pos++] = *idp++;
+        i++;
+    }
+    buf[pos-i-2] = (i >> 8);
+    buf[pos-i-1] = (i & 0xFF);
+    return pos;
+}
+
+
+boolean PubSubClient::connected() {
+    boolean rc;
+    if (_client == NULL ) {
+        rc = false;
+    } else {
+        rc = (int)_client->connected();
+        if (!rc) {
+            if (this->_state == MQTT_CONNECTED) {
+                this->_state = MQTT_CONNECTION_LOST;
+                _client->flush();
+                _client->stop();
+            }
+        } else {
+            return this->_state == MQTT_CONNECTED;
+        }
+    }
+    return rc;
+}
+
+PubSubClient& PubSubClient::setServer(uint8_t * ip, uint16_t port) {
+    IPAddress addr(ip[0],ip[1],ip[2],ip[3]);
+    return setServer(addr,port);
+}
+
+PubSubClient& PubSubClient::setServer(IPAddress ip, uint16_t port) {
+    this->ip = ip;
+    this->port = port;
+    this->domain = NULL;
+    return *this;
+}
+
+PubSubClient& PubSubClient::setServer(const char * domain, uint16_t port) {
+    this->domain = domain;
+    this->port = port;
+    return *this;
+}
+
+PubSubClient& PubSubClient::setCallback(MQTT_CALLBACK_SIGNATURE) {
+    this->callback = callback;
+    return *this;
+}
+
+PubSubClient& PubSubClient::setClient(Client& client){
+    this->_client = &client;
+    return *this;
+}
+
+PubSubClient& PubSubClient::setStream(Stream& stream){
+    this->stream = &stream;
+    return *this;
+}
+
+int PubSubClient::state() {
+    return this->_state;
+}
+
+boolean PubSubClient::setBufferSize(uint16_t size) {
+    if (size == 0) {
+        // Cannot set it back to 0
+        return false;
+    }
+    if (this->bufferSize == 0) {
+        this->buffer = (uint8_t*)malloc(size);
+    } else {
+        uint8_t* newBuffer = (uint8_t*)realloc(this->buffer, size);
+        if (newBuffer != NULL) {
+            this->buffer = newBuffer;
+        } else {
+            return false;
+        }
+    }
+    this->bufferSize = size;
+    return (this->buffer != NULL);
+}
+
+uint16_t PubSubClient::getBufferSize() {
+    return this->bufferSize;
+}
+PubSubClient& PubSubClient::setKeepAlive(uint16_t keepAlive) {
+    this->keepAlive = keepAlive;
+    return *this;
+}
+PubSubClient& PubSubClient::setSocketTimeout(uint16_t timeout) {
+    this->socketTimeout = timeout;
+    return *this;
+}

Software/src/lib/knolleary-pubsubclient/PubSubClient.h

@@ -0,0 +1,184 @@
+/*
+ PubSubClient.h - A simple client for MQTT.
+  Nick O'Leary
+  http://knolleary.net
+*/
+
+#ifndef PubSubClient_h
+#define PubSubClient_h
+
+#include <Arduino.h>
+#include "IPAddress.h"
+#include "Client.h"
+#include "Stream.h"
+
+#define MQTT_VERSION_3_1      3
+#define MQTT_VERSION_3_1_1    4
+
+// MQTT_VERSION : Pick the version
+//#define MQTT_VERSION MQTT_VERSION_3_1
+#ifndef MQTT_VERSION
+#define MQTT_VERSION MQTT_VERSION_3_1_1
+#endif
+
+// MQTT_MAX_PACKET_SIZE : Maximum packet size. Override with setBufferSize().
+#ifndef MQTT_MAX_PACKET_SIZE
+#define MQTT_MAX_PACKET_SIZE 256
+#endif
+
+// MQTT_KEEPALIVE : keepAlive interval in Seconds. Override with setKeepAlive()
+#ifndef MQTT_KEEPALIVE
+#define MQTT_KEEPALIVE 15
+#endif
+
+// MQTT_SOCKET_TIMEOUT: socket timeout interval in Seconds. Override with setSocketTimeout()
+#ifndef MQTT_SOCKET_TIMEOUT
+#define MQTT_SOCKET_TIMEOUT 15
+#endif
+
+// MQTT_MAX_TRANSFER_SIZE : limit how much data is passed to the network client
+//  in each write call. Needed for the Arduino Wifi Shield. Leave undefined to
+//  pass the entire MQTT packet in each write call.
+//#define MQTT_MAX_TRANSFER_SIZE 80
+
+// Possible values for client.state()
+#define MQTT_CONNECTION_TIMEOUT     -4
+#define MQTT_CONNECTION_LOST        -3
+#define MQTT_CONNECT_FAILED         -2
+#define MQTT_DISCONNECTED           -1
+#define MQTT_CONNECTED               0
+#define MQTT_CONNECT_BAD_PROTOCOL    1
+#define MQTT_CONNECT_BAD_CLIENT_ID   2
+#define MQTT_CONNECT_UNAVAILABLE     3
+#define MQTT_CONNECT_BAD_CREDENTIALS 4
+#define MQTT_CONNECT_UNAUTHORIZED    5
+
+#define MQTTCONNECT     1 << 4  // Client request to connect to Server
+#define MQTTCONNACK     2 << 4  // Connect Acknowledgment
+#define MQTTPUBLISH     3 << 4  // Publish message
+#define MQTTPUBACK      4 << 4  // Publish Acknowledgment
+#define MQTTPUBREC      5 << 4  // Publish Received (assured delivery part 1)
+#define MQTTPUBREL      6 << 4  // Publish Release (assured delivery part 2)
+#define MQTTPUBCOMP     7 << 4  // Publish Complete (assured delivery part 3)
+#define MQTTSUBSCRIBE   8 << 4  // Client Subscribe request
+#define MQTTSUBACK      9 << 4  // Subscribe Acknowledgment
+#define MQTTUNSUBSCRIBE 10 << 4 // Client Unsubscribe request
+#define MQTTUNSUBACK    11 << 4 // Unsubscribe Acknowledgment
+#define MQTTPINGREQ     12 << 4 // PING Request
+#define MQTTPINGRESP    13 << 4 // PING Response
+#define MQTTDISCONNECT  14 << 4 // Client is Disconnecting
+#define MQTTReserved    15 << 4 // Reserved
+
+#define MQTTQOS0        (0 << 1)
+#define MQTTQOS1        (1 << 1)
+#define MQTTQOS2        (2 << 1)
+
+// Maximum size of fixed header and variable length size header
+#define MQTT_MAX_HEADER_SIZE 5
+
+#if defined(ESP8266) || defined(ESP32)
+#include <functional>
+#define MQTT_CALLBACK_SIGNATURE std::function<void(char*, uint8_t*, unsigned int)> callback
+#else
+#define MQTT_CALLBACK_SIGNATURE void (*callback)(char*, uint8_t*, unsigned int)
+#endif
+
+#define CHECK_STRING_LENGTH(l,s) if (l+2+strnlen(s, this->bufferSize) > this->bufferSize) {_client->stop();return false;}
+
+class PubSubClient : public Print {
+private:
+   Client* _client;
+   uint8_t* buffer;
+   uint16_t bufferSize;
+   uint16_t keepAlive;
+   uint16_t socketTimeout;
+   uint16_t nextMsgId;
+   unsigned long lastOutActivity;
+   unsigned long lastInActivity;
+   bool pingOutstanding;
+   MQTT_CALLBACK_SIGNATURE;
+   uint32_t readPacket(uint8_t*);
+   boolean readByte(uint8_t * result);
+   boolean readByte(uint8_t * result, uint16_t * index);
+   boolean write(uint8_t header, uint8_t* buf, uint16_t length);
+   uint16_t writeString(const char* string, uint8_t* buf, uint16_t pos);
+   // Build up the header ready to send
+   // Returns the size of the header
+   // Note: the header is built at the end of the first MQTT_MAX_HEADER_SIZE bytes, so will start
+   //       (MQTT_MAX_HEADER_SIZE - <returned size>) bytes into the buffer
+   size_t buildHeader(uint8_t header, uint8_t* buf, uint16_t length);
+   IPAddress ip;
+   const char* domain;
+   uint16_t port;
+   Stream* stream;
+   int _state;
+public:
+   PubSubClient();
+   PubSubClient(Client& client);
+   PubSubClient(IPAddress, uint16_t, Client& client);
+   PubSubClient(IPAddress, uint16_t, Client& client, Stream&);
+   PubSubClient(IPAddress, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client);
+   PubSubClient(IPAddress, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client, Stream&);
+   PubSubClient(uint8_t *, uint16_t, Client& client);
+   PubSubClient(uint8_t *, uint16_t, Client& client, Stream&);
+   PubSubClient(uint8_t *, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client);
+   PubSubClient(uint8_t *, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client, Stream&);
+   PubSubClient(const char*, uint16_t, Client& client);
+   PubSubClient(const char*, uint16_t, Client& client, Stream&);
+   PubSubClient(const char*, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client);
+   PubSubClient(const char*, uint16_t, MQTT_CALLBACK_SIGNATURE,Client& client, Stream&);
+
+   ~PubSubClient();
+
+   PubSubClient& setServer(IPAddress ip, uint16_t port);
+   PubSubClient& setServer(uint8_t * ip, uint16_t port);
+   PubSubClient& setServer(const char * domain, uint16_t port);
+   PubSubClient& setCallback(MQTT_CALLBACK_SIGNATURE);
+   PubSubClient& setClient(Client& client);
+   PubSubClient& setStream(Stream& stream);
+   PubSubClient& setKeepAlive(uint16_t keepAlive);
+   PubSubClient& setSocketTimeout(uint16_t timeout);
+
+   boolean setBufferSize(uint16_t size);
+   uint16_t getBufferSize();
+
+   boolean connect(const char* id);
+   boolean connect(const char* id, const char* user, const char* pass);
+   boolean connect(const char* id, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage);
+   boolean connect(const char* id, const char* user, const char* pass, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage);
+   boolean connect(const char* id, const char* user, const char* pass, const char* willTopic, uint8_t willQos, boolean willRetain, const char* willMessage, boolean cleanSession);
+   void disconnect();
+   boolean publish(const char* topic, const char* payload);
+   boolean publish(const char* topic, const char* payload, boolean retained);
+   boolean publish(const char* topic, const uint8_t * payload, unsigned int plength);
+   boolean publish(const char* topic, const uint8_t * payload, unsigned int plength, boolean retained);
+   boolean publish_P(const char* topic, const char* payload, boolean retained);
+   boolean publish_P(const char* topic, const uint8_t * payload, unsigned int plength, boolean retained);
+   // Start to publish a message.
+   // This API:
+   //   beginPublish(...)
+   //   one or more calls to write(...)
+   //   endPublish()
+   // Allows for arbitrarily large payloads to be sent without them having to be copied into
+   // a new buffer and held in memory at one time
+   // Returns 1 if the message was started successfully, 0 if there was an error
+   boolean beginPublish(const char* topic, unsigned int plength, boolean retained);
+   // Finish off this publish message (started with beginPublish)
+   // Returns 1 if the packet was sent successfully, 0 if there was an error
+   int endPublish();
+   // Write a single byte of payload (only to be used with beginPublish/endPublish)
+   virtual size_t write(uint8_t);
+   // Write size bytes from buffer into the payload (only to be used with beginPublish/endPublish)
+   // Returns the number of bytes written
+   virtual size_t write(const uint8_t *buffer, size_t size);
+   boolean subscribe(const char* topic);
+   boolean subscribe(const char* topic, uint8_t qos);
+   boolean unsubscribe(const char* topic);
+   boolean loop();
+   boolean connected();
+   int state();
+
+};
+
+
+#endif