Bridge between Garmin and FTMS treadmill using ESP32

garmin-ftms-esp32.ino

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLE2902.h>

// --- UUIDs ---
#define RSC_SERVICE "1814"
#define RSC_MEASUREMENT "2a53"
#define RSC_FEATURE "2a54"
#define DIS_SERVICE "180a"

#define FTMS_SERVICE "1826"
#define FTMS_TREADMILL_DATA "2acd"

BLEServer* bleServer = nullptr;
BLECharacteristic* rscMeasurement = nullptr;
BLECharacteristic* hrMeasurement = nullptr;
BLEClient* treadmillClient = nullptr;
BLEAdvertisedDevice* treadmillDevice = nullptr;

bool treadmillConnected = false;
bool garminConnected = false;
bool doConnectTreadmill = false;
float currentSpeedMs = 0.0;
unsigned long lastNotifyTime = 0;
unsigned long lastHrTime = 0;

// --- CALLBACKS ---
class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer*) override {
    garminConnected = true;
    lastNotifyTime = 0;
    lastHrTime = 0;
    Serial.println(">>> Garmin connected");
  }
  void onDisconnect(BLEServer*) override {
    garminConnected = false;
    Serial.println(">>> Garmin disconnected");
    BLEDevice::startAdvertising();
  }
};

void treadmillNotifyCallback(BLERemoteCharacteristic*, uint8_t* pData, size_t length, bool) {
  if (length < 4) return;
  uint16_t rawSpeed = pData[2] | (pData[3] << 8);
  currentSpeedMs = (rawSpeed / 100.0f) / 3.6f;
}

class MyScanCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) override {
    if (advertisedDevice.isAdvertisingService(BLEUUID(FTMS_SERVICE))) {
      BLEDevice::getScan()->stop();
      treadmillDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnectTreadmill = true;
    }
  }
};

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("Booting fake HRM-Pro+");

  BLEDevice::init("HRM-Pro+");
  bleServer = BLEDevice::createServer();
  bleServer->setCallbacks(new MyServerCallbacks());

  // 1. Device Information Service
  BLEService* pDis = bleServer->createService(DIS_SERVICE);
  pDis->createCharacteristic("2a29", BLECharacteristic::PROPERTY_READ)->setValue("Garmin");
  pDis->createCharacteristic("2a24", BLECharacteristic::PROPERTY_READ)->setValue("HRM-Pro Plus");
  pDis->createCharacteristic("2a27", BLECharacteristic::PROPERTY_READ)->setValue("A4123");
  pDis->createCharacteristic("2a26", BLECharacteristic::PROPERTY_READ)->setValue("4.10");
  pDis->createCharacteristic("2a25", BLECharacteristic::PROPERTY_READ)->setValue("3954123456");
  pDis->start();

  // 2. RSC Service (running speed/cadence)
  BLEService* pRsc = bleServer->createService(RSC_SERVICE);
  rscMeasurement = pRsc->createCharacteristic(RSC_MEASUREMENT, BLECharacteristic::PROPERTY_NOTIFY);
  rscMeasurement->addDescriptor(new BLE2902());

  // Feature 0x03 = Instantaneous Stride Length + Total Distance Measurement
  // Use 0x00 for minimal — speed and cadence only
  uint8_t fData[] = { 0x00, 0x00 };
  pRsc->createCharacteristic(RSC_FEATURE, BLECharacteristic::PROPERTY_READ)->setValue(fData, 2);
  pRsc->start();

  // 3. Advertising
  BLEAdvertising* pAdv = BLEDevice::getAdvertising();
  pAdv->addServiceUUID(RSC_SERVICE);
  pAdv->setAppearance(0x0341);
  pAdv->setScanResponse(true);
  pAdv->start();

  // 4. Scan for treadmill
  BLEScan* pScan = BLEDevice::getScan();
  pScan->setAdvertisedDeviceCallbacks(new MyScanCallbacks());
  pScan->setActiveScan(true);
  pScan->start(0);
}

void loop() {
  if (doConnectTreadmill) {
    doConnectTreadmill = false;
    treadmillClient = BLEDevice::createClient();
    if (treadmillClient->connect(treadmillDevice)) {
      BLERemoteService* pSvc = treadmillClient->getService(FTMS_SERVICE);
      if (pSvc) {
        BLERemoteCharacteristic* pChar = pSvc->getCharacteristic(FTMS_TREADMILL_DATA);
        if (pChar) {
          pChar->registerForNotify(treadmillNotifyCallback);
          treadmillConnected = true;
          Serial.println(">>> Treadmill connected");
        }
      }
    }
  }

  if (garminConnected && treadmillConnected) {
    unsigned long now = millis();

    // Send RSC data every 750ms
    if (now - lastNotifyTime >= 750) {
      lastNotifyTime = now;

      uint16_t s = (uint16_t)(currentSpeedMs * 256.0f);
      uint8_t cadence = 160;

      uint8_t payload[4];
      payload[0] = 0x00;
      payload[1] = s & 0xFF;
      payload[2] = (s >> 8) & 0xFF;
      payload[3] = cadence;

      rscMeasurement->setValue(payload, 4);
      rscMeasurement->notify();

      Serial.printf("[RSC] kmh=%.2f cadence=%d\n", currentSpeedMs * 3.6f, cadence);
    }
  }
}