こちらの続きです

前回までで作成したCAN通信環境上でUDSサーバを実行します。
本当はAUTOSAR的なssas-publicというソフトウェアを使用したかったのですが、難しくて今回は断念。
ミニUDSサーバを作成します。

ミニUDSサーバ

ECU Resetリクエスト（SID: $11）を受信したら、M5StickC Plusをリセットする
Read Data By Identifierリクエスト（SID: $22）を受信したら、以下のDIDに対応した値（M5StickC Plusのセンサーデータ）を返信する

DID	内容
0xF100	加速度センサー（x軸）
0xF101	加速度センサー（y軸）
0xF102	加速度センサー（z軸）
0xF103	温度センサー

実装

main.cppをこちらの「作成したファイル」のように記述します。

概要

動作確認

platformIOのプロジェクトをビルドしてM5StickC Plusにアップロードすると、UDSリクエスト受信を待ちます。

ECU Reset

ラズパイからリセット要求（$11 01）を送ると、M5StickC Plusでリセット実行されます。

# candump can0
  can0  456   [8]  02 11 01 00 00 00 00 00
  can0  123   [8]  02 51 01 00 00 00 00 00

Read Data By Identifier

ラズパイから４つのDIDの読み出し要求（$22 F100 F101 F102 F103）を送ると、以下のように応答します。

# candump can0
  can0  456   [8]  10 09 22 F1 00 F1 01 F1
  can0  123   [8]  30 00 00 00 00 00 00 00
  can0  456   [8]  21 02 F1 03 00 00 00 00
  can0  123   [8]  10 11 62 F1 00 FD A8 F1
  can0  456   [8]  30 08 20 00 00 00 00 00
  can0  123   [8]  21 01 FF E7 F1 02 03 6A
  can0  123   [8]  22 F1 03 12 9E 00 00 00

作成したファイル

main.cpp

main.cpp

#include "M5StickCPlus.h"
#include "ESP32CAN.h"
#include "CAN_config.h"
#include "iso-tp-ESP32CAN.h"
#include "esp_system.h"

// ========================
// GPIO 定義
// ========================
#define TX_GPIO GPIO_NUM_32  // TXピン
#define RX_GPIO GPIO_NUM_33  // RXピン

// ========================
// グローバル変数
// ========================
CAN_device_t CAN_cfg;  // CAN設定構造体
IsoTp isotp;
struct Message_t txMsg, rxMsg;
uint32_t tx_can_id = 0x123;
uint32_t rx_can_id = 0x456;

// ========================
// ミニUDSサーバ処理
// ========================
void processUdsRequest(uint8_t *data, uint8_t length) {
    uint8_t sid = data[0];  // SID を取得

    if (sid == 0x22) {
        Serial.println("0x22の処理");

        float accX, accY, accZ, temp;
        M5.IMU.getAccelData(&accX, &accY, &accZ);
        M5.IMU.getTempData(&temp);
        Serial.printf("X: %.3f, Y: %.3f, Z: %.3f, temp: %.3f\n", accX, accY, accZ, temp);

        uint8_t response[64];
        uint8_t response_length = 0;

        response[response_length++] = 0x62;

        for (int i = 1; i + 1 < length; i += 2) {
            uint16_t did = (data[i] << 8) | data[i + 1];

            response[response_length++] = data[i];
            response[response_length++] = data[i + 1];

            switch (did) {
                case 0xF100: {  // Accel X
                    int16_t val = accX * 1000;  // 例: milli-g 単位で送信
                    response[response_length++] = (val >> 8) & 0xFF;
                    response[response_length++] = val & 0xFF;
                    break;
                }
                case 0xF101: {  // Accel Y
                    int16_t val = accY * 1000;
                    response[response_length++] = (val >> 8) & 0xFF;
                    response[response_length++] = val & 0xFF;
                    break;
                }
                case 0xF102: {  // Accel Z
                    int16_t val = accZ * 1000;
                    response[response_length++] = (val >> 8) & 0xFF;
                    response[response_length++] = val & 0xFF;
                    break;
                }
                case 0xF103: {  // 温度（例：℃ × 100）
                    int16_t val = temp * 100;
                    response[response_length++] = (val >> 8) & 0xFF;
                    response[response_length++] = val & 0xFF;
                    break;
                }
                default:
                    response_length -= 2;  // 未対応DIDのID削除
                    Serial.printf("未対応DID: 0x%04X\n", did);
                    break;
            }
        }

        txMsg.len = response_length;
        memcpy(txMsg.Buffer, response, response_length);
        txMsg.tx_id = tx_can_id;
        txMsg.rx_id = rx_can_id;
        isotp.send(&txMsg);
    }
    else if (sid == 0x11) {
        Serial.println("0x11の処理");
        uint8_t response[] = {0x51, 0x01};
        txMsg.len = sizeof(response);
        memcpy(txMsg.Buffer, response, txMsg.len);
        txMsg.tx_id = tx_can_id;
        txMsg.rx_id = rx_can_id;
        isotp.send(&txMsg);
        esp_restart();
    } 
    else {
        Serial.println("未対応サービスのリクエスト");
    }
}

// ========================
// CAN 初期化関数
// ========================
void initCAN() {
    CAN_cfg.speed = CAN_SPEED_500KBPS;
    CAN_cfg.tx_pin_id = TX_GPIO;
    CAN_cfg.rx_pin_id = RX_GPIO;
    CAN_cfg.rx_queue = xQueueCreate(10, sizeof(CAN_frame_t));  // 受信キュー

    if (ESP32Can.CANInit() == ESP_OK) {
        Serial.println("CAN bus started successfully!");
    } else {
        Serial.println("CAN bus failed to start!");
        while (1);  // 停止
    }
}

// ========================
// CAN 送信関数（ESP32CAN 用）
// ========================
int can_tx_ESP32CAN(uint32_t id, uint8_t *data, uint8_t len) {
    CAN_frame_t txFrame = { 0 };
    txFrame.FIR.B.FF = CAN_frame_std;
    txFrame.MsgID = id;
    txFrame.FIR.B.DLC = (len > 8) ? 8 : len;
    memcpy(txFrame.data.u8, data, txFrame.FIR.B.DLC);

    Serial.print("送信データ: ");
    for (int i = 0; i < txFrame.FIR.B.DLC; i++) {
        Serial.printf("%02X ", txFrame.data.u8[i]);
    }

    if (ESP32Can.CANWriteFrame(&txFrame) == ESP_OK) {
        Serial.println("-> 送信成功");
        return 0;
    } else {
        Serial.println("-> 送信エラー");
        return -1;
    }
}

// ========================
// CAN 受信関数（ESP32CAN 用）
// ========================
int can_rx_ESP32CAN(uint32_t *id, uint8_t *data, uint8_t *len) {
    CAN_frame_t rxFrame = { 0 };

    if (xQueueReceive(CAN_cfg.rx_queue, &rxFrame, pdMS_TO_TICKS(100)) == pdTRUE) {
        *id = rxFrame.MsgID;
        *len = rxFrame.FIR.B.DLC;
        memcpy(data, rxFrame.data.u8, *len);
        return 0;
    }

    return -1;  // 受信失敗
}

// ========================
// Arduino 初期化関数
// ========================
void setup() {
    M5.begin();
    M5.IMU.Init();
    Serial.begin(115200);
    Serial.println("Start");

    initCAN();  // CAN初期化

    // ISO-TP の初期化（ESP32CAN 用の送受信関数を登録）
    isotp.set_can_tx_rx_ESP32CAN(can_tx_ESP32CAN, can_rx_ESP32CAN);
    txMsg.Buffer = (uint8_t *)calloc(MAX_MSGBUF, sizeof(uint8_t));
    rxMsg.Buffer = (uint8_t *)calloc(MAX_MSGBUF, sizeof(uint8_t));
}

// ========================
// メインループ
// ========================
void loop() {
    uint8_t len = 0;
    uint8_t data[8];
    rxMsg.tx_id = tx_can_id;
    rxMsg.rx_id = rx_can_id;
    if (isotp.receive(&rxMsg) == 0) {
        isotp.print_buffer(rxMsg.rx_id, rxMsg.Buffer, rxMsg.len);
        processUdsRequest(rxMsg.Buffer, rxMsg.len);
    }
}

iso-tp-ESP32CAN.h

iso-tp-ESP32CAN.h

#ifndef _ISOTP_H
#define _ISOTP_H

// #include <mcp_can.h>

// #define ISO_TP_DEBUG

typedef enum {
  ISOTP_IDLE = 0,
  ISOTP_SEND,
  ISOTP_SEND_FF,
  ISOTP_SEND_CF,
  ISOTP_WAIT_FIRST_FC,
  ISOTP_WAIT_FC,
  ISOTP_WAIT_DATA,
  ISOTP_FINISHED,
  ISOTP_ERROR
} isotp_states_t;

#define CAN_MAX_DLEN 8  //Not extended CAN

/* N_PCI type values in bits 7-4 of N_PCI bytes */
#define N_PCI_SF  0x00  /* single frame */
#define N_PCI_FF  0x10  /* first frame */
#define N_PCI_CF  0x20  /* consecutive frame */
#define N_PCI_FC  0x30  /* flow control */

#define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */

/* Flow Status given in FC frame */
#define ISOTP_FC_CTS  0   /* clear to send */
#define ISOTP_FC_WT 1     /* wait */
#define ISOTP_FC_OVFLW  2 /* overflow */

/* Timeout values */
#define TIMEOUT_SESSION  500 /* Timeout between successfull send and receive */
#define TIMEOUT_FC       250 /* Timeout between FF and FC or Block CF and FC */
#define TIMEOUT_CF       250 /* Timeout between CFs                          */
#define MAX_FCWAIT_FRAME  10

#define MAX_MSGBUF 128    /* Received Message Buffer. Depends on uC ressources!
                             Should be enough for our needs */
struct Message_t
{
  uint16_t len=0;
  isotp_states_t tp_state=ISOTP_IDLE;
  uint16_t seq_id=1;
  uint8_t fc_status=ISOTP_FC_CTS;
  uint8_t blocksize=0;
  uint8_t min_sep_time=0;
  uint32_t tx_id=0;
  uint32_t rx_id=0;
  uint8_t *Buffer;
};

class IsoTp
{
  public:
    // IsoTp(MCP_CAN* bus, uint8_t mcp_int);
    IsoTp();
    void set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
                                int (*rx_func)(uint32_t*, uint8_t*, uint8_t*));
    uint8_t send(Message_t* msg);
    uint8_t receive(Message_t* msg);
    void    print_buffer(uint32_t id, uint8_t *buffer, uint16_t len);
  private:
    // MCP_CAN* _bus;
    // uint8_t  _mcp_int;
    int (*can_tx_func)(uint32_t, uint8_t*, uint8_t);
    int (*can_rx_func)(uint32_t*, uint8_t*, uint8_t*);
    uint32_t   rxId;
    uint8_t  rxLen;
    uint8_t  rxBuffer[8];
    uint16_t rest;
    uint8_t  fc_wait_frames=0;
    uint32_t   wait_fc=0;
    uint32_t   wait_cf=0;
    uint32_t   wait_session=0;
    // uint8_t  can_send(uint32_t id, uint8_t len, uint8_t *data);
    // uint8_t  can_receive(void);
    uint8_t  send_fc(struct Message_t* msg);
    uint8_t  send_sf(struct Message_t* msg);
    uint8_t  send_ff(struct Message_t* msg);
    uint8_t  send_cf(struct Message_t* msg);
    uint8_t  rcv_sf(struct Message_t* msg);
    uint8_t  rcv_ff(struct Message_t* msg);
    uint8_t  rcv_cf(struct Message_t* msg);
    uint8_t  rcv_fc(struct Message_t* msg);
    void     fc_delay(uint8_t sep_time);
};

#endif

iso-tp-ESP32CAN.cpp

iso-tp-ESP32CAN.cpp

#include "Arduino.h"
// #include "iso-tp.h"
// #include <mcp_can.h>
// #include <mcp_can_dfs.h>
// #include <SPI.h>
#include "iso-tp-ESP32CAN.h"

// IsoTp::IsoTp(MCP_CAN* bus, uint8_t mcp_int)
// {
//   _mcp_int = mcp_int;
//   _bus = bus;
// }

IsoTp::IsoTp() {
    can_tx_func = nullptr;
    can_rx_func = nullptr;
}
 
void IsoTp::set_can_tx_rx_ESP32CAN(int (*tx_func)(uint32_t, uint8_t*, uint8_t),
                                   int (*rx_func)(uint32_t*, uint8_t*, uint8_t*)) {
    can_tx_func = tx_func;
    can_rx_func = rx_func;
}

void IsoTp::print_buffer(uint32_t id, uint8_t *buffer, uint16_t len)
{
  uint16_t i=0;

  Serial.print(F("Buffer: "));
  Serial.print(id,HEX); Serial.print(F(" ["));
  Serial.print(len); Serial.print(F("] "));
  for(i=0;i<len;i++)
  {
    if(buffer[i] < 0x10) Serial.print(F("0"));
    Serial.print(buffer[i],HEX);
    Serial.print(F(" "));
  }
  Serial.println();
}

// uint8_t IsoTp::can_send(uint32_t id, uint8_t len, uint8_t *data)
// {
// #ifdef ISO_TP_DEBUG
//   Serial.println(F("Send CAN RAW Data:"));
//   print_buffer(id, data, len);
// #endif
//   return _bus->sendMsgBuf(id, 0, len, data);
// }

// uint8_t IsoTp::can_receive(void)
// {
//   bool msgReceived;

//   if (_mcp_int)
//     msgReceived = (!digitalRead(_mcp_int));                     // IRQ: if pin is low, read receive buffer
//   else
//     msgReceived = (_bus->checkReceive() == CAN_MSGAVAIL);       // No IRQ: poll receive buffer

//   if (msgReceived)
//   {
//      memset(rxBuffer,0,sizeof(rxBuffer));       // Cleanup Buffer
//      _bus->readMsgBuf(&rxId, &rxLen, rxBuffer); // Read data: buf = data byte(s)
// #ifdef ISO_TP_DEBUG
//      Serial.println(F("Received CAN RAW Data:"));
//      print_buffer(rxId, rxBuffer, rxLen);
// #endif
//     return true;
//   }
//   else return false;
// }

uint8_t IsoTp::send_fc(struct Message_t *msg)
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  // FC message high nibble = 0x3 , low nibble = FC Status
  TxBuf[0]=(N_PCI_FC | msg->fc_status);
  TxBuf[1]=msg->blocksize;
  /* fix wrong separation time values according spec */
  if ((msg->min_sep_time > 0x7F) && ((msg->min_sep_time < 0xF1)
      || (msg->min_sep_time > 0xF9))) msg->min_sep_time = 0x7F;
  TxBuf[2]=msg->min_sep_time;
  // return can_send(msg->tx_id,8,TxBuf);
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_sf(struct Message_t *msg) //Send SF Message
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  // SF message high nibble = 0x0 , low nibble = Length
  TxBuf[0]=(N_PCI_SF | msg->len);
  memcpy(TxBuf+1,msg->Buffer,msg->len);
//  return can_send(msg->tx_id,msg->len+1,TxBuf);// Add PCI length
  // return can_send(msg->tx_id,8,TxBuf);// Always send full frame
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_ff(struct Message_t *msg) // Send FF
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  msg->seq_id=1;

  TxBuf[0]=(N_PCI_FF | ((msg->len&0x0F00) >> 8));
  TxBuf[1]=(msg->len&0x00FF);
  memcpy(TxBuf+2,msg->Buffer,6);             // Skip 2 Bytes PCI
  // return can_send(msg->tx_id,8,TxBuf);       // First Frame has full length
  return can_tx_func(msg->tx_id, TxBuf, 8);
}

uint8_t IsoTp::send_cf(struct Message_t *msg) // Send SF Message
{
  uint8_t TxBuf[8]={0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  uint16_t len=7;

  TxBuf[0]=(N_PCI_CF | (msg->seq_id & 0x0F));
  if(msg->len>7) len=7; else len=msg->len;
  memcpy(TxBuf+1,msg->Buffer,len);         // Skip 1 Byte PCI
  //return can_send(msg->tx_id,len+1,TxBuf); // Last frame is probably shorter
                                           // than 8 -> Signals last CF Frame
  // return can_send(msg->tx_id,8,TxBuf);     // Last frame is probably shorter
  return can_tx_func(msg->tx_id, TxBuf, 8);
                                           // than 8, pad with 00
}

void IsoTp::fc_delay(uint8_t sep_time)
{
  /*
  * 0x00 - 0x7F: 0 - 127ms
  * 0x80 - 0xF0: reserved
  * 0xF1 - 0xF9: 100us - 900us
  * 0xFA - 0xFF: reserved
  * default 0x7F, 127ms
  */
  if(sep_time <= 0x7F)
    delay(sep_time);
  else if ((sep_time >= 0xF1) && (sep_time <= 0xF9))
    delayMicroseconds((sep_time-0xF0)*100);
  else
    delay(0x7F);
}

uint8_t IsoTp::rcv_sf(struct Message_t* msg)
{
  /* get the SF_DL from the N_PCI byte */
  msg->len = rxBuffer[0] & 0x0F;
  /* copy the received data bytes */
  memcpy(msg->Buffer,rxBuffer+1,msg->len); // Skip PCI, SF uses len bytes
  msg->tp_state=ISOTP_FINISHED;

  return 0;
}

uint8_t IsoTp::rcv_ff(struct Message_t* msg)
{
  msg->seq_id=1;

  /* get the FF_DL */
  msg->len = (rxBuffer[0] & 0x0F) << 8;
  msg->len += rxBuffer[1];
  rest=msg->len;

  /* copy the first received data bytes */
  memcpy(msg->Buffer,rxBuffer+2,6); // Skip 2 bytes PCI, FF must have 6 bytes!
  rest-=6; // Restlength

  msg->tp_state = ISOTP_WAIT_DATA;

#ifdef ISO_TP_DEBUG
  Serial.print(F("First frame received with message length: "));
  Serial.println(rest);
  Serial.println(F("Send flow controll."));
  Serial.print(F("ISO-TP state: ")); Serial.println(msg->tp_state);
#endif

  /* send our first FC frame with Target Address*/
  struct Message_t fc;
  fc.tx_id=msg->tx_id;
  fc.fc_status=ISOTP_FC_CTS;
  fc.blocksize=0;
  fc.min_sep_time=0;
  return send_fc(&fc);
}

uint8_t IsoTp::rcv_cf(struct Message_t* msg)
{
  //Handle Timeout
  //If no Frame within 250ms change State to ISOTP_IDLE
  uint32_t delta=millis()-wait_cf;

  if((delta >= TIMEOUT_FC) && msg->seq_id>1)
  {
#ifdef ISO_TP_DEBUG
    Serial.println(F("CF frame timeout during receive wait_cf="));
    Serial.print(wait_cf); Serial.print(F(" delta="));
    Serial.println(delta);
#endif
    msg->tp_state = ISOTP_IDLE;
    return 1;
  }
  wait_cf=millis();

#ifdef ISO_TP_DEBUG
  Serial.print(F("ISO-TP state: ")); Serial.println(msg->tp_state);
  Serial.print(F("CF received with message rest length: "));
  Serial.println(rest);
#endif

  if (msg->tp_state != ISOTP_WAIT_DATA) return 0;

  if ((rxBuffer[0] & 0x0F) != (msg->seq_id & 0x0F))
  {
#ifdef ISO_TP_DEBUG
    Serial.print(F("Got sequence ID: ")); Serial.print(rxBuffer[0] & 0x0F);
    Serial.print(F(" Expected: ")); Serial.println(msg->seq_id & 0x0F);
#endif
    msg->tp_state = ISOTP_IDLE;
    msg->seq_id = 1;
    return 1;
  }

  if(rest<=7) // Last Frame
  {
    memcpy(msg->Buffer+6+7*(msg->seq_id-1),rxBuffer+1,rest);// 6 Bytes in FF +7
    msg->tp_state=ISOTP_FINISHED;                           // per CF skip PCI
#ifdef ISO_TP_DEBUG
    Serial.print(F("Last CF received with seq. ID: "));
    Serial.println(msg->seq_id);
#endif
  }
  else
  {
#ifdef ISO_TP_DEBUG
    Serial.print(F("CF received with seq. ID: "));
    Serial.println(msg->seq_id);
#endif
    memcpy(msg->Buffer+6+7*(msg->seq_id-1),rxBuffer+1,7); // 6 Bytes in FF +7
                                                          // per CF
    rest-=7; // Got another 7 Bytes of Data;
  }

  msg->seq_id++;

  return 0;
}

uint8_t IsoTp::rcv_fc(struct Message_t* msg)
{
  uint8_t retval=0;

  if (msg->tp_state != ISOTP_WAIT_FC && msg->tp_state != ISOTP_WAIT_FIRST_FC)
    return 0;

  /* get communication parameters only from the first FC frame */
  if (msg->tp_state == ISOTP_WAIT_FIRST_FC)
  {
    msg->blocksize = rxBuffer[1];
    msg->min_sep_time = rxBuffer[2];

    /* fix wrong separation time values according spec */
    if ((msg->min_sep_time > 0x7F) && ((msg->min_sep_time < 0xF1)
	|| (msg->min_sep_time > 0xF9))) msg->min_sep_time = 0x7F;
  }

#ifdef ISO_TP_DEBUG
  Serial.print(F("FC frame: FS "));
  Serial.print(rxBuffer[0]&0x0F);
  Serial.print(F(", Blocksize "));
  Serial.print(msg->blocksize);
  Serial.print(F(", Min. separation Time "));
  Serial.println(msg->min_sep_time);
#endif

  switch (rxBuffer[0] & 0x0F)
  {
    case ISOTP_FC_CTS:
                         msg->tp_state = ISOTP_SEND_CF;
                         break;
    case ISOTP_FC_WT:
                         fc_wait_frames++;
			 if(fc_wait_frames >= MAX_FCWAIT_FRAME)
                         {
#ifdef ISO_TP_DEBUG
                           Serial.println(F("FC wait frames exceeded."));
#endif
                           fc_wait_frames=0;
                           msg->tp_state = ISOTP_IDLE;
                           retval=1;
                         }
#ifdef ISO_TP_DEBUG
                         Serial.println(F("Start waiting for next FC"));
#endif
                         break;
    case ISOTP_FC_OVFLW:
#ifdef ISO_TP_DEBUG
                         Serial.println(F("Overflow in receiver side"));
#endif
    default:
                         msg->tp_state = ISOTP_IDLE;
                         retval=1;
  }
  return retval;
}

uint8_t IsoTp::send(Message_t* msg)
{
  uint8_t bs=false;
  uint32_t delta=0;
  uint8_t retval=0;

  msg->tp_state=ISOTP_SEND;

  while(msg->tp_state!=ISOTP_IDLE && msg->tp_state!=ISOTP_ERROR)
  {
    bs=false;

#ifdef ISO_TP_DEBUG
    Serial.print(F("ISO-TP State: ")); Serial.println(msg->tp_state);
    Serial.print(F("Length      : ")); Serial.println(msg->len);
#endif

    switch(msg->tp_state)
    {
      case ISOTP_IDLE         :  break;
      case ISOTP_SEND         :
                                 if(msg->len<=7)
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.println(F("Send SF"));
#endif
                                   retval=send_sf(msg);
                                   msg->tp_state=ISOTP_IDLE;
                                 }
                                 else
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.println(F("Send FF"));
#endif
                                   if(!(retval=send_ff(msg))) // FF complete
                                   {
                                     msg->Buffer+=6;
                                     msg->len-=6;
                                     msg->tp_state=ISOTP_WAIT_FIRST_FC;
                                     fc_wait_frames=0;
                                     wait_fc=millis();
                                   }
                                 }
                                 break;
      case ISOTP_WAIT_FIRST_FC:
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Wait first FC"));
#endif
                                 delta=millis()-wait_fc;
                                 if(delta >= TIMEOUT_FC)
                                 {
#ifdef ISO_TP_DEBUG
                                   Serial.print(F("FC timeout during receive"));
                                   Serial.print(F(" wait_fc="));
                                   Serial.print(wait_fc);
                                   Serial.print(F(" delta="));
                                   Serial.println(delta);
#endif
                                   msg->tp_state = ISOTP_IDLE;
				   retval=1;
                                 }
                                 break;
      case ISOTP_WAIT_FC      :
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Wait FC"));
#endif
                                 break;
      case ISOTP_SEND_CF      :
#ifdef ISO_TP_DEBUG
                                 Serial.println(F("Send CF"));
#endif
                                 while(msg->len>7 && !bs)
                                 {
                                   fc_delay(msg->min_sep_time);
                                   if(!(retval=send_cf(msg)))
                                   {
#ifdef ISO_TP_DEBUG
                                     Serial.print(F("Send Seq "));
                                     Serial.println(msg->seq_id);
#endif
                                     if(msg->blocksize > 0)
                                     {
#ifdef ISO_TP_DEBUG
                                       Serial.print(F("Blocksize trigger "));
                                       Serial.print(msg->seq_id %
                                                    msg->blocksize);
#endif
                                       if(!(msg->seq_id % msg->blocksize))
                                       {
                                         bs=true;
                                         msg->tp_state=ISOTP_WAIT_FC;
#ifdef ISO_TP_DEBUG
                                         Serial.println(F(" yes"));
#endif
                                       }
#ifdef ISO_TP_DEBUG
                                       else Serial.println(F(" no"));
#endif
                                     }
                                     msg->seq_id++;
				     if (msg->blocksize < 16)
                                       msg->seq_id %= 16;
                                     else
                                       msg->seq_id %= msg->blocksize;
                                     msg->Buffer+=7;
                                     msg->len-=7;
#ifdef ISO_TP_DEBUG
                                     Serial.print(F("Length      : "));
                                     Serial.println(msg->len);
#endif
                                   }
                                 }
                                 if(!bs)
                                 {
                                   fc_delay(msg->min_sep_time);
#ifdef ISO_TP_DEBUG
                                   Serial.print(F("Send last Seq "));
                                   Serial.println(msg->seq_id);
#endif
                                   retval=send_cf(msg);
                                   msg->tp_state=ISOTP_IDLE;
                                 }
                                 break;
      default                 :  break;
    }


    if(msg->tp_state==ISOTP_WAIT_FIRST_FC ||
       msg->tp_state==ISOTP_WAIT_FC)
    {
      // if(can_receive())
      uint8_t data[8];
      uint8_t len;
      uint32_t id;
      if (can_rx_func(&id, data, &len) == 0)
      {
#ifdef ISO_TP_DEBUG
        Serial.println(F("Send branch:"));
#endif
        // if(rxId==msg->rx_id)
        if(id==msg->rx_id)
        {
          retval=rcv_fc(msg);
          memset(rxBuffer,0,sizeof(rxBuffer));
#ifdef ISO_TP_DEBUG
          Serial.println(F("rxId OK!"));
#endif
        }
      }
    }
  }

  return retval;
}

uint8_t IsoTp::receive(Message_t* msg)
{
  uint8_t n_pci_type=0;
  uint32_t delta=0;

  wait_session=millis();
#ifdef ISO_TP_DEBUG
  Serial.println(F("Start receive..."));
#endif
  msg->tp_state=ISOTP_IDLE;

  while(msg->tp_state!=ISOTP_FINISHED && msg->tp_state!=ISOTP_ERROR)
  {
    delta=millis()-wait_session;
    if(delta >= TIMEOUT_SESSION)
    {
#ifdef ISO_TP_DEBUG
      Serial.print(F("ISO-TP Session timeout wait_session="));
      Serial.print(wait_session); Serial.print(F(" delta="));
      Serial.println(delta);
#endif
      return 1;
    }

    // if(can_receive())
    uint8_t data[8];
    uint8_t len;
    uint32_t id;
    if (can_rx_func(&id, data, &len) == 0)
    {
      // if(rxId==msg->rx_id)
      if(id==msg->rx_id)
      {
#ifdef ISO_TP_DEBUG
        Serial.println(F("rxId OK!"));
#endif
        memcpy(rxBuffer, data, len); // rxBuffer にデータをコピー
        n_pci_type=rxBuffer[0] & 0xF0;

        switch (n_pci_type)
        {
          case N_PCI_FC:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("FC"));
#endif
                      /* tx path: fc frame */
                      rcv_fc(msg);
                      break;

          case N_PCI_SF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("SF"));
#endif
                      /* rx path: single frame */
                      rcv_sf(msg);
//		      msg->tp_state=ISOTP_FINISHED;
                      break;

          case N_PCI_FF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("FF"));
#endif
                      /* rx path: first frame */
                      rcv_ff(msg);
//		      msg->tp_state=ISOTP_WAIT_DATA;
                      break;
                      break;

          case N_PCI_CF:
#ifdef ISO_TP_DEBUG
                      Serial.println(F("CF"));
#endif
                      /* rx path: consecutive frame */
                      rcv_cf(msg);
                      break;
        }
        memset(rxBuffer,0,sizeof(rxBuffer));
      }
    }
  }
#ifdef ISO_TP_DEBUG
  Serial.println(F("ISO-TP message received:"));
  print_buffer(msg->rx_id, msg->Buffer, msg->len);
#endif

  return 0;
}

UDS編以上