こちらの続きです
CAN通信は１フレームで最大８バイトしかデータを送れません。
大きなデータを送受信するために、トランスポート層プロトコルのISO-TPに対応し、データ分割やフロー制御を行えるようにします。
使用するライブラリ
こちらのiso-tpライブラリを使用します。
このライブラリはCAN通信にMCP_CAN_libを使用する前提となっており、私の環境ではそのままでは使えません。
ESP32CANで通信できるよう改造します。
iso-tpライブラリの改造
作成したファイルはこちら
１．元ファイルを取得
iso-tpライブラリのiso-tp.hとiso-tp.cppをコピーして以下のようにファイル名を変更して配置します。
２．依存関係の解消
ベースのiso-tpライブラリは MCP_CAN_lib の使用が前提となっているので、関連する箇所を変更します。
iso-tp-ESP32CAN.h の変更箇所
iso-tp-ESP32CAN.h の変更箇所
// mcp_can.hのインクルードを削除orコメントアウト
- #include <mcp_can.h>
+ // #include <mcp_can.h>
// INT32U型をuint32_tに変更（INT32Uを使用している箇所すべてを変更）
- INT32U tx_id=0;
+ uint32_t tx_id=0;
iso-tp-ESP32CAN.cpp の変更箇所
iso-tp-ESP32CAN.cpp の変更箇所
// インクルードファイルの変更
- #include "iso-tp.h"
- #include <mcp_can.h>
- #include <mcp_can_dfs.h>
- #include <SPI.h>
+ #include "iso-tp-ESP32CAN.h"
// INT32U型をuint32_tに変更（INT32Uを使用している箇所すべてを変更）
- INT32U delta=millis()-wait_cf;
+ uint32_t delta=millis()-wait_cf;
３．CAN送受信関数の変更
CAN送受信関数として、ライブラリ外の関数を紐づけて使用できるように変更します。
送信処理のイメージ
受信処理のイメージ
iso-tp-ESP32CAN.h の変更箇所
iso-tp-ESP32CAN.h の変更箇所
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*));
private:
- MCP_CAN* _bus;
- uint8_t _mcp_int;
- uint8_t can_send(uint32_t id, uint8_t len, uint8_t *data);
- uint8_t can_receive(void);
+ int (*can_tx_func)(uint32_t, uint8_t*, uint8_t);
+ int (*can_rx_func)(uint32_t*, uint8_t*, uint8_t*);
iso-tp-ESP32CAN.cpp の変更箇所
iso-tp-ESP32CAN.cpp の変更箇所
- IsoTp::IsoTp(MCP_CAN* bus, uint8_t mcp_int)
- {
- _mcp_int = mcp_int;
- _bus = bus;
- }
+ IsoTp::IsoTp() {
+ can_tx_funcN = 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;
+ }
// IsoTp::can_send および IsoTp::can_receive を削除orコメントアウト
- uint8_t IsoTp::can_send(uint32_t id, uint8_t len, uint8_t *data)
- {
- }
-
- uint8_t IsoTp::can_receive(void)
- {
- }
４．CAN送受信関数の呼び出し箇所の変更
iso-tp-ESP32CAN.cpp のCAN送信箇所の変更
iso-tp-ESP32CAN.cpp の変更箇所
// 送信箇所（複数個所すべて）
- return can_send(msg->tx_id,8,TxBuf);
+ return can_tx_func(msg->tx_id, TxBuf, 8);
iso-tp-ESP32CAN.cpp のIsoTp::sendの中のCAN受信箇所の変更
iso-tp-ESP32CAN.cpp の変更箇所
// IsoTp::sendの中の受信箇所
- 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)
{
retval=rcv_fc(msg);
memset(rxBuffer,0,sizeof(rxBuffer));
}
}
iso-tp-ESP32CAN.cpp のIsoTp::receiveの中のCAN受信箇所の変更
iso-tp-ESP32CAN.cpp の変更箇所
// IsoTp::receiveの中の受信箇所
- 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)
{
+ memcpy(rxBuffer, data, len); // rxBuffer にデータをコピー
n_pci_type=rxBuffer[0] & 0xF0;
switch (n_pci_type)
{
case N_PCI_FC:
case N_PCI_SF:
case N_PCI_FF:
case N_PCI_CF:
}
memset(rxBuffer,0,sizeof(rxBuffer));
}
}
５．main.cppを記述
main.cppをこちらの「作成したファイル」のように記述します。
ポイント
setup関数の中の以下の記述でIsoTpのCAN送受信関数とESP32CAN用のCAN送受信関数を紐づける
// ISO-TP の初期化（ESP32CAN 用の送受信関数を登録）
isotp.set_can_tx_rx_ESP32CAN(can_tx_ESP32CAN, can_rx_ESP32CAN);
６．送信の確認
ラズパイ側のCANを起動しておかないと送信されません。
ラズパイ側の環境設定は以下をご参照ください。
車両診断通信プロトコル：UDSをラズパイでシミュレート
platformIOのプロジェクトをビルドしてM5StickC Plusにアップロードすると、９バイトのデータ（0x11 0x22 ･･･ 0x99）を１秒間隔で繰り返し送信します。
ラズパイ側でcandumpして確認すると、以下のようにデータ分割して、定められたフロー制御に従って送信されていることが確認できます。
# candump can0
can0 123 [8] 10 09 11 22 33 44 55 66
can0 456 [8] 30 08 20 00 00 00 00 00
can0 123 [8] 21 77 88 99 00 00 00 00
７．受信の確認
ラズパイ側から２２バイトのデータ（0x11 0x01 0x11 0x22 ･･･）を送信し、
ラズパイ側でcandumpして確認すると、以下のように定められたフロー制御に従って受信していることが確認できます。
can0 456 [8] 10 16 11 01 11 22 33 44
can0 123 [8] 30 00 00 00 00 00 00 00
can0 456 [8] 21 55 66 77 88 99 00 11
can0 456 [8] 22 22 33 44 55 66 77 88
can0 456 [8] 23 99 00 00 00 00 00 00
作成したファイル
main.cpp
main.cpp
#include "M5StickCPlus.h"
#include "ESP32CAN.h"
#include "CAN_config.h"
#include "iso-tp-ESP32CAN.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;
// ========================
// 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();
Serial.begin(115200);
initCAN();
// 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 tx_data[] = {0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99};
txMsg.len = sizeof(tx_data);
txMsg.tx_id = 0x123;
txMsg.rx_id = 0x456;
memcpy(txMsg.Buffer,tx_data,sizeof(tx_data));
isotp.send(&txMsg);
// // 受信
rxMsg.tx_id = 0x123;
rxMsg.rx_id = 0x456;
isotp.receive(&rxMsg);
isotp.print_buffer(rxMsg.rx_id, rxMsg.Buffer, rxMsg.len);
delay(1000); // 周期調整
}
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;
}
ISO-TP編 以上