JC3636W518EN/12-OBD-Speed/src/obd_link.cpp

#include "obd_link.h"

#include <NimBLEDevice.h>
#include <Preferences.h>

#define RETRY_DELAY_MS 3000
#define IDLE_RETRY_DELAY_MS 30000
#define POLL_FAIL_LIMIT 3
#define VOLTAGE_INTERVAL_MS 2000       // ATRV while driving
#define IDLE_VOLTAGE_INTERVAL_MS 30000 // ATRV while idle (adapter-internal, doesn't wake the car's bus)
#define WAKE_VOLTAGE 13.2f             // above this the alternator is charging: engine running

// ---------- shared state (protected by `mtx`) ----------
static SemaphoreHandle_t mtx;
static volatile ObdState state = ObdState::NoDevice;
static volatile int speed_kmh = -1;
static volatile float voltage = -1;
static volatile bool idle = false;
static volatile uint32_t idle_timeout_ms = 2 * 60 * 1000;
static volatile uint32_t last_data_ms = 0; // last valid speed (or wake-up)
static String status_text = "starting";
static String saved_addr, saved_name;
static uint8_t saved_type = 0;
static std::vector<ObdDevInfo> devices;
static uint32_t devices_version = 1;
static String elm_rx; // bytes received from the ELM (notifications)

// ---------- requests from the UI ----------
static volatile bool req_select = false, req_forget = false, req_wake = false;
static volatile bool scan_wanted = false;
static ObdDevInfo req_dev;

// ---------- worker-owned ----------
static NimBLEClient *client = nullptr;
static NimBLERemoteCharacteristic *chr_write = nullptr, *chr_notify = nullptr;
static Preferences prefs;

static void lock() { xSemaphoreTake(mtx, portMAX_DELAY); }
static void unlock() { xSemaphoreGive(mtx); }

static void set_status(const String &s)
{
  lock();
  bool changed = status_text != s;
  status_text = s;
  unlock();
  if (changed)
    Serial.println("[obd] " + s);
}

// Services that cheap BLE OBD adapters use for their serial (UART-like) link
static const uint16_t ELM_SERVICE_HINTS[] = {0xFFF0, 0xFFE0, 0x18F0};

static bool name_looks_like_obd(String n)
{
  n.toUpperCase();
  return n.indexOf("OBD") >= 0 || n.indexOf("ELM") >= 0 || n.indexOf("VLINK") >= 0 ||
         n.indexOf("V-LINK") >= 0 || n.indexOf("VEEPEAK") >= 0 || n.indexOf("VGATE") >= 0;
}

// ---------- scanning (menu) ----------
class ScanCallbacks : public NimBLEScanCallbacks
{
  void onResult(const NimBLEAdvertisedDevice *adv) override
  {
    if (!adv->isConnectable())
      return;
    String addr = adv->getAddress().toString().c_str();
    String name = adv->haveName() ? String(adv->getName().c_str()) : String();
    bool hint = name_looks_like_obd(name);
    for (uint8_t i = 0; i < adv->getServiceUUIDCount(); i++)
      for (uint16_t h : ELM_SERVICE_HINTS)
        if (adv->getServiceUUID(i).equals(NimBLEUUID(h)))
          hint = true;

    lock();
    ObdDevInfo *d = nullptr;
    for (auto &e : devices)
      if (e.address == addr)
        d = &e;
    if (!d)
    {
      devices.push_back(ObdDevInfo());
      d = &devices.back();
      d->address = addr;
      d->obd_hint = false;
    }
    d->addr_type = adv->getAddress().getType();
    if (name.length())
      d->name = name;
    d->rssi = adv->getRSSI();
    d->obd_hint = d->obd_hint || hint;
    devices_version++;
    unlock();
  }
};
static ScanCallbacks scan_callbacks;

static void scan_set(bool on)
{
  NimBLEScan *scan = NimBLEDevice::getScan();
  if (on && !scan->isScanning())
  {
    scan->setScanCallbacks(&scan_callbacks, true);
    scan->setActiveScan(true);
    scan->setInterval(100);
    scan->setWindow(90);
    scan->start(0, false, true);
  }
  else if (!on && scan->isScanning())
  {
    scan->stop();
  }
}

// ---------- ELM327 communication ----------
static void on_notify(NimBLERemoteCharacteristic *c, uint8_t *data, size_t len, bool is_notify)
{
  lock();
  for (size_t i = 0; i < len; i++)
    elm_rx += (char)data[i];
  unlock();
}

// Sends a command, waits for the '>' prompt. Returns the reply without prompt/CR, or "" on timeout.
static String elm_command(const char *cmd, uint32_t timeout_ms)
{
  lock();
  elm_rx = "";
  unlock();
  String line = String(cmd) + "\r";
  if (!chr_write->writeValue((const uint8_t *)line.c_str(), line.length(), !chr_write->canWriteNoResponse()))
    return "";
  uint32_t t0 = millis();
  String rx;
  while (millis() - t0 < timeout_ms)
  {
    lock();
    rx = elm_rx;
    unlock();
    if (rx.indexOf('>') >= 0)
    {
      rx.replace(">", "");
      rx.replace("\r", "\n");
      rx.trim();
      return rx;
    }
    if (!client->isConnected())
      return "";
    vTaskDelay(pdMS_TO_TICKS(5));
  }
  return "";
}

// "410D3C" (spaces/headers removed by ATS0/ATH0; multiple ECUs possible) -> 60
static int parse_speed(String reply)
{
  reply.replace(" ", "");
  reply.toUpperCase();
  int i = reply.indexOf("410D");
  if (i < 0 || reply.length() < (unsigned)i + 6)
    return -1;
  return (int)strtol(reply.substring(i + 4, i + 6).c_str(), nullptr, 16);
}

// "12.6V" -> 12.6, -1 if not a voltage
static float parse_voltage(const String &reply)
{
  int v = reply.indexOf('V');
  if (v <= 0)
    return -1;
  float f = reply.substring(0, v).toFloat();
  return f > 0 ? f : -1;
}

static void read_voltage()
{
  float v = parse_voltage(elm_command("ATRV", 1500));
  if (v > 0)
    voltage = v;
}

// ---------- connection ----------
static void disconnect()
{
  chr_write = chr_notify = nullptr;
  if (client && client->isConnected())
    client->disconnect();
  speed_kmh = -1;
}

// Finds the ELM UART characteristics: service FFF0 (verified: FFF1 notify + write), else any notify+write pair
static bool find_characteristics()
{
  chr_write = chr_notify = nullptr;
  NimBLERemoteService *svc = client->getService(NimBLEUUID((uint16_t)0xFFF0));
  if (svc)
  {
    chr_notify = svc->getCharacteristic(NimBLEUUID((uint16_t)0xFFF1));
    if (chr_notify && (chr_notify->canWrite() || chr_notify->canWriteNoResponse()))
      chr_write = chr_notify;
    else
      chr_write = svc->getCharacteristic(NimBLEUUID((uint16_t)0xFFF2));
  }
  if (!chr_write || !chr_notify)
  {
    chr_write = chr_notify = nullptr;
    for (NimBLERemoteService *s : client->getServices(true))
    {
      NimBLERemoteCharacteristic *w = nullptr, *n = nullptr;
      for (NimBLERemoteCharacteristic *c : s->getCharacteristics(true))
      {
        if (!n && (c->canNotify() || c->canIndicate()))
          n = c;
        if (!w && (c->canWrite() || c->canWriteNoResponse()))
          w = c;
      }
      if (w && n)
      {
        chr_write = w;
        chr_notify = n;
        break;
      }
    }
  }
  return chr_write && chr_notify;
}

static bool connect_and_init()
{
  bool was_scanning = NimBLEDevice::getScan()->isScanning();
  scan_set(false); // NimBLE can't connect while scanning

  state = ObdState::Connecting;
  set_status("Connecting " + (saved_name.length() ? saved_name : saved_addr));
  if (!client)
  {
    client = NimBLEDevice::createClient();
    client->setConnectTimeout(8000);
  }
  bool ok = client->connect(NimBLEAddress(std::string(saved_addr.c_str()), saved_type));
  if (was_scanning && scan_wanted)
    scan_set(true);
  if (!ok)
  {
    set_status("Adapter not found");
    return false;
  }
  if (!find_characteristics() || !chr_notify->subscribe(chr_notify->canNotify(), on_notify))
  {
    set_status("Not an ELM adapter?");
    disconnect();
    return false;
  }

  state = ObdState::Initializing;
  set_status("Initializing ELM327");
  if (elm_command("ATZ", 4000).length() == 0)
  {
    set_status("ELM327 not responding");
    disconnect();
    return false;
  }
  // echo off, no linefeeds, no spaces, no headers, automatic protocol
  const char *init[] = {"ATE0", "ATL0", "ATS0", "ATH0", "ATSP0"};
  for (const char *c : init)
    elm_command(c, 2000);
  set_status("Searching protocol");
  elm_command("0100", 10000); // the first OBD request triggers the protocol search
  state = ObdState::Running;
  return true;
}

// ---------- worker task ----------
static void load_saved()
{
  prefs.begin("obd", true);
  saved_addr = prefs.getString("addr", "");
  saved_name = prefs.getString("name", "");
  saved_type = prefs.getUChar("type", 0);
  prefs.end();
}

static void worker_task(void *)
{
  uint32_t next_attempt = 0;
  uint32_t next_voltage = 0;
  int fails = 0;
  last_data_ms = millis();
  for (;;)
  {
    if (req_wake)
    {
      req_wake = false;
      if (idle)
        Serial.println("[obd] wake (touch)");
      idle = false;
      last_data_ms = millis();
      next_attempt = 0;
    }
    // no speed data for a while (ignition off / adapter gone): go idle
    if (!idle && idle_timeout_ms && millis() - last_data_ms > idle_timeout_ms)
    {
      idle = true;
      speed_kmh = -1;
      Serial.println("[obd] idle");
    }

    // requests from the UI
    if (req_forget)
    {
      req_forget = false;
      disconnect();
      prefs.begin("obd", false);
      prefs.clear();
      prefs.end();
      lock();
      saved_addr = saved_name = "";
      unlock();
    }
    if (req_select)
    {
      req_select = false;
      disconnect();
      prefs.begin("obd", false);
      prefs.putString("addr", req_dev.address);
      prefs.putString("name", req_dev.name);
      prefs.putUChar("type", req_dev.addr_type);
      prefs.end();
      lock();
      saved_addr = req_dev.address;
      saved_name = req_dev.name;
      saved_type = req_dev.addr_type;
      unlock();
      next_attempt = 0;
    }
    if (state != ObdState::Connecting) // never scan during a connect attempt
      scan_set(scan_wanted);

    if (saved_addr.length() == 0)
    {
      state = ObdState::NoDevice;
      idle = false;
      last_data_ms = millis(); // no idle while no adapter is chosen (user is setting up)
      set_status("No adapter - swipe left");
      vTaskDelay(pdMS_TO_TICKS(100));
      continue;
    }

    if (!client || !client->isConnected() || !chr_write)
    {
      if (state == ObdState::Running)
        set_status("Connection lost");
      speed_kmh = -1;
      if ((int32_t)(millis() - next_attempt) < 0)
      {
        state = ObdState::Waiting;
        vTaskDelay(pdMS_TO_TICKS(50));
        continue;
      }
      fails = 0;
      if (!connect_and_init())
      {
        disconnect();
        state = ObdState::Waiting;
        next_attempt = millis() + (idle ? IDLE_RETRY_DELAY_MS : RETRY_DELAY_MS);
      }
      next_voltage = 0;
      continue;
    }

    if (idle)
    {
      // only the battery voltage now and then; an engine start (charging voltage) wakes us up
      if ((int32_t)(millis() - next_voltage) >= 0)
      {
        next_voltage = millis() + IDLE_VOLTAGE_INTERVAL_MS;
        read_voltage();
        if (voltage > WAKE_VOLTAGE)
        {
          idle = false;
          last_data_ms = millis();
          Serial.printf("[obd] wake (%.1f V)\n", voltage);
        }
      }
      vTaskDelay(pdMS_TO_TICKS(100));
      continue;
    }

    if ((int32_t)(millis() - next_voltage) >= 0)
    {
      next_voltage = millis() + VOLTAGE_INTERVAL_MS;
      read_voltage();
    }

    // connected: poll the vehicle speed
    String reply = elm_command("010D", 1500);
    int v = parse_speed(reply);
    speed_kmh = v;
    if (v >= 0)
    {
      fails = 0;
      last_data_ms = millis();
      set_status(saved_name.length() ? saved_name : saved_addr);
    }
    else if (reply.length() == 0)
    {
      // no prompt at all: adapter hung or link dead
      if (++fails >= POLL_FAIL_LIMIT)
      {
        set_status("Adapter not answering");
        disconnect();
        next_attempt = millis() + RETRY_DELAY_MS;
      }
    }
    else
    {
      // "NO DATA", "UNABLE TO CONNECT", "STOPPED"...: ignition off or ECU busy
      fails = 0;
      String r = reply;
      r.replace("\n", " ");
      set_status(r.substring(0, 24));
      vTaskDelay(pdMS_TO_TICKS(500));
    }
  }
}

// ---------- public API ----------
void obd_begin()
{
  mtx = xSemaphoreCreateMutex();
  load_saved();
  NimBLEDevice::init("JC3636W518-OBD");
  NimBLEDevice::setPower(9);
  xTaskCreatePinnedToCore(worker_task, "obd_link", 8192, nullptr, 2, nullptr, 0);
}

ObdState obd_state() { return state; }
int obd_speed() { return speed_kmh; }
float obd_voltage() { return voltage; }
bool obd_is_idle() { return idle; }
void obd_wake() { req_wake = true; }
void obd_set_idle_timeout(uint32_t minutes) { idle_timeout_ms = minutes * 60 * 1000; }

String obd_status_text()
{
  lock();
  String s = status_text;
  unlock();
  return s;
}

String obd_saved_name()
{
  lock();
  String s = saved_name;
  unlock();
  return s;
}

String obd_saved_address()
{
  lock();
  String s = saved_addr;
  unlock();
  return s;
}

void obd_scan_enable(bool on) { scan_wanted = on; }

bool obd_get_devices(std::vector<ObdDevInfo> &out, uint32_t &version)
{
  lock();
  bool changed = version != devices_version;
  if (changed)
  {
    out = devices;
    version = devices_version;
  }
  unlock();
  return changed;
}

void obd_select_device(const ObdDevInfo &dev)
{
  req_dev = dev;
  req_select = true;
}

void obd_forget_device() { req_forget = true; }