BYD_OBD2_Read.ino

#include "driver/gpio.h"
#include "driver/twai.h"

typedef float (*DataConvertFn)(uint32_t);

typedef struct {
  int8_t mode; // Mode/UDS
  uint16_t pid;
  const char * name;
  uint8_t size; // in byte
  DataConvertFn convertFn;
  const char * unit;
} PIDInfo_t;

PIDInfo_t pidList[] = {
  { .mode = 0x22, .pid = 0x1FFC, .name = "SOC", .size = 2, .convertFn = [](uint32_t raw) { return (float) raw / 100.0f; }, .unit = "%" },
  { .mode = 0x22, .pid = 0x0008, .name = "Battery Voltage", .size = 2, .convertFn = [](uint32_t raw) { return (float) raw; }, .unit = "V" },
  { .mode = 0x22, .pid = 0x0009, .name = "Battery Current", .size = 2, .convertFn = [](uint32_t raw) { return ((float)raw - 5000.0f) / 10.0f; }, .unit = "A" },
  { .mode = 0x22, .pid = 0x0032, .name = "Battery Temperature", .size = 1, .convertFn = [](uint32_t raw) { return (float)(raw) - 40.0f; }, .unit = "°C" },
};

void setup() {
  Serial.begin(115200);

  twai_general_config_t g_config = { // สร้างต้วแปร g_config ใช้กำหนดค่าเกี่ยวกับบัส CAN
    .mode = TWAI_MODE_NORMAL,
    .tx_io = GPIO_NUM_26, // กำหนดขา TX ต่อกับ 26
    .rx_io = GPIO_NUM_27, // กำหนดขา TX ต่อกับ 27
    .clkout_io = ((gpio_num_t) - 1),
    .bus_off_io = ((gpio_num_t) - 1),
    .tx_queue_len = 5,
    .rx_queue_len = 5,
    .alerts_enabled = TWAI_ALERT_NONE,
    .clkout_divider = 0
  };
  twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS();
  // twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
  twai_filter_config_t f_config = {
    .acceptance_code = (uint32_t)(0x7ef) << 21, 
    .acceptance_mask = ~(0x7ff << 21), 
    .single_filter = true
  };
  
  Serial.println("TWAI install driver...");
  if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) {
    Serial.println("Failed to install driver");
    return;
  }
  
  Serial.println("TWAI start...");
  if (twai_start() != ESP_OK) {
    Serial.println("Failed to start driver");
    return;
  }
}

void pollPid(PIDInfo_t item) {
  // Full Requests Packet: 0x7E7(Message ID) 0x03(Data Length) 0x22(UDS/Mode) PID[2]

  twai_message_t message;
  message.identifier = 0x7E7;
  message.flags = TWAI_MSG_FLAG_NONE;  //
  message.data_length_code = 8;
  memset(message.data, 0, 8);
  message.data[0] = 3; // Data Length
  message.data[1] = item.mode; // UDS: ReadDataByIdentifier (16 bit PID)
  message.data[2] = (item.pid >> 8) & 0xFF;
  message.data[3] = (item.pid >> 0) & 0xFF;
  if (twai_transmit(&message, pdMS_TO_TICKS(1000)) == ESP_OK) {
    String payload = "";
    for (int i=0;i<message.data_length_code;i++) {
      if (i != 0) {
        payload += " ";
      }
      payload += "0x";
      if (message.data[i] < 0x10) {
        payload += "0";
      }
      payload += String(message.data[i], HEX);
    }

    Serial.printf("Transmit ID 0x%02x: %s\n", message.identifier, payload.c_str());
  } else {
    Serial.println("Failed to queue message for transmission");
  }
}

void loop() {
  {
    static unsigned long timer = 0;
    if ((timer == 0) || ((millis() - timer) >= 2000)) {
      timer = millis();

      static int i = 0;

      pollPid(pidList[i]);

      i++;
      if (i == (sizeof(pidList) / sizeof(PIDInfo_t))) {
        i = 0;
      }
    }
  }

  twai_message_t message;

  if (twai_receive(&message, pdMS_TO_TICKS(100)) != ESP_OK) {
    return;
  }
  
  String payload = "";
  for (int i=0;i<message.data_length_code;i++) {
    if (i != 0) {
      payload += " ";
    }
    payload += "0x";
    if (message.data[i] < 0x10) {
      payload += "0";
    }
    payload += String(message.data[i], HEX);
  }
  Serial.printf("Received ID 0x%02x: %s\n", message.identifier, payload.c_str());

  // Find in PID List
  for (PIDInfo_t item : pidList) {
    if (message.identifier == 0x7EF) { // if is response Message ID
      if ((message.data[1] & 0xBF) == item.mode) { // if reply of UDS: ReadDataByIdentifier (0x22)
        uint16_t pid = (message.data[2] << 8) | message.data[3];
        if (pid == item.pid) { // SOC
          uint32_t raw = 0;
          if (item.size == 1) {
            raw = message.data[4];
          } else if (item.size == 2) {
            raw = (message.data[5] << 8) | message.data[4];
          } else if (item.size == 3) {
            raw = (message.data[6] << 16) | (message.data[5] << 8) | message.data[4];
          } else if (item.size == 4) {
            raw = (message.data[7] << 24) | (message.data[6] << 16) | (message.data[5] << 8) | message.data[4];
          }
          float value = item.convertFn(raw);

          Serial.printf("%s= %.1f%s\n", item.name, value, item.unit);
        }
      }
    }
  }
}