- After the DID scan, the log page reads the 327 PCM DIDs of unknown meaning
round after round and logs every change ("W did data"), 3 per request if
the PCM allows it (reply split with the lengths from the scan)
- tools/make_watch_dids.py generates src/watch_dids.h from the scan logs
- README/CLAUDE.md: scan results, test-drive plan, git remote
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
1183 lines
34 KiB
C++
1183 lines
34 KiB
C++
#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; }
|