espilon-source/espilon_bot/components/mod_canbus/canbus_obd.c

/*
 * canbus_obd.c
 * OBD-II PID decoder with lookup table for ~40 common PIDs.
 *
 * Uses ISO-TP for communication (even single-frame OBD fits in SF,
 * but VIN and DTC responses may require multi-frame).
 */
#include "sdkconfig.h"

#if defined(CONFIG_MODULE_CANBUS) && defined(CONFIG_CANBUS_OBD)

#include <stdio.h>
#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include "canbus_obd.h"
#include "canbus_isotp.h"
#include "canbus_driver.h"
#include "utils.h"

#define TAG "CAN_OBD"

/* ============================================================
 * PID Decoder Table
 * ============================================================ */

typedef float (*decode_fn_t)(const uint8_t *data, int len);

typedef struct {
    uint8_t     pid;
    const char *name;
    const char *unit;
    int         data_bytes;   /* Expected response data bytes (A, AB, ABC...) */
    decode_fn_t decode;
} pid_decoder_t;

/* Decode functions — all check buffer length before access */
static float decode_a(const uint8_t *d, int len)           { if (len < 1) return 0.0f; return (float)d[0]; }
static float decode_a_minus_40(const uint8_t *d, int len)  { if (len < 1) return 0.0f; return (float)d[0] - 40.0f; }
static float decode_a_percent(const uint8_t *d, int len)   { if (len < 1) return 0.0f; return (float)d[0] * 100.0f / 255.0f; }
static float decode_ab(const uint8_t *d, int len)          { if (len < 2) return 0.0f; return (float)((d[0] << 8) | d[1]); }
static float decode_ab_div_4(const uint8_t *d, int len)    { if (len < 2) return 0.0f; return (float)((d[0] << 8) | d[1]) / 4.0f; }
static float decode_a_div_2_m64(const uint8_t *d, int len) { if (len < 1) return 0.0f; return (float)d[0] / 2.0f - 64.0f; }
static float decode_ab_div_100(const uint8_t *d, int len)  { if (len < 2) return 0.0f; return (float)((d[0] << 8) | d[1]) / 100.0f; }
static float decode_a_x3(const uint8_t *d, int len)        { if (len < 1) return 0.0f; return (float)d[0] * 3.0f; }
static float decode_ab_div_20(const uint8_t *d, int len)   { if (len < 2) return 0.0f; return (float)((d[0] << 8) | d[1]) / 20.0f; }
static float decode_signed_a_minus_128(const uint8_t *d, int len) { if (len < 1) return 0.0f; return (float)d[0] - 128.0f; }

static const pid_decoder_t s_pid_table[] = {
    /* PID   Name                          Unit     Bytes  Decoder */
    { 0x04, "Engine Load",                "%",     1, decode_a_percent },
    { 0x05, "Coolant Temp",               "C",     1, decode_a_minus_40 },
    { 0x06, "Short Fuel Trim B1",         "%",     1, decode_signed_a_minus_128 },
    { 0x07, "Long Fuel Trim B1",          "%",     1, decode_signed_a_minus_128 },
    { 0x0B, "Intake MAP",                 "kPa",   1, decode_a },
    { 0x0C, "Engine RPM",                 "rpm",   2, decode_ab_div_4 },
    { 0x0D, "Vehicle Speed",              "km/h",  1, decode_a },
    { 0x0E, "Timing Advance",             "deg",   1, decode_a_div_2_m64 },
    { 0x0F, "Intake Temp",                "C",     1, decode_a_minus_40 },
    { 0x10, "MAF Rate",                   "g/s",   2, decode_ab_div_100 },
    { 0x11, "Throttle Position",          "%",     1, decode_a_percent },
    { 0x1C, "OBD Standard",              "",       1, decode_a },
    { 0x1F, "Engine Runtime",             "s",     2, decode_ab },
    { 0x21, "Distance w/ MIL",            "km",    2, decode_ab },
    { 0x2C, "Commanded EGR",              "%",     1, decode_a_percent },
    { 0x2F, "Fuel Level",                 "%",     1, decode_a_percent },
    { 0x30, "Warmups since DTC clear",    "",      1, decode_a },
    { 0x31, "Distance since DTC clear",   "km",    2, decode_ab },
    { 0x33, "Baro Pressure",              "kPa",   1, decode_a },
    { 0x42, "Control Module Voltage",     "V",     2, decode_ab_div_100 },  /* Approx */
    { 0x45, "Relative Throttle",          "%",     1, decode_a_percent },
    { 0x46, "Ambient Temp",               "C",     1, decode_a_minus_40 },
    { 0x49, "Accelerator Position D",     "%",     1, decode_a_percent },
    { 0x4A, "Accelerator Position E",     "%",     1, decode_a_percent },
    { 0x4C, "Commanded Throttle",         "%",     1, decode_a_percent },
    { 0x5C, "Oil Temp",                   "C",     1, decode_a_minus_40 },
    { 0x5E, "Fuel Rate",                  "L/h",   2, decode_ab_div_20 },
    { 0x67, "Coolant Temp (wide)",        "C",     1, decode_a_minus_40 },  /* First byte only */
    { 0xA6, "Odometer",                   "km",    2, decode_ab },          /* Simplified */
};

#define PID_TABLE_SIZE  (sizeof(s_pid_table) / sizeof(s_pid_table[0]))

/* Find decoder for a PID */
static const pid_decoder_t *find_pid(uint8_t pid)
{
    for (int i = 0; i < (int)PID_TABLE_SIZE; i++) {
        if (s_pid_table[i].pid == pid) return &s_pid_table[i];
    }
    return NULL;
}

/* ============================================================
 * OBD-II Communication (via ISO-TP)
 * ============================================================ */

/* Send OBD request and receive response */
static int obd_transact(uint8_t mode, uint8_t pid,
                        uint8_t *resp, size_t resp_cap, size_t *resp_len)
{
    uint8_t req[2] = { mode, pid };

    /* Use functional broadcast (0x7DF) for Mode 01/03/09 */
    /* Listen on first responder (0x7E8) — most vehicles respond here */
    isotp_status_t st = isotp_request(
        OBD_REQUEST_ID, OBD_RESPONSE_MIN,
        req, 2,
        resp, resp_cap, resp_len,
        2000
    );

    return (st == ISOTP_OK) ? 0 : -1;
}

/* ============================================================
 * Public API
 * ============================================================ */

int obd_query_pid(uint8_t mode, uint8_t pid, obd_result_t *out)
{
    if (!out) return -1;
    memset(out, 0, sizeof(*out));

    uint8_t resp[16];
    size_t resp_len = 0;

    if (obd_transact(mode, pid, resp, sizeof(resp), &resp_len) < 0) {
        return -1;
    }

    /* Response: mode+0x40, PID, data bytes */
    if (resp_len < 2 || resp[0] != (mode + 0x40) || resp[1] != pid) {
        return -1;
    }

    out->pid = pid;

    /* Try to decode with known PID table */
    const pid_decoder_t *dec = find_pid(pid);
    if (dec) {
        out->name = dec->name;
        out->unit = dec->unit;
        int data_offset = 2;  /* After mode+0x40 and PID */
        int data_avail = (int)resp_len - data_offset;
        if (data_avail >= dec->data_bytes) {
            out->value = dec->decode(&resp[data_offset], data_avail);
        }
    } else {
        out->name = "Unknown";
        out->unit = "";
        out->value = (resp_len > 2) ? (float)resp[2] : 0;
    }

    return 0;
}

int obd_query_supported(uint8_t pids_out[], int max_pids)
{
    int total = 0;
    uint8_t resp[16];
    size_t resp_len = 0;

    /* PID 00: supported PIDs 01-20 */
    /* PID 20: supported PIDs 21-40 */
    /* PID 40: supported PIDs 41-60 */
    /* PID 60: supported PIDs 61-80 */

    uint8_t range_pids[] = { 0x00, 0x20, 0x40, 0x60 };

    for (int r = 0; r < 4; r++) {
        if (obd_transact(0x01, range_pids[r], resp, sizeof(resp), &resp_len) < 0) {
            break;
        }

        if (resp_len < 6 || resp[0] != 0x41 || resp[1] != range_pids[r]) {
            break;
        }

        /* 4 bytes = 32 bits, each bit = supported PID */
        uint32_t bitmap = ((uint32_t)resp[2] << 24)
                        | ((uint32_t)resp[3] << 16)
                        | ((uint32_t)resp[4] << 8)
                        | (uint32_t)resp[5];

        for (int bit = 0; bit < 32 && total < max_pids; bit++) {
            if (bitmap & (1U << (31 - bit))) {
                pids_out[total++] = range_pids[r] + bit + 1;
            }
        }

        /* If last PID in range is not supported, no point checking next range */
        if (!(bitmap & 0x01)) break;
    }

    return total;
}

int obd_read_vin(char *vin_out, size_t cap)
{
    if (!vin_out || cap < 18) return -1;

    uint8_t req[2] = { 0x09, 0x02 };  /* Mode 09, PID 02 = VIN */
    uint8_t resp[64];
    size_t resp_len = 0;

    isotp_status_t st = isotp_request(
        OBD_REQUEST_ID, OBD_RESPONSE_MIN,
        req, 2,
        resp, sizeof(resp), &resp_len,
        3000
    );

    if (st != ISOTP_OK) return -1;

    /* Response: 0x49, 0x02, count, VIN (17 ASCII chars) */
    if (resp_len < 20 || resp[0] != 0x49 || resp[1] != 0x02) {
        return -1;
    }

    /* VIN starts at offset 3 (after 0x49, 0x02, count) */
    int vin_start = 3;
    int vin_len = (int)resp_len - vin_start;
    if (vin_len > 17) vin_len = 17;
    if (vin_len > (int)cap - 1) vin_len = (int)cap - 1;

    memcpy(vin_out, &resp[vin_start], vin_len);
    vin_out[vin_len] = '\0';

    return 0;
}

int obd_read_dtcs(char *dtc_buf, size_t cap)
{
    uint8_t req[1] = { 0x03 };  /* Mode 03: Request DTCs */
    uint8_t resp[128];
    size_t resp_len = 0;

    isotp_status_t st = isotp_request(
        OBD_REQUEST_ID, OBD_RESPONSE_MIN,
        req, 1,
        resp, sizeof(resp), &resp_len,
        3000
    );

    if (st != ISOTP_OK || resp_len < 1 || resp[0] != 0x43) {
        snprintf(dtc_buf, cap, "No DTCs or read error");
        return -1;
    }

    int num_dtcs = resp[1];  /* Number of DTCs */
    if (num_dtcs == 0) {
        snprintf(dtc_buf, cap, "No DTCs stored");
        return 0;
    }

    int off = 0;
    off += snprintf(dtc_buf + off, cap - off, "DTCs (%d): ", num_dtcs);

    /* Each DTC is 2 bytes, starting at offset 2 */
    static const char dtc_prefixes[] = { 'P', 'C', 'B', 'U' };

    for (int i = 0; i < num_dtcs && (2 + i * 2 + 1) < (int)resp_len; i++) {
        uint16_t raw = (resp[2 + i * 2] << 8) | resp[2 + i * 2 + 1];

        char prefix = dtc_prefixes[(raw >> 14) & 0x03];
        int code = raw & 0x3FFF;

        off += snprintf(dtc_buf + off, cap - off, "%c%04X ", prefix, code);
        if (off >= (int)cap - 8) break;
    }

    return off;
}

/* ============================================================
 * Continuous Monitoring
 * ============================================================ */

static volatile bool     s_monitor_running = false;
static TaskHandle_t      s_monitor_task    = NULL;
static uint8_t           s_monitor_pids[16];
static int               s_monitor_pid_count = 0;
static int               s_monitor_interval  = 1000;
static const char       *s_monitor_req_id    = NULL;
static SemaphoreHandle_t s_mon_mutex         = NULL;

static void monitor_task(void *arg)
{
    (void)arg;
    ESP_LOGI(TAG, "OBD monitor started: %d PIDs, %d ms interval",
             s_monitor_pid_count, s_monitor_interval);

    while (s_monitor_running) {
        for (int i = 0; i < s_monitor_pid_count && s_monitor_running; i++) {
            obd_result_t result;
            if (obd_query_pid(0x01, s_monitor_pids[i], &result) == 0) {
                char line[96];
                snprintf(line, sizeof(line), "OBD|%s|%.1f|%s",
                         result.name, result.value, result.unit);
                msg_data(TAG, line, strlen(line), false, s_monitor_req_id);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(s_monitor_interval));
    }

    ESP_LOGI(TAG, "OBD monitor stopped");
    s_monitor_task = NULL;
    vTaskDelete(NULL);
}

void obd_monitor_start(const uint8_t *pids, int pid_count,
                       int interval_ms, const char *request_id)
{
    if (!s_mon_mutex) s_mon_mutex = xSemaphoreCreateMutex();
    xSemaphoreTake(s_mon_mutex, portMAX_DELAY);

    if (s_monitor_running) {
        xSemaphoreGive(s_mon_mutex);
        obd_monitor_stop();
        xSemaphoreTake(s_mon_mutex, portMAX_DELAY);
    }

    if (pid_count > 16) pid_count = 16;
    memcpy(s_monitor_pids, pids, pid_count);
    s_monitor_pid_count = pid_count;
    s_monitor_interval = (interval_ms > 0) ? interval_ms : 1000;
    s_monitor_req_id = request_id;
    s_monitor_running = true;

    xTaskCreatePinnedToCore(
        monitor_task, "obd_mon", 4096, NULL, 3, &s_monitor_task, 1
    );

    xSemaphoreGive(s_mon_mutex);
}

void obd_monitor_stop(void)
{
    if (!s_mon_mutex) s_mon_mutex = xSemaphoreCreateMutex();
    xSemaphoreTake(s_mon_mutex, portMAX_DELAY);
    s_monitor_running = false;
    xSemaphoreGive(s_mon_mutex);

    for (int i = 0; i < 20 && s_monitor_task != NULL; i++) {
        vTaskDelay(pdMS_TO_TICKS(50));
    }
}

bool obd_monitor_is_running(void)
{
    return s_monitor_running;
}

#endif /* CONFIG_MODULE_CANBUS && CONFIG_CANBUS_OBD */