#include "obd_link.h" #include #include #include #include "can_log.h" #include "watch_dids.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 #define LOG_OBD_INTERVAL_MS 2000 // speed while scanning (between requests) // ---------- 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 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 uint32_t next_attempt = 0, next_voltage = 0; static int fails = 0; static bool scan_ready = false; // diagnostic scan set up on this connection (see below) 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(); } 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()); } // 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(); if (!elm_write(String(cmd) + "\r")) 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 ""; } 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 &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)); } } // "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; if (scan_ready) { can_log_line("E disconnected"); can_log_set_phase("Adapter not connected"); } scan_ready = false; // ATZ on reconnect resets the adapter: set up again } // 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; } client->updateConnParams(6, 12, 0, 400); // 7.5-15 ms interval (the scan sends thousands of requests) 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; } // ---------- 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)); } } // ---------- diagnostic scan (while logging is on, see can_log.h) ---------- // The Fiesta Mk8's OBD port only carries diagnostic traffic (gateway; first test drive, docs/canlog), so the // cruise set speed and the speed limit have to be asked from the modules. Only UDS ReadDataByIdentifier (0x22) // in the default session is used: read-only, nothing in the car is changed. Steps (progress kept in NVS "scan"): // 1. unfiltered monitor check, once: is there really no broadcast traffic? // 2. module discovery: 22 F188 (software number) to every address 0x700..0x7EF, reply on address + 8 // 3. DID scan of the modules, most promising first; every positive reply is logged with its data enum class ScanStep : uint8_t { RawCheck, Discover, Dids, Done, }; // Ford: IPMA (camera, speed signs), CCM (radar, ACC), IPC (cluster), PCM; other modules found come after these static const uint16_t SCAN_PRIORITY[] = {0x706, 0x764, 0x720, 0x7E0}; // order in which the DID space is walked: likely Ford live-data ranges first static const uint16_t SCAN_SEGMENTS[][2] = { {0xD000, 0xDFFF}, {0x4000, 0x4FFF}, {0x0000, 0x3FFF}, {0x5000, 0xCFFF}, {0xE000, 0xFFFF}}; static bool scan_loaded = false; // progress read from NVS static ScanStep scan_step = ScanStep::RawCheck; static std::vector scan_modules; // discovered, in scan order static uint32_t scan_module = 0; // index into scan_modules static uint32_t scan_pos = 0; // discovery: address; DID scan: position in the walk (0..65535) static uint32_t scan_found = 0; // positive replies in the current module static uint32_t scan_unsaved = 0; static int scan_silent = 0; // consecutive requests without any reply static uint16_t current_header = 0x7DF; static uint32_t next_scan_obd = 0; static bool count_suffix = false; // adapter understands "1" (return after the first reply) static bool caf_on = true; // CAN auto formatting (ISO-TP), the adapter's default static volatile bool req_scan_reset = false; static bool scan_probed = false; // second module search done (NVS) static uint16_t probe_addr = 0; // 0 = not started on this connection static int probe_new = 0; static int multi_did = 0; // 0 = not tested, 1 = several DIDs per request work, 2 = one at a time static uint16_t multi_module = 0; // module multi_did was tested on static int watch_multi = 0; // watch mode: 0 = not tested (per connection), 1 = 3 per request, 2 = single static uint32_t progress_ms = 0; static String hex3(uint16_t v) { char b[4]; snprintf(b, sizeof(b), "%03X", v); return b; } static String hex4(uint16_t v) { char b[5]; snprintf(b, sizeof(b), "%04X", v); return b; } // Monitoring wants the raw frames (CAF off), requests need CAF on (adds/strips the ISO-TP bytes) static void set_caf(bool on) { if (on != caf_on && elm_command(on ? "ATCAF1" : "ATCAF0", 1500).length()) caf_on = on; } static uint16_t did_at(uint32_t pos) { for (auto &seg : SCAN_SEGMENTS) { uint32_t len = seg[1] - seg[0] + 1; if (pos < len) return seg[0] + pos; pos -= len; } return 0xFFFF; } static void scan_save() { Preferences p; p.begin("scan", false); p.putUChar("step", (uint8_t)scan_step); p.putBytes("mods", scan_modules.data(), scan_modules.size() * sizeof(uint16_t)); p.putUInt("mod", scan_module); p.putUInt("pos", scan_pos); p.putUInt("found", scan_found); p.putBool("probed", scan_probed); p.end(); scan_unsaved = 0; } static void scan_load() { Preferences p; p.begin("scan", true); scan_step = (ScanStep)p.getUChar("step", (uint8_t)ScanStep::RawCheck); scan_modules.resize(p.getBytesLength("mods") / sizeof(uint16_t)); if (scan_modules.size()) p.getBytes("mods", scan_modules.data(), scan_modules.size() * sizeof(uint16_t)); scan_module = p.getUInt("mod", 0); scan_pos = p.getUInt("pos", 0); scan_found = p.getUInt("found", 0); scan_probed = p.getBool("probed", false); p.end(); if (scan_step > ScanStep::Done) scan_step = ScanStep::RawCheck; scan_loaded = true; } static void scan_reset() { scan_step = ScanStep::RawCheck; scan_modules.clear(); scan_module = scan_pos = scan_found = 0; scan_probed = false; scan_save(); can_log_line("E scan reset"); } // Header (request address) + receive filter (reply address = request + 8); 0x7DF = default OBD broadcast static void use_header(uint16_t addr) { if (addr == current_header) return; elm_command("ATSH " + hex3(addr), 1500); elm_command(addr == 0x7DF ? String("ATAR") : "ATCRA " + hex3(addr + 8), 1500); current_header = addr; } // Reply without spaces / multi-frame line numbers: "62D00012", "7F2231", "" = nothing, else adapter text static String clean_reply(const String &reply) { String out; int start = 0; int line_no = 0; while (start <= (int)reply.length()) { int nl = reply.indexOf('\n', start); if (nl < 0) nl = reply.length(); String line = reply.substring(start, nl); start = nl + 1; line.replace(" ", ""); line.trim(); if (!line.length()) continue; if (line.length() > 2 && line[1] == ':') line = line.substring(2); // "0:62D0..." multi-frame part else if (line_no == 0 && line.length() == 3 && reply.indexOf(':') >= 0) continue; // multi-frame byte count "014" out += line; line_no++; } out.toUpperCase(); return out; } static String scan_request(uint16_t addr, const String &req) { use_header(addr); String r = elm_command(count_suffix ? req + "1" : req, 1500); if (r.length() == 0) { note_no_reply(); return "?TIMEOUT"; } fails = 0; r = clean_reply(r); if (r.indexOf("7F" + req.substring(0, 2) + "78") >= 0) { vTaskDelay(pdMS_TO_TICKS(300)); // "response pending": let the late reply pass (next command clears it) } return r; } static bool is_reply(const String &r, const String &req) { return r.startsWith(String("6") + req.substring(1, 2)) || r.startsWith("7F" + req.substring(0, 2)); } // Called once per connection while logging is on static void scan_setup() { if (!scan_loaded) scan_load(); 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); caf_on = true; // adapter was just reset (ATZ) current_header = 0x7DF; elm_command("ATST19", 1500); // reply timeout 100 ms (default 200): modules answer within a few ms String r = elm_command("01001", 1500); count_suffix = r.indexOf("4100") >= 0 || r.indexOf("41 00") >= 0; can_log_line(String("E count suffix ") + (count_suffix ? "yes" : "no") + ", scan step " + (int)scan_step + ", module " + scan_module + ", pos " + scan_pos); next_scan_obd = 0; probe_addr = 0; multi_module = 0; watch_multi = 0; progress_ms = millis(); scan_ready = true; } static void scan_stop() { use_header(0x7DF); elm_command("ATST32", 1500); scan_ready = false; can_log_set_phase(""); } static void scan_raw_check() { can_log_set_phase("Listening (no filter)"); int frames = 0; for (int caf = 0; caf < 2; caf++) { set_caf(caf == 1); elm_command("ATH1", 1500); std::vector lines; bool ok = elm_monitor(5000, 200, lines); elm_command("ATH0", 1500); for (const String &l : lines) { can_log_line(String("F ") + l); if (l.indexOf("NO DATA") < 0) frames++; } if (!ok) return note_no_reply(); } set_caf(true); can_log_line("E raw check: " + String(frames) + " lines"); scan_step = ScanStep::Discover; scan_pos = 0x700; scan_save(); } static void scan_discover_step() { if (scan_pos == 0x700) scan_modules.clear(); if (scan_pos == 0x7DF) // functional (broadcast) address scan_pos++; if (scan_pos > 0x7EF) { bool pcm = std::find(scan_modules.begin(), scan_modules.end(), 0x7E0) != scan_modules.end(); String list; for (uint16_t m : scan_modules) list += " " + hex3(m); can_log_line("I" + list); if (!pcm) { // not even the engine computer answered: ignition off? try again later can_log_line("E discovery found no PCM, again"); scan_pos = 0x700; vTaskDelay(pdMS_TO_TICKS(2000)); return; } // scan order: the promising modules first, then the rest std::vector order; for (uint16_t p : SCAN_PRIORITY) if (std::find(scan_modules.begin(), scan_modules.end(), p) != scan_modules.end()) order.push_back(p); for (uint16_t m : scan_modules) if (std::find(order.begin(), order.end(), m) == order.end()) order.push_back(m); scan_modules = order; scan_step = ScanStep::Dids; scan_module = scan_pos = scan_found = 0; scan_save(); return; } uint16_t addr = scan_pos; can_log_set_phase("Finding modules " + hex3(addr) + ", " + String(scan_modules.size()) + " found"); String r = scan_request(addr, "22F188"); if (r == "?TIMEOUT") return; if (is_reply(r, "22F188")) { scan_modules.push_back(addr); can_log_line("D " + hex3(addr) + " " + r); } else if (r.length() && r != "NODATA") can_log_line("E " + hex3(addr) + " " + r); scan_pos++; } // Second module search (test drive 2 found only the PCM with 22F188 and a 100 ms timeout): TesterPresent (3E00, // single-frame reply) with a 1 s timeout to every address not known yet. Runs once (NVS "probed"). static void scan_probe_step() { if (probe_addr == 0) { probe_addr = 0x700; probe_new = 0; elm_command("ATSTFF", 1500); // ~1 s } while (probe_addr == 0x7DF || std::find(scan_modules.begin(), scan_modules.end(), probe_addr) != scan_modules.end()) probe_addr++; if (probe_addr > 0x7EF) { elm_command("ATST19", 1500); String list; for (uint16_t m : scan_modules) list += " " + hex3(m); can_log_line("I probe, " + String(probe_new) + " new:" + list); if (probe_new) { // promising modules first again; the PCM scan starts over (it had only just begun) std::vector order; for (uint16_t p : SCAN_PRIORITY) if (std::find(scan_modules.begin(), scan_modules.end(), p) != scan_modules.end()) order.push_back(p); for (uint16_t m : scan_modules) if (std::find(order.begin(), order.end(), m) == order.end()) order.push_back(m); if (order[0] != scan_modules[scan_module < scan_modules.size() ? scan_module : 0]) scan_module = scan_pos = scan_found = 0; scan_modules = order; } scan_probed = true; scan_save(); return; } uint16_t addr = probe_addr; can_log_set_phase("Probing modules " + hex3(addr) + " (1 s timeout)"); String r = scan_request(addr, "3E00"); if (r == "?TIMEOUT") return; if (r.startsWith("7E") || r.startsWith("7F3E")) { String id = scan_request(addr, "22F188"); can_log_line("D " + hex3(addr) + " 3E:" + r + " F188:" + id); scan_modules.push_back(addr); probe_new++; } else if (r.length() && r != "NODATA") can_log_line("E " + hex3(addr) + " 3E00 " + r); probe_addr++; } // Does the module answer a request with several DIDs (ISO 14229: only the supported ones are returned)? // F1FF / F1FE are practically never used, F188 always is (the module was found with it). static void scan_test_multi(uint16_t mod) { String r = scan_request(mod, "22F1FFF188F1FE"); multi_did = r.startsWith("62F188") ? 1 : 2; can_log_line("E multi-DID " + hex3(mod) + ": " + (multi_did == 1 ? "yes" : "no") + " (" + r.substring(0, 12) + ")"); multi_module = mod; } // Logs one DID's reply; returns false if the module looks gone (no reply) static bool scan_single(uint16_t mod, uint16_t did) { String r = scan_request(mod, "22" + hex4(did)); if (r.startsWith("62" + hex4(did))) { scan_found++; String data = r.substring(6); can_log_line("S " + hex3(mod) + " " + hex4(did) + " " + data); can_log_set_frame(2, "Last: " + hex3(mod) + " " + hex4(did) + " = " + data.substring(0, 16)); } else if (r.startsWith("7F22")) { String nrc = r.substring(4, 6); if (nrc != "31") // 31 = not supported; others (33 security, 22 conditions...) are worth knowing can_log_line("R " + hex3(mod) + " " + hex4(did) + " " + nrc); } else if (r == "NODATA" || r.length() == 0) return false; else if (r != "?TIMEOUT") can_log_line("E " + hex3(mod) + " " + hex4(did) + " " + r); return true; } static void scan_did_step() { if (scan_module >= scan_modules.size()) { scan_step = ScanStep::Done; scan_save(); can_log_line("E scan done"); return; } uint16_t mod = scan_modules[scan_module]; if (scan_pos >= 0x10000) { can_log_line("E module " + hex3(mod) + " done, " + String(scan_found) + " DIDs"); scan_module++; scan_pos = scan_found = 0; scan_save(); return; } if (multi_module != mod) scan_test_multi(mod); // up to 3 DIDs per request (fits one CAN frame); a hit is then asked one by one int n = multi_did == 1 ? (int)std::min(3, 0x10000 - scan_pos) : 1; uint16_t dids[3]; String req = "22"; for (int i = 0; i < n; i++) { dids[i] = did_at(scan_pos + i); req += hex4(dids[i]); } bool answered = true; if (n == 1) { answered = scan_single(mod, dids[0]); } else { String r = scan_request(mod, req); if (r == "?TIMEOUT") return; if (r == "NODATA" || r.length() == 0) answered = false; else if (r.startsWith("7F2231")) ; // none of them supported else if (r.startsWith("7F2213") || r.startsWith("7F2212")) { can_log_line("E multi-DID refused at " + hex4(dids[0]) + ", single from now on"); multi_did = 2; return; } else for (int i = 0; i < n; i++) scan_single(mod, dids[i]); } if (!answered) { if (++scan_silent >= 20) { // module stopped answering (ignition off / asleep): go back and wait can_log_line("E " + hex3(mod) + " not answering at " + hex4(dids[0])); scan_pos = scan_pos >= 19u * n ? scan_pos - 19 * n : 0; scan_silent = 0; vTaskDelay(pdMS_TO_TICKS(5000)); return; } } else scan_silent = 0; uint32_t before = scan_pos; scan_pos += n; scan_unsaved += n; if (scan_unsaved >= 64) scan_save(); if ((before >> 6) != (scan_pos >> 6)) { char ph[64]; snprintf(ph, sizeof(ph), "Scan %03X (%u/%u) %04X %.1f %%", mod, (unsigned)scan_module + 1, (unsigned)scan_modules.size(), dids[0], scan_pos * 100.0f / 65536); can_log_set_phase(ph); can_log_set_frame(1, "Found in " + hex3(mod) + ": " + String(scan_found)); } if ((before >> 9) != (scan_pos >> 9)) { // progress line every 512 DIDs, with the rate uint32_t now = millis(); can_log_line("E progress " + hex3(mod) + " " + hex4(dids[0]) + " pos " + String(scan_pos) + ", " + String(scan_found) + " found, " + String((now - progress_ms) / 512) + " ms/DID"); progress_ms = now; } } // ---------- watch (after the scan): read the unknown PCM DIDs over and over, log every change ---------- // To find the cruise set speed / speed limit: values that only change when the set speed is changed or a // sign is passed (marks). Up to 3 DIDs per request when the PCM allows it; the reply is split with the // lengths known from the scan. Log: "W " on every change, "E round ..." per round. static std::vector watch_last; // last value per WATCH_DIDS entry static uint32_t watch_index = 0, watch_round = 0, watch_round_start = 0, watch_changes = 0; static void watch_record(int i, const String &data) { if (data == watch_last[i]) return; watch_last[i] = data; watch_changes++; can_log_line("W " + hex4(WATCH_DIDS[i].did) + " " + data); if (watch_round > 0) can_log_set_frame(2, "Changed: " + hex4(WATCH_DIDS[i].did) + " = " + data.substring(0, 16)); } // Reads WATCH_DIDS[i..i+n-1] in one request; false if the reply couldn't be split (then ask one by one) static bool watch_read(int i, int n, bool &answered) { String req = "22"; for (int k = 0; k < n; k++) req += hex4(WATCH_DIDS[i + k].did); String r = scan_request(0x7E0, req); answered = r != "NODATA" && r.length() && r != "?TIMEOUT"; if (!r.startsWith("62")) return n == 1; // single: nothing more to try; several: retry one by one int pos = 2; for (int k = 0; k < n; k++) { int len = WATCH_DIDS[i + k].len * 2; if (r.substring(pos, pos + 4) != hex4(WATCH_DIDS[i + k].did) || (int)r.length() < pos + 4 + len) return false; watch_record(i + k, r.substring(pos + 4, pos + 4 + len)); pos += 4 + len; } return true; } static void watch_step() { if (watch_last.empty()) watch_last.resize(WATCH_COUNT); if (watch_multi == 0) { // three 1-byte DIDs: reply "62D10001D13400D13700" if the PCM handles several per request String r = scan_request(0x7E0, "22D100D134D137"); watch_multi = (r.startsWith("62D100") && r.indexOf("D137") > 0) ? 1 : 2; can_log_line("E watch multi-DID: " + String(watch_multi == 1 ? "yes" : "no") + " (" + r.substring(0, 24) + ")"); watch_round_start = millis(); } if (watch_index >= WATCH_COUNT) { uint32_t ms = millis() - watch_round_start; can_log_line("E round " + String(watch_round) + " done, " + String(ms / 1000) + " s, " + String(watch_changes) + " changes"); can_log_set_frame(1, "Round " + String(watch_round + 1) + ", last " + String(ms / 1000) + " s"); watch_round++; watch_index = 0; watch_changes = 0; watch_round_start = millis(); } int n = watch_multi == 1 ? std::min(3, WATCH_COUNT - watch_index) : 1; bool answered; if (!watch_read(watch_index, n, answered)) for (int k = 0; k < n; k++) watch_read(watch_index + k, 1, answered); if (!answered) { if (++scan_silent >= 20) { scan_silent = 0; can_log_line("E 7E0 not answering (watch)"); vTaskDelay(pdMS_TO_TICKS(5000)); return; } } else scan_silent = 0; watch_index += n; if ((watch_index & 0x1F) < (uint32_t)n) can_log_set_phase("Watching PCM: round " + String(watch_round) + ", " + String(watch_index) + "/" + String(WATCH_COUNT)); } // One unit of scan work (one request), with speed + voltage every 2 s static void log_step() { if (req_scan_reset) { req_scan_reset = false; if (!scan_loaded) scan_load(); scan_reset(); } if (!scan_ready) scan_setup(); if ((int32_t)(millis() - next_scan_obd) >= 0) { next_scan_obd = millis() + LOG_OBD_INTERVAL_MS; use_header(0x7DF); poll_speed(); if ((int32_t)(millis() - next_voltage) >= 0) { next_voltage = millis() + VOLTAGE_INTERVAL_MS; read_voltage(); can_log_line("B " + String(voltage, 1)); } String mods; for (uint16_t m : scan_modules) mods += " " + hex3(m); can_log_set_frame(0, "Modules:" + (mods.length() ? mods : String(" -"))); return; } if (!client->isConnected() || !chr_write) return; switch (scan_step) { case ScanStep::RawCheck: scan_raw_check(); break; case ScanStep::Discover: scan_discover_step(); break; case ScanStep::Dids: if (!scan_probed) scan_probe_step(); else scan_did_step(); break; default: // scan finished: watch the unknown DIDs watch_step(); break; } } // ---------- 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 *) { 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"); can_log_line("E 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); can_log_line("E wake " + String(voltage, 1) + " V"); } } vTaskDelay(pdMS_TO_TICKS(100)); continue; } // logging on: diagnostic scan with speed/voltage in between if (can_log_enabled()) { log_step(); continue; } if (scan_ready) scan_stop(); if ((int32_t)(millis() - next_voltage) >= 0) { next_voltage = millis() + VOLTAGE_INTERVAL_MS; read_voltage(); } // connected: poll the vehicle speed poll_speed(); } } // ---------- 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 &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; } void obd_scan_reset() { req_scan_reset = true; }