Merge pull request #5 from dkwstas/ble_scanner_init

Initial scanner code

src/officesense_scanner/.gitignore

@@ -0,0 +1,5 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch

src/officesense_scanner/include/README

@@ -0,0 +1,37 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the convention is to give header files names that end with `.h'.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

src/officesense_scanner/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into the executable file.
+
+The source code of each library should be placed in a separate directory
+("lib/your_library_name/[Code]").
+
+For example, see the structure of the following example libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+Example contents of `src/main.c` using Foo and Bar:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+The PlatformIO Library Dependency Finder will find automatically dependent
+libraries by scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

src/officesense_scanner/platformio.ini

@@ -0,0 +1,24 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:esp32-s3-devkitc-1]
+platform = espressif32
+board = esp32-s3-devkitc-1
+framework = arduino
+monitor_speed = 115200
+board_upload.flash_size = 4MB
+board_build.partitions = default.csv
+build_flags = 
+  -DARDUINO_USB_CDC_ON_BOOT=1
+  -DBOARD_HAS_PSRAM
+
+lib_deps =
+    h2zero/NimBLE-Arduino
+    knolleary/PubSubClient

src/officesense_scanner/src/main.cpp

@@ -0,0 +1,273 @@
+#include <unordered_map>
+#include "main.hpp"
+#include "tag/Tag.hpp"
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+#include <HTTPClient.h>
+#include <PubSubClient.h>
+
+CB cb;
+char MQTT_PUB_TOPIC[sizeof(MQTT_TOPIC) + sizeof(DEV_NAME) - 1];
+std::unordered_map<std::string, Tag> tags;
+std::unordered_map<std::string, UUIDCacheEntry> uuid_cache;
+SemaphoreHandle_t uuid_mtx;
+QueueHandle_t ble_queue;
+QueueHandle_t mqtt_queue;
+QueueHandle_t http_queue;
+WiFiClient wifiClient;
+PubSubClient mqtt(wifiClient);
+
+void cleanupCache()
+{
+  uint32_t now = millis();
+
+  xSemaphoreTake(uuid_mtx, portMAX_DELAY);
+
+  for (auto it = uuid_cache.begin(); it != uuid_cache.end();)
+  {
+    uint32_t age = now - it->second.last_check;
+
+    if (age > UUID_CACHE_CLEAN_TIMEOUT)
+    {
+      Serial.printf("Erasing %s from cache after being inactive for %dms.\n",
+                    it->first.c_str(),
+                    age);
+      it = uuid_cache.erase(it);
+    }
+    else
+      ++it;
+  }
+
+  xSemaphoreGive(uuid_mtx);
+}
+
+void cleanupTags()
+{
+  uint32_t now = millis();
+
+  for (auto it = tags.begin(); it != tags.end();)
+  {
+    if ((now - it->second.getLastSeen()) >= TAG_TIMEOUT)
+    {
+      Serial.printf("Erasing %s from memory after being inactive for %dms.\n",
+                    it->first.c_str(),
+                    now - it->second.getLastSeen());
+      it = tags.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+void httpTask(void *)
+{
+  HttpEvent ev;
+
+  while (true)
+  {
+    if (xQueueReceive(http_queue, &ev, portMAX_DELAY))
+    {
+      HTTPClient http;
+
+      String url = UUID_CHECK_ENDPOINT + String(ev.uuid);
+
+      http.setTimeout(3000);
+      http.setReuse(false);
+
+      if (!http.begin(url))
+      {
+        Serial.println("Failed HTTP begin.");
+        continue;
+      }
+
+      int code = http.GET();
+
+      bool valid = (code == 200);
+
+      http.end();
+
+      xSemaphoreTake(uuid_mtx, portMAX_DELAY);
+      uuid_cache[ev.uuid] = {
+          (valid) ? UUIDState::VALID : UUIDState::INVALID,
+          millis()};
+
+#ifdef DEBUG
+      Serial.printf("[%s] is now marked as %s\n", ev.uuid, (valid) ? "valid" : "invalid");
+#endif
+
+      xSemaphoreGive(uuid_mtx);
+    }
+
+    vTaskDelay(pdMS_TO_TICKS(10));
+  }
+}
+
+void mqttTask(void *)
+{
+  NetEvent ev;
+
+  while (true)
+  {
+    while (!mqtt.connected())
+    {
+      Serial.printf("MQTT Disconnected. Connecting to %s:%d\n", MQTT_HOST, MQTT_PORT);
+      if (mqtt.connect(DEV_NAME, MQTT_USER, MQTT_PASS))
+        Serial.println("MQTT connected.");
+      vTaskDelay(pdMS_TO_TICKS(1000));
+    }
+
+    if (xQueueReceive(mqtt_queue, &ev, pdMS_TO_TICKS(100)))
+    {
+      char payload[128];
+
+      snprintf(payload, sizeof(payload),
+               "{\"uuid\":\"%s\",\"rssi\":%.2f}",
+               ev.uuid,
+               ev.ema);
+
+      bool ok = mqtt.publish(MQTT_PUB_TOPIC, payload);
+
+      if (!ok)
+        Serial.println("Failed to publish on MQTT.");
+    }
+    else
+      mqtt.loop();
+  }
+}
+
+void processEvent(const BleEvent &event)
+{
+  xSemaphoreTake(uuid_mtx, portMAX_DELAY);
+
+  auto it = uuid_cache.find(event.uuid);
+
+  uint32_t now = millis();
+
+  if (it == uuid_cache.end())
+  {
+    uuid_cache[event.uuid] = {UUIDState::PENDING, now};
+
+    xSemaphoreGive(uuid_mtx);
+
+    HttpEvent he;
+
+    strlcpy(he.uuid, event.uuid, sizeof(event.uuid));
+    xQueueSend(http_queue, &he, 0);
+
+    return;
+  }
+
+  if (it->second.state == UUIDState::INVALID &&
+      now - it->second.last_check > UUID_CACHE_RETRY_TIMEOUT)
+  {
+    it->second.state = UUIDState::PENDING;
+    it->second.last_check = now;
+
+    xSemaphoreGive(uuid_mtx);
+
+    HttpEvent he;
+
+    strlcpy(he.uuid, event.uuid, sizeof(event.uuid));
+    xQueueSend(http_queue, &he, 0);
+
+    return;
+  }
+
+  UUIDState state = it->second.state;
+
+  xSemaphoreGive(uuid_mtx);
+
+  if (state != UUIDState::VALID)
+    return;
+
+  Tag &tag = tags[event.uuid];
+
+  tag.addSample(event.rssi);
+
+  if (tag.shouldPublish())
+  {
+    NetEvent ne;
+
+    strlcpy(ne.uuid, event.uuid, sizeof(event.uuid));
+    ne.ema = tag.getEMA();
+
+    if (!xQueueSend(mqtt_queue, &ne, 0))
+      Serial.println("Failed to send MQTT data over MQTT Queue.");
+  }
+}
+
+void CB::onResult(const NimBLEAdvertisedDevice *d)
+{
+  if (!d->haveServiceUUID() || !d->haveName())
+    return;
+
+  NimBLEUUID uuid = d->getServiceUUID(0);
+  const std::string &name = d->getName();
+
+  if ((uuid.bitSize() != 128) || (name != TAG_NAME))
+    return;
+
+  BleEvent event;
+
+  strlcpy(event.uuid, uuid.toString().c_str(), sizeof(event.uuid));
+  event.rssi = d->getRSSI();
+
+  xQueueSend(ble_queue, &event, pdMS_TO_TICKS(5));
+}
+
+void setup()
+{
+  Serial.begin(115200);
+
+  WiFi.begin(WIFI_SSID, WIFI_PASS);
+
+  while (WiFi.status() != WL_CONNECTED)
+  {
+    delay(500);
+    Serial.print(".");
+  }
+
+  Serial.println("\nWiFi connected");
+
+  snprintf(MQTT_PUB_TOPIC, sizeof(MQTT_PUB_TOPIC), "%s%s", MQTT_TOPIC, DEV_NAME);
+
+  mqtt.setServer(MQTT_HOST, MQTT_PORT);
+  mqtt.setKeepAlive(60);
+  mqtt.setSocketTimeout(5);
+
+  ble_queue = xQueueCreate(100, sizeof(BleEvent));
+  mqtt_queue = xQueueCreate(100, sizeof(NetEvent));
+  http_queue = xQueueCreate(100, sizeof(HttpEvent));
+
+  uuid_mtx = xSemaphoreCreateMutex();
+
+  xTaskCreatePinnedToCore(mqttTask, "mqtt", 4096, NULL, 1, NULL, 1);
+  xTaskCreatePinnedToCore(httpTask, "http", 4096, NULL, 1, NULL, 1);
+
+  NimBLEDevice::init(DEV_NAME);
+
+  NimBLEScan *scan = NimBLEDevice::getScan();
+
+  scan->setScanCallbacks(&cb);
+  scan->setActiveScan(false);
+  scan->setDuplicateFilter(false);
+
+  scan->start(0, false, true);
+}
+
+void loop()
+{
+  BleEvent ev;
+
+  while (xQueueReceive(ble_queue, &ev, 0))
+    processEvent(ev);
+
+  static uint32_t last_cleanup = 0;
+
+  if (millis() - last_cleanup >= CLEANUP_INTERVAL)
+  {
+    cleanupTags();
+    cleanupCache();
+    last_cleanup = millis();
+  }
+}

src/officesense_scanner/src/main.hpp

@@ -0,0 +1,59 @@
+#ifndef MAIN_HPP
+#define MAIN_HPP
+
+#include <Arduino.h>
+#include <NimBLEDevice.h>
+#include <string>
+
+#define CLEANUP_INTERVAL 5 * 1000 // ms
+#define TAG_TIMEOUT 60 * 1000 // ms
+#define UUID_CACHE_RETRY_TIMEOUT 5 * 60 * 1000 // ms
+#define UUID_CACHE_CLEAN_TIMEOUT 60 * 60 * 1000 // ms
+#define TAG_NAME "X6TAG"
+#define UUID_CHECK_ENDPOINT "http://192.168.1.2/check/"
+#define MQTT_HOST "192.168.1.2"
+#define MQTT_PORT 1883
+#define MQTT_USER "user"
+#define MQTT_PASS "pass"
+#define MQTT_TOPIC "scanners/"
+#define WIFI_SSID "COSMOTE-489882"
+#define WIFI_PASS "x32hbh54673ngccdsfa9"
+#define DEV_NAME "Scanner_Room_A"
+
+struct BleEvent
+{
+    char uuid[37];
+    int8_t rssi;
+};
+
+struct NetEvent
+{
+    char uuid[37];
+    float ema;
+};
+
+struct HttpEvent
+{
+    char uuid[37];
+};
+
+enum class UUIDState
+{
+    PENDING,
+    VALID,
+    INVALID
+};
+
+struct UUIDCacheEntry
+{
+    UUIDState state;
+    uint32_t last_check;
+};
+
+class CB : public NimBLEScanCallbacks
+{
+public:
+    void onResult(const NimBLEAdvertisedDevice *d) override;
+};
+
+#endif

src/officesense_scanner/src/tag/Tag.cpp

@@ -0,0 +1,177 @@
+#include <iostream>
+#include "Tag.hpp"
+
+uint32_t Tag::getLastSeen() const { return this->last_seen; }
+
+void Tag::addSample(int8_t rssi)
+{
+#ifdef DEBUG
+    Serial.printf("[addSample()]: rssi_window[%d] = %d\n", this->win_idx, rssi);
+    Serial.flush();
+#endif
+
+    this->rssi_window[this->win_idx] = rssi;
+    this->win_idx = (this->win_idx + 1) % 5;
+
+#ifdef DEBUG
+    Serial.printf("[addSample()]: count = %d", this->count);
+    Serial.flush();
+#endif
+
+    if (this->count < 5)
+    {
+        this->count++;
+
+#ifdef DEBUG
+        Serial.printf(" -> %d", this->count);
+        Serial.flush();
+#endif
+    }
+
+#ifdef DEBUG
+    Serial.println();
+    Serial.flush();
+#endif
+
+    int8_t median = getMedian();
+
+#ifdef DEBUG
+    Serial.printf("[addSample()]: median = %d\n", median);
+    Serial.flush();
+#endif
+
+    // EMA update
+    if (!this->init)
+    {
+#ifdef DEBUG
+        Serial.printf("[addSample()]: init = false\n");
+        Serial.flush();
+#endif
+
+        this->ema = median;
+
+#ifdef DEBUG
+        Serial.printf("[addSample()]: ema = %f\n", this->ema);
+        Serial.flush();
+#endif
+
+        this->init = true;
+    }
+    else
+    {
+        this->ema = 0.8f * this->ema + 0.2f * median;
+
+#ifdef DEBUG
+        Serial.printf("[addSample()]: ema = %f\n", this->ema);
+        Serial.flush();
+#endif
+    }
+
+    last_seen = millis();
+}
+
+int8_t Tag::getMedian() const
+{
+    if (this->count == 0)
+    {
+#ifdef DEBUG
+        Serial.printf("[getMedian()]: count = 0\n");
+        Serial.flush();
+#endif
+
+        return 0;
+    }
+
+    int8_t sorted[5];
+
+#ifdef DEBUG
+    Serial.printf("[getMedian()]: rssi_window[] = {");
+    Serial.flush();
+#endif
+
+    for (uint8_t i = 0; i < this->count; i++)
+    {
+#ifdef DEBUG
+        Serial.printf(" %d", rssi_window[i]);
+        Serial.flush();
+#endif
+
+        sorted[i] = this->rssi_window[i];
+    }
+
+#ifdef DEBUG
+    Serial.printf(" }\n");
+    Serial.flush();
+#endif
+
+    std::sort(sorted, sorted + this->count);
+
+    return sorted[this->count / 2];
+}
+
+float Tag::getEMA() const { return this->ema; }
+
+bool Tag::isReady() const { return count == 5; }
+
+bool Tag::shouldPublish()
+{
+    if (!this->isReady())
+    {
+#ifdef DEBUG
+        Serial.printf("[shouldPublish()]: isReady = false\n");
+        Serial.flush();
+#endif
+
+        return false;
+    }
+
+    uint32_t now = millis();
+
+    uint32_t time_delta = (this->prev_time == 0 ? INTERVAL : now - this->prev_time);
+    float ema_delta = std::abs(this->ema - this->prev_ema);
+
+    bool time_trigger = time_delta >= INTERVAL;
+    bool ema_trigger = ema_delta >= EMA_THRESHOLD;
+
+#ifdef DEBUG
+    Serial.printf("[shouldPublish()]: time_delta = %u\n"
+                  "[shouldPublish()]: ema_delta = %f\n"
+                  "[shouldPublish()]: time_trigger = %s\n"
+                  "[shouldPublish()]: ema_trigger = %s\n",
+                  time_delta,
+                  ema_delta,
+                  (time_trigger ? "true" : "false"),
+                  (ema_trigger ? "true" : "false"));
+    Serial.flush();
+#endif
+
+    if (time_trigger || ema_trigger)
+    {
+#ifdef DEBUG
+        Serial.printf("[shouldPublish()]: return true\n");
+        Serial.flush();
+#endif
+
+        this->prev_time = now;
+        this->prev_ema = this->ema;
+        return true;
+    }
+
+#ifdef DEBUG
+    Serial.printf("[shouldPublish()]: return false\n");
+    Serial.flush();
+#endif
+
+    return false;
+}
+
+void Tag::reset()
+{
+    this->win_idx = 0;
+    this->count = 0;
+    this->ema = 0;
+    this->init = false;
+
+    for (uint8_t i = 0; i < 5; i++)
+        this->rssi_window[i] = 0;
+}

src/officesense_scanner/src/tag/Tag.hpp

@@ -0,0 +1,41 @@
+#ifndef TAG_HPP
+#define TAG_HPP
+
+#include <Arduino.h>
+
+/*
+ * EMA_THRESHOLD Options:
+ * 1   - High Sensitivity / Fine Tracking
+ * 2   - Static Object / Presence Detection
+ * 3   - General Tracking (Default)
+ * 4-5 - Noisy Environment / Interference
+ */
+#define EMA_THRESHOLD 3
+#define INTERVAL 5000 // ms
+#define WINDOW_SIZE 5
+// #define DEBUG 1
+
+class Tag
+{
+private:
+    int8_t rssi_window[WINDOW_SIZE];
+    uint8_t win_idx = 0;
+    uint8_t count = 0;
+    uint32_t prev_time = 0;
+    uint32_t last_seen = 0;
+    float ema = 0;
+    float prev_ema = 0;
+    bool init = false;
+
+public:
+    Tag() = default;
+    uint32_t getLastSeen() const;
+    void addSample(int8_t rssi);
+    int8_t getMedian() const;
+    float getEMA() const;
+    bool isReady() const;
+    bool shouldPublish();
+    void reset();
+};
+
+#endif

src/officesense_scanner/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html