2026-10-23 16:49:37 +02:00
|
|
|
#include "obd_link.h"
|
|
|
|
|
|
|
|
|
|
#include <NimBLEDevice.h>
|
|
|
|
|
#include <Preferences.h>
|
2026-11-08 19:14:07 +01:00
|
|
|
#include <algorithm>
|
|
|
|
|
|
|
|
|
|
#include "can_log.h"
|
2026-10-23 16:49:37 +02:00
|
|
|
|
|
|
|
|
#define RETRY_DELAY_MS 3000
|
2026-10-23 18:32:56 +02:00
|
|
|
#define IDLE_RETRY_DELAY_MS 30000
|
2026-10-23 16:49:37 +02:00
|
|
|
#define POLL_FAIL_LIMIT 3
|
2026-10-23 18:32:56 +02:00
|
|
|
#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
|
2026-11-08 19:14:07 +01:00
|
|
|
#define LOG_OBD_INTERVAL_MS 2000 // speed while logging (between monitor windows)
|
|
|
|
|
#define LOG_SURVEY_MS 1300 // per ID range; > 1 s so slow (1 Hz) messages are seen
|
|
|
|
|
#define LOG_SURVEY_MAX_LINES 300 // more in one window: split the range
|
|
|
|
|
#define LOG_CAPTURE_MS 1300 // waiting for one frame of an ID
|
|
|
|
|
#define LOG_RAW_MS 2000
|
|
|
|
|
#define LOG_RESURVEY_MS (10 * 60 * 1000)
|
|
|
|
|
#define LOG_TARGET_EVERY 8 // the candidate IDs after every 8 others
|
2026-10-23 16:49:37 +02:00
|
|
|
|
|
|
|
|
// ---------- shared state (protected by `mtx`) ----------
|
|
|
|
|
static SemaphoreHandle_t mtx;
|
|
|
|
|
static volatile ObdState state = ObdState::NoDevice;
|
|
|
|
|
static volatile int speed_kmh = -1;
|
2026-10-23 18:32:56 +02:00
|
|
|
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)
|
2026-10-23 16:49:37 +02:00
|
|
|
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 ----------
|
2026-10-23 18:32:56 +02:00
|
|
|
static volatile bool req_select = false, req_forget = false, req_wake = false;
|
2026-10-23 16:49:37 +02:00
|
|
|
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;
|
2026-11-08 19:14:07 +01:00
|
|
|
static uint32_t next_attempt = 0, next_voltage = 0;
|
|
|
|
|
static int fails = 0;
|
|
|
|
|
|
|
|
|
|
// CAN logging (see can_log.h), worker-owned
|
|
|
|
|
enum class LogMode
|
|
|
|
|
{
|
|
|
|
|
Off, // not logging (or not set up since the last connect)
|
|
|
|
|
Survey, // finding the IDs on the bus, range by range
|
|
|
|
|
Capture, // one frame of every known ID in turn
|
|
|
|
|
Raw, // adapter can't filter by wildcard: unfiltered monitor windows
|
|
|
|
|
NoCan, // not a CAN car: only speed/voltage are logged
|
|
|
|
|
};
|
|
|
|
|
static LogMode log_mode = LogMode::Off;
|
2026-10-23 16:49:37 +02:00
|
|
|
|
|
|
|
|
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();
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-08 19:14:07 +01:00
|
|
|
static bool elm_write(const String &s)
|
|
|
|
|
{
|
|
|
|
|
if (!chr_write) // dropped meanwhile (adapter not answering)
|
|
|
|
|
return false;
|
|
|
|
|
return chr_write->writeValue((const uint8_t *)s.c_str(), s.length(), !chr_write->canWriteNoResponse());
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// 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();
|
2026-11-08 19:14:07 +01:00
|
|
|
if (!elm_write(String(cmd) + "\r"))
|
2026-10-23 16:49:37 +02:00
|
|
|
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 "";
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-08 19:14:07 +01:00
|
|
|
static String elm_command(const String &cmd, uint32_t timeout_ms) { return elm_command(cmd.c_str(), timeout_ms); }
|
|
|
|
|
|
|
|
|
|
// Monitor mode (ATMA) with the current CAN filter: collects received lines until `max_lines` arrived or `ms`
|
|
|
|
|
// passed, then stops it with a CR. Returns false if the adapter never came back with a prompt.
|
|
|
|
|
static bool elm_monitor(uint32_t ms, int max_lines, std::vector<String> &lines)
|
|
|
|
|
{
|
|
|
|
|
lines.clear();
|
|
|
|
|
lock();
|
|
|
|
|
elm_rx = "";
|
|
|
|
|
unlock();
|
|
|
|
|
if (!elm_write("ATMA\r"))
|
|
|
|
|
return false;
|
|
|
|
|
bool prompt = false, stop_sent = false;
|
|
|
|
|
uint32_t t0 = millis(), t_stop = 0;
|
|
|
|
|
for (;;)
|
|
|
|
|
{
|
|
|
|
|
lock();
|
|
|
|
|
int cr;
|
|
|
|
|
while ((cr = elm_rx.indexOf('\r')) >= 0)
|
|
|
|
|
{
|
|
|
|
|
String line = elm_rx.substring(0, cr);
|
|
|
|
|
elm_rx.remove(0, cr + 1);
|
|
|
|
|
line.trim();
|
|
|
|
|
if (line.length() && line != "STOPPED")
|
|
|
|
|
lines.push_back(line);
|
|
|
|
|
}
|
|
|
|
|
prompt = elm_rx.indexOf('>') >= 0; // monitoring ended (stopped by us, BUFFER FULL, error)
|
|
|
|
|
unlock();
|
|
|
|
|
if (prompt)
|
|
|
|
|
return true;
|
|
|
|
|
if (!client->isConnected())
|
|
|
|
|
return false;
|
|
|
|
|
if (!stop_sent && (millis() - t0 >= ms || (int)lines.size() >= max_lines))
|
|
|
|
|
{
|
|
|
|
|
if (!elm_write("\r"))
|
|
|
|
|
return false;
|
|
|
|
|
stop_sent = true;
|
|
|
|
|
t_stop = millis();
|
|
|
|
|
}
|
|
|
|
|
if (stop_sent && millis() - t_stop > 1500)
|
|
|
|
|
return false;
|
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(5));
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// "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);
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 18:32:56 +02:00
|
|
|
// "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;
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// ---------- connection ----------
|
|
|
|
|
static void disconnect()
|
|
|
|
|
{
|
|
|
|
|
chr_write = chr_notify = nullptr;
|
|
|
|
|
if (client && client->isConnected())
|
|
|
|
|
client->disconnect();
|
|
|
|
|
speed_kmh = -1;
|
2026-11-08 19:14:07 +01:00
|
|
|
if (log_mode != LogMode::Off)
|
|
|
|
|
{
|
|
|
|
|
can_log_line("E disconnected");
|
|
|
|
|
can_log_set_phase("Adapter not connected");
|
|
|
|
|
}
|
|
|
|
|
log_mode = LogMode::Off; // ATZ on reconnect resets the adapter: set up again
|
2026-10-23 16:49:37 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-08 19:14:07 +01:00
|
|
|
// ---------- speed polling ----------
|
|
|
|
|
// Counts a missing prompt; after a few the adapter is considered hung and the link is dropped.
|
|
|
|
|
static void note_no_reply()
|
|
|
|
|
{
|
|
|
|
|
if (++fails >= POLL_FAIL_LIMIT)
|
|
|
|
|
{
|
|
|
|
|
set_status("Adapter not answering");
|
|
|
|
|
disconnect();
|
|
|
|
|
next_attempt = millis() + RETRY_DELAY_MS;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void poll_speed()
|
|
|
|
|
{
|
|
|
|
|
String reply = elm_command("010D", 1500);
|
|
|
|
|
int v = parse_speed(reply);
|
|
|
|
|
speed_kmh = v;
|
|
|
|
|
can_log_line("V " + String(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
|
|
|
|
|
note_no_reply();
|
|
|
|
|
}
|
|
|
|
|
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));
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---------- CAN logging ----------
|
|
|
|
|
// Ford candidates from opendbc (ford_lincoln_base_pt.dbc, newer Fords - to be confirmed on the Fiesta Mk8):
|
|
|
|
|
// 0x165 EngBrakeData: Veh_V_DsplyCcSet (cruise set speed as shown), 0x202: Veh_V_RqCcSet (ACC set speed),
|
|
|
|
|
// 0x3D9 IPMA_Data2: IsaVLim_D_Rq (speed limit from the camera)
|
|
|
|
|
static const uint16_t LOG_TARGETS[CANLOG_SLOTS] = {0x165, 0x202, 0x3D9};
|
|
|
|
|
static std::vector<String> survey_queue; // ID patterns still to listen to ("3XX", "3DX", "3D9")
|
|
|
|
|
static std::vector<uint16_t> log_ids; // all IDs seen, sorted
|
|
|
|
|
static size_t capture_index = 0;
|
|
|
|
|
static uint32_t capture_count = 0;
|
|
|
|
|
static uint32_t next_log_obd = 0, next_survey = 0;
|
|
|
|
|
static bool exact_filter = false; // adapter supports ATCRA hhh
|
|
|
|
|
static int id_digits = 3; // 11-bit IDs; 29-bit in Raw mode
|
|
|
|
|
static bool caf_on = true; // CAN auto formatting (ISO-TP), the adapter's default
|
|
|
|
|
|
|
|
|
|
// Monitoring wants the raw frames (CAF off), OBD requests need CAF on (adds/strips the ISO-TP byte)
|
|
|
|
|
static void set_caf(bool on)
|
|
|
|
|
{
|
|
|
|
|
if (on != caf_on && elm_command(on ? "ATCAF1" : "ATCAF0", 1500).length())
|
|
|
|
|
caf_on = on;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// "3D9FE00000000000" (ATH1, ATS0) -> id "3D9", data "FE00000000000000"
|
|
|
|
|
static bool parse_frame(String line, String &id, String &data)
|
|
|
|
|
{
|
|
|
|
|
line.replace(" ", "");
|
|
|
|
|
if (line.length() < (unsigned)id_digits || (line.length() - id_digits) % 2)
|
|
|
|
|
return false;
|
|
|
|
|
for (unsigned i = 0; i < line.length(); i++)
|
|
|
|
|
if (!isxdigit((unsigned char)line[i]))
|
|
|
|
|
return false;
|
|
|
|
|
line.toUpperCase();
|
|
|
|
|
id = line.substring(0, id_digits);
|
|
|
|
|
data = line.substring(id_digits);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void note_id(uint16_t id)
|
|
|
|
|
{
|
|
|
|
|
auto it = std::lower_bound(log_ids.begin(), log_ids.end(), id);
|
|
|
|
|
if (it == log_ids.end() || *it != id)
|
|
|
|
|
log_ids.insert(it, id);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Logs the received lines: frames (at most `per_id` per ID, 0 = all) and adapter messages
|
|
|
|
|
static void log_lines(const std::vector<String> &lines, int per_id, bool *overflow)
|
|
|
|
|
{
|
|
|
|
|
std::vector<std::pair<String, int>> counts;
|
|
|
|
|
for (const String &line : lines)
|
|
|
|
|
{
|
|
|
|
|
String id, data;
|
|
|
|
|
if (!parse_frame(line, id, data))
|
|
|
|
|
{
|
|
|
|
|
if (line.indexOf("BUFFER FULL") >= 0 && overflow)
|
|
|
|
|
*overflow = true;
|
|
|
|
|
can_log_line("E " + line);
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
if (id_digits == 3)
|
|
|
|
|
note_id((uint16_t)strtol(id.c_str(), nullptr, 16));
|
|
|
|
|
int n = 0;
|
|
|
|
|
for (auto &c : counts)
|
|
|
|
|
if (c.first == id)
|
|
|
|
|
n = ++c.second;
|
|
|
|
|
if (n == 0)
|
|
|
|
|
counts.push_back({id, n = 1});
|
|
|
|
|
if (per_id == 0 || n <= per_id)
|
|
|
|
|
can_log_line("F " + id + " " + data);
|
|
|
|
|
for (int s = 0; s < CANLOG_SLOTS; s++)
|
|
|
|
|
if (id_digits == 3 && strtol(id.c_str(), nullptr, 16) == LOG_TARGETS[s])
|
|
|
|
|
can_log_set_frame(s, data);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static String id_hex(uint16_t id)
|
|
|
|
|
{
|
|
|
|
|
char b[4];
|
|
|
|
|
snprintf(b, sizeof(b), "%03X", id);
|
|
|
|
|
return b;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Waits for one frame of `id`
|
|
|
|
|
static void capture_id(uint16_t id)
|
|
|
|
|
{
|
|
|
|
|
if (elm_command("ATCRA " + id_hex(id), 1500).length() == 0)
|
|
|
|
|
return note_no_reply();
|
|
|
|
|
std::vector<String> lines;
|
|
|
|
|
if (!elm_monitor(LOG_CAPTURE_MS, 1, lines))
|
|
|
|
|
return note_no_reply();
|
|
|
|
|
fails = 0;
|
|
|
|
|
if (lines.empty())
|
|
|
|
|
can_log_line("N " + id_hex(id));
|
|
|
|
|
log_lines(lines, 0, nullptr);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Listens to one ID range ("3XX"); a range too busy for the adapter is split into 16 smaller ones
|
|
|
|
|
static void survey_range(const String &pattern)
|
|
|
|
|
{
|
|
|
|
|
if (elm_command("ATCRA " + pattern, 1500).length() == 0)
|
|
|
|
|
return note_no_reply();
|
|
|
|
|
std::vector<String> lines;
|
|
|
|
|
if (!elm_monitor(LOG_SURVEY_MS, LOG_SURVEY_MAX_LINES, lines))
|
|
|
|
|
return note_no_reply();
|
|
|
|
|
fails = 0;
|
|
|
|
|
bool overflow = (int)lines.size() >= LOG_SURVEY_MAX_LINES;
|
|
|
|
|
log_lines(lines, 2, &overflow);
|
|
|
|
|
int x = pattern.indexOf('X');
|
|
|
|
|
if (overflow && x >= 0)
|
|
|
|
|
for (int d = 0; d < 16; d++)
|
|
|
|
|
{
|
|
|
|
|
String child = pattern;
|
|
|
|
|
child.setCharAt(x, "0123456789ABCDEF"[d]);
|
|
|
|
|
survey_queue.push_back(child);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void start_survey()
|
|
|
|
|
{
|
|
|
|
|
survey_queue.clear();
|
|
|
|
|
for (int i = 0; i < 8; i++)
|
|
|
|
|
survey_queue.push_back(String(i) + "XX");
|
|
|
|
|
log_mode = LogMode::Survey;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Called once per connection while logging is on: checks the protocol and what the adapter can filter
|
|
|
|
|
static void log_setup()
|
|
|
|
|
{
|
|
|
|
|
can_log_line("C " + (saved_name.length() ? saved_name : saved_addr) + " " + elm_command("ATI", 1500));
|
|
|
|
|
String dp = elm_command("ATDPN", 1500); // "A6" = auto, ISO 15765-4 CAN 11 bit 500 kbit
|
|
|
|
|
can_log_line("P " + dp);
|
|
|
|
|
char p = dp.length() ? dp[dp.length() - 1] : '?';
|
|
|
|
|
next_log_obd = 0;
|
|
|
|
|
caf_on = true; // adapter was just reset (ATZ)
|
|
|
|
|
if (p < '6' || p > '9')
|
|
|
|
|
{
|
|
|
|
|
log_mode = LogMode::NoCan;
|
|
|
|
|
can_log_set_phase("Not a CAN protocol (" + dp + ")");
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
// silent monitoring: the adapter never acknowledges or disturbs frames on the bus
|
|
|
|
|
can_log_line("E ATCSM1 " + elm_command("ATCSM1", 1500));
|
|
|
|
|
elm_command("ATH1", 1500); // IDs in front of the data
|
|
|
|
|
id_digits = (p == '7' || p == '9') ? 8 : 3;
|
|
|
|
|
exact_filter = id_digits == 3 && elm_command("ATCRA 3D9", 1500).indexOf('?') < 0;
|
|
|
|
|
bool wildcard = exact_filter && elm_command("ATCRA 7XX", 1500).indexOf('?') < 0;
|
|
|
|
|
elm_command("ATAR", 1500);
|
|
|
|
|
if (wildcard)
|
|
|
|
|
{
|
|
|
|
|
start_survey();
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
log_mode = LogMode::Raw;
|
|
|
|
|
can_log_line(String("E no wildcard filter") + (exact_filter ? "" : ", no ID filter") + ": unfiltered");
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static void log_stop()
|
|
|
|
|
{
|
|
|
|
|
set_caf(true);
|
|
|
|
|
elm_command("ATAR", 1500);
|
|
|
|
|
elm_command("ATH0", 1500);
|
|
|
|
|
log_mode = LogMode::Off;
|
|
|
|
|
can_log_set_phase("");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// One unit of logging work (one ID range / one ID / one raw window), with speed + voltage every 2 s
|
|
|
|
|
static void log_step()
|
|
|
|
|
{
|
|
|
|
|
if (log_mode == LogMode::Off)
|
|
|
|
|
log_setup();
|
|
|
|
|
if ((int32_t)(millis() - next_log_obd) >= 0)
|
|
|
|
|
{
|
|
|
|
|
next_log_obd = millis() + LOG_OBD_INTERVAL_MS;
|
|
|
|
|
elm_command("ATAR", 1500); // the ID filter would hide the ECU's reply
|
|
|
|
|
set_caf(true);
|
|
|
|
|
poll_speed();
|
|
|
|
|
if ((int32_t)(millis() - next_voltage) >= 0)
|
|
|
|
|
{
|
|
|
|
|
next_voltage = millis() + VOLTAGE_INTERVAL_MS;
|
|
|
|
|
read_voltage();
|
|
|
|
|
can_log_line("B " + String(voltage, 1));
|
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
if (!client->isConnected() || !chr_write)
|
|
|
|
|
return;
|
|
|
|
|
if (log_mode != LogMode::NoCan)
|
|
|
|
|
set_caf(false);
|
|
|
|
|
|
|
|
|
|
switch (log_mode)
|
|
|
|
|
{
|
|
|
|
|
case LogMode::Survey:
|
|
|
|
|
if (survey_queue.empty())
|
|
|
|
|
{
|
|
|
|
|
String ids;
|
|
|
|
|
for (uint16_t id : log_ids)
|
|
|
|
|
ids += " " + id_hex(id);
|
|
|
|
|
can_log_line("I" + ids);
|
|
|
|
|
log_mode = LogMode::Capture;
|
|
|
|
|
next_survey = millis() + LOG_RESURVEY_MS;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
{
|
|
|
|
|
String pattern = survey_queue.front();
|
|
|
|
|
survey_queue.erase(survey_queue.begin());
|
|
|
|
|
can_log_set_phase("Survey " + pattern + ": " + log_ids.size() + " IDs, " + survey_queue.size() + " ranges left");
|
|
|
|
|
survey_range(pattern);
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
case LogMode::Capture:
|
|
|
|
|
if ((int32_t)(millis() - next_survey) >= 0)
|
|
|
|
|
{
|
|
|
|
|
start_survey(); // now and then look for IDs that were quiet before
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
if (capture_count++ % (LOG_TARGET_EVERY + 1) == 0 || log_ids.empty())
|
|
|
|
|
{
|
|
|
|
|
for (uint16_t t : LOG_TARGETS)
|
|
|
|
|
capture_id(t);
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
uint16_t id = log_ids[capture_index++ % log_ids.size()];
|
|
|
|
|
if (std::find(std::begin(LOG_TARGETS), std::end(LOG_TARGETS), id) == std::end(LOG_TARGETS))
|
|
|
|
|
capture_id(id);
|
|
|
|
|
}
|
|
|
|
|
can_log_set_phase("Capturing " + String(log_ids.size()) + " IDs");
|
|
|
|
|
break;
|
|
|
|
|
case LogMode::Raw:
|
|
|
|
|
{
|
|
|
|
|
can_log_set_phase("Unfiltered (adapter can't filter)");
|
|
|
|
|
std::vector<String> lines;
|
|
|
|
|
if (!elm_monitor(LOG_RAW_MS, LOG_SURVEY_MAX_LINES, lines))
|
|
|
|
|
return note_no_reply();
|
|
|
|
|
fails = 0;
|
|
|
|
|
log_lines(lines, 0, nullptr);
|
|
|
|
|
if (exact_filter)
|
|
|
|
|
{
|
|
|
|
|
for (uint16_t t : LOG_TARGETS)
|
|
|
|
|
capture_id(t);
|
|
|
|
|
elm_command("ATAR", 1500);
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
default: // NoCan: only speed and voltage
|
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// ---------- 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 *)
|
|
|
|
|
{
|
2026-10-23 18:32:56 +02:00
|
|
|
last_data_ms = millis();
|
2026-10-23 16:49:37 +02:00
|
|
|
for (;;)
|
|
|
|
|
{
|
2026-10-23 18:32:56 +02:00
|
|
|
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");
|
2026-11-08 19:14:07 +01:00
|
|
|
can_log_line("E idle");
|
2026-10-23 18:32:56 +02:00
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// 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;
|
2026-10-23 18:32:56 +02:00
|
|
|
idle = false;
|
|
|
|
|
last_data_ms = millis(); // no idle while no adapter is chosen (user is setting up)
|
2026-10-23 16:49:37 +02:00
|
|
|
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;
|
2026-10-23 18:32:56 +02:00
|
|
|
next_attempt = millis() + (idle ? IDLE_RETRY_DELAY_MS : RETRY_DELAY_MS);
|
2026-10-23 16:49:37 +02:00
|
|
|
}
|
2026-10-23 18:32:56 +02:00
|
|
|
next_voltage = 0;
|
2026-10-23 16:49:37 +02:00
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 18:32:56 +02:00
|
|
|
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);
|
2026-11-08 19:14:07 +01:00
|
|
|
can_log_line("E wake " + String(voltage, 1) + " V");
|
2026-10-23 18:32:56 +02:00
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
2026-11-08 19:14:07 +01:00
|
|
|
// CAN logging on: monitor windows with speed/voltage in between
|
|
|
|
|
if (can_log_enabled())
|
|
|
|
|
{
|
|
|
|
|
log_step();
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
if (log_mode != LogMode::Off)
|
|
|
|
|
log_stop();
|
|
|
|
|
|
2026-10-23 18:32:56 +02:00
|
|
|
if ((int32_t)(millis() - next_voltage) >= 0)
|
|
|
|
|
{
|
|
|
|
|
next_voltage = millis() + VOLTAGE_INTERVAL_MS;
|
|
|
|
|
read_voltage();
|
|
|
|
|
}
|
|
|
|
|
|
2026-10-23 16:49:37 +02:00
|
|
|
// connected: poll the vehicle speed
|
2026-11-08 19:14:07 +01:00
|
|
|
poll_speed();
|
2026-10-23 16:49:37 +02:00
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ---------- 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; }
|
2026-10-23 18:32:56 +02:00
|
|
|
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; }
|
2026-10-23 16:49:37 +02:00
|
|
|
|
|
|
|
|
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; }
|