1 files changed, 85 insertions, 0 deletions

diff --git a/src/mqtt.c b/src/mqtt.c
new file mode 100644
index 0000000..716feec
--- /dev/null
+++ b/src/mqtt.c
@@ -0,0 +1,85 @@
+#include "mqtt.h"
+#include "config.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifndef BUFFER_SIZE
+#define BUFFER_SIZE 64
+#endif
+
+#ifndef PUBLISH_INTERVAL_MS
+#define PUBLISH_INTERVAL_MS 1000
+#endif
+
+#ifndef PUBLISHER_TASK_STACK_SIZE
+#define PUBLISHER_TASK_STACK_SIZE 4096
+#endif
+
+#ifndef PUBLISHER_TASK_PRIORITY
+#define PUBLISHER_TASK_PRIORITY 5
+#endif
+
+static esp_mqtt_client_handle_t client;
+
+static void mqtt_event_handler(void *handler_args, esp_event_base_t base,
+                               int32_t event_id, void *event_data) {
+  esp_mqtt_event_handle_t event = event_data;
+
+  switch (event->event_id) {
+  case MQTT_EVENT_CONNECTED:
+    printf("MQTT connected!\n");
+    esp_mqtt_client_subscribe(event->client, SUB_TOPIC, 1);
+    break;
+
+  case MQTT_EVENT_DATA: {
+    char topic[event->topic_len + 1];
+    char data[event->data_len + 1];
+    memcpy(topic, event->topic, event->topic_len);
+    topic[event->topic_len] = '\0';
+    memcpy(data, event->data, event->data_len);
+    data[event->data_len] = '\0';
+
+    printf("MQTT Msg received\nTopic: %s\nData: %s\n", topic, data);
+
+    char *endptr;
+    long long sent = strtoll(data, &endptr, 10);
+    if (endptr != data) {
+      long long now = esp_timer_get_time() / 1000ULL;
+      long long rtt = now - sent;
+      printf("RTT: %lld ms\n", rtt);
+    }
+    break;
+  }
+
+  default:
+    break;
+  }
+}
+
+static void publisher_task(void *pvParameters) {
+  while (1) {
+    char buf[BUFFER_SIZE];
+    snprintf(buf, BUFFER_SIZE, "%lld",
+             (long long)(esp_timer_get_time() / 1000ULL));
+    esp_mqtt_client_publish(client, PUB_TOPIC, buf, 0, 1, 0);
+    vTaskDelay(pdMS_TO_TICKS(PUBLISH_INTERVAL_MS));
+  }
+}
+
+void mqtt_app_start(void) {
+  esp_mqtt_client_config_t mqtt_cfg = {
+      .broker.address.uri = MQTT_URI,
+  };
+  client = esp_mqtt_client_init(&mqtt_cfg);
+  esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler,
+                                 NULL);
+  esp_mqtt_client_start(client);
+
+  xTaskCreate(publisher_task, "publisher_task", PUBLISHER_TASK_STACK_SIZE, NULL,
+              PUBLISHER_TASK_PRIORITY, NULL);
+}