diff --git a/firmware/mqtt.cpp b/firmware/mqtt.cpp
index 9622645..5cbeaac 100644
--- a/firmware/mqtt.cpp
+++ b/firmware/mqtt.cpp
@@ -3,3 +3,158 @@
 //
 
 #include "mqtt.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#include <espressif/esp_common.h>
+#include <espressif/user_interface.h>
+
+extern "C" {
+
+#include <paho_mqtt_c/MQTTESP8266.h>
+#include <paho_mqtt_c/MQTTClient.h>
+
+}
+
+#include <semphr.h>
+
+
+/* You can use http://test.mosquitto.org/ to test mqtt_client instead
+ * of setting up your own MQTT server */
+#define MQTT_HOST ("172.16.1.53")
+#define MQTT_PORT 1883
+
+#define MQTT_USER NULL
+#define MQTT_PASS NULL
+
+QueueHandle_t publish_queue;
+#define PUB_MSG_LEN 16
+
+extern "C" void beat_task(void *pvParameters) {
+    TickType_t xLastWakeTime = xTaskGetTickCount();
+    char msg[PUB_MSG_LEN];
+    int count = 0;
+
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, 10000 / portTICK_PERIOD_MS);
+        printf("beat\r\n");
+        snprintf(msg, PUB_MSG_LEN, "Beat %d\r\n", count++);
+        if(xQueueSend(publish_queue, (void *) msg, 0) == pdFALSE) {
+            printf("Publish queue overflow.\r\n");
+        }
+    }
+}
+
+static void topic_received(mqtt_message_data_t *md) {
+    int i;
+    mqtt_message_t *message = md->message;
+    printf("Received: ");
+    for (i = 0; i < md->topic->lenstring.len; ++i)
+        printf("%c", md->topic->lenstring.data[i]);
+
+    printf(" = ");
+    for (i = 0; i < (int) message->payloadlen; ++i)
+        printf("%c", ((char *) (message->payload))[i]);
+
+    printf("\r\n");
+}
+
+static const char *get_my_id(void) {
+    // Use MAC address for Station as unique ID
+    static char my_id[13];
+    static bool my_id_done = false;
+    int8_t i;
+    uint8_t x;
+    if(my_id_done)
+        return my_id;
+    if(!sdk_wifi_get_macaddr(STATION_IF, (uint8_t *) my_id))
+        return NULL;
+    for (i = 5; i >= 0; --i) {
+        x = my_id[i] & 0x0F;
+        if(x > 9) x += 7;
+        my_id[i * 2 + 1] = x + '0';
+        x = my_id[i] >> 4;
+        if(x > 9) x += 7;
+        my_id[i * 2] = x + '0';
+    }
+    my_id[12] = '\0';
+    my_id_done = true;
+    return my_id;
+}
+
+extern "C" void mqtt_task(void *pvParameters) {
+    int ret = 0;
+    struct mqtt_network network;
+    mqtt_client_t client = mqtt_client_default;
+    char mqtt_client_id[20];
+    uint8_t mqtt_buf[100];
+    uint8_t mqtt_readbuf[100];
+    mqtt_packet_connect_data_t data = mqtt_packet_connect_data_initializer;
+
+    mqtt_network_new(&network);
+    memset(mqtt_client_id, 0, sizeof(mqtt_client_id));
+    strcpy(mqtt_client_id, "ESP-");
+    strcat(mqtt_client_id, get_my_id());
+
+    while (1) {
+        printf("%s: started\n\r", __func__);
+        printf("%s: (Re)connecting to MQTT server %s ... ", __func__,
+               MQTT_HOST);
+        ret = mqtt_network_connect(&network, MQTT_HOST, MQTT_PORT);
+        if(ret) {
+            printf("error: %d\n\r", ret);
+            taskYIELD();
+            continue;
+        }
+        printf("done\n\r");
+        mqtt_client_new(&client, &network, 5000, mqtt_buf, 100,
+                        mqtt_readbuf, 100);
+
+        data.willFlag = 0;
+        data.MQTTVersion = 3;
+        data.clientID.cstring = mqtt_client_id;
+        data.username.cstring = MQTT_USER;
+        data.password.cstring = MQTT_PASS;
+        data.keepAliveInterval = 10;
+        data.cleansession = 0;
+        printf("Send MQTT connect ... ");
+        ret = mqtt_connect(&client, &data);
+        if(ret) {
+            printf("error: %d\n\r", ret);
+            mqtt_network_disconnect(&network);
+            taskYIELD();
+            continue;
+        }
+        printf("done\r\n");
+        mqtt_subscribe(&client, "/esptopic", MQTT_QOS1, topic_received);
+        xQueueReset(publish_queue);
+
+        while (1) {
+
+            char msg[PUB_MSG_LEN - 1] = "\0";
+            while (xQueueReceive(publish_queue, (void *) msg, 0) ==
+                   pdTRUE) {
+                printf("got message to publish\r\n");
+                mqtt_message_t message;
+                message.payload = msg;
+                message.payloadlen = PUB_MSG_LEN;
+                message.dup = 0;
+                message.qos = MQTT_QOS1;
+                message.retained = 0;
+                ret = mqtt_publish(&client, "/beat", &message);
+                if(ret != MQTT_SUCCESS) {
+                    printf("error while publishing message: %d\n", ret);
+                    break;
+                }
+            }
+
+            ret = mqtt_yield(&client, 1000);
+            if(ret == MQTT_DISCONNECTED)
+                break;
+        }
+        printf("Connection dropped, request restart\n\r");
+        mqtt_network_disconnect(&network);
+        taskYIELD();
+    }
+}
\ No newline at end of file
diff --git a/firmware/mqtt.h b/firmware/mqtt.h
index a5e03b6..b8d8a8e 100644
--- a/firmware/mqtt.h
+++ b/firmware/mqtt.h
@@ -9,6 +9,9 @@
 extern "C" {
 #endif
 
+void mqtt_task(void *pvParameters);
+void beat_task(void *pvParameters);
+
 #ifdef __cplusplus
 }
 #endif