LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@O_positive

M5StickC PlusでミニUDSサーバを実行（その４ UDS編）

Posted at

こちらの続きです

前回までで作成した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編 以上

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?