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

1182 lines
34 KiB
C++
Raw Normal View History

#include "obd_link.h"
#include <NimBLEDevice.h>
#include <Preferences.h>
#include <algorithm>
#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<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 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<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));
}
}
// "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<uint16_t> 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 "<request>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<String> 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<uint16_t> 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<uint16_t> 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<uint32_t>(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 <did> <data>" on every change, "E round ..." per round.
static std::vector<String> 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<int>(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<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; }
void obd_scan_reset() { req_scan_reset = true; }