#include "obd_link.h" #include #include #include #include "can_log.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 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 // ---------- 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; // 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; 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 (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 } // 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; } // ---------- 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 survey_queue; // ID patterns still to listen to ("3XX", "3DX", "3D9") static std::vector 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 &lines, int per_id, bool *overflow) { std::vector> 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 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 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 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; } } // ---------- 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; } // CAN logging on: monitor windows with speed/voltage in between if (can_log_enabled()) { log_step(); continue; } if (log_mode != LogMode::Off) log_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; }