Bluepillは廉価でCANを使えて便利なんだけど、メッセージが増えると受信処理がけっこう大変なことになるのでフィルタを設定してみた。
フィルタの設定方法を探していたらライブラリなしでCANを使えるようなスケッチを公開してくださった方がいたのでこちらに切り替えてみた。CANフィルタの説明もしてくれてるのでわかりやすい。
しかしスケッチが長いのでsetupとloopをカットしてヘッダファイルとしてinoファイルに取り込むことに。
#include "stm32f103-can.h"
#include <EEPROM.h>
#define LED_BUILTIN PC13
#define Protocol_Version 0x01
#define Hw_Version_Major 0x00
#define Hw_Version_Minor 0x00
#define Hw_Version_Variant 0x00
#define Fw_Version_Major 0x00
#define Fw_Version_Minor 0x00
#define Fw_Version_Revision 0x00
#define Fw_Version_Unreleased 0x00
//#define DEBUG_CAN_RCV
#define NODE_ID_DEFAULT 0x005
#define NODE_BC 0x3F
uint16_t nodeID = 0x005;
//HardwareSerial Serial1(PA10, PA9);
typedef union { //IEEE 754 Float
uint8_t uint[8];
float value[2];
} FLOAT_BYTE_UNION;
uint8_t eepromRead(uint8_t reqID) {
uint8_t lastData = 0xFF;
for (uint16_t i = 0; i < EEPROM.length(); i += 2) {
uint8_t dataID = EEPROM.read(i);
uint8_t data = EEPROM.read(i + 1);
//Serial1.println(dataID);
if (dataID == reqID) {
lastData = data;
//Serial1.print("EEPROM Read:");
//Serial1.println(i);
}
if (dataID == 0xFF | dataID == 0x00) {
break;
}
}
//Serial1.print("EEPROM Data:");
//Serial1.println(lastData);
return lastData;
}
void eepromWrite(uint8_t reqID, uint8_t data) {
uint16_t lastID = 0xFFFF;
for (uint16_t i = 0; i < EEPROM.length(); i += 2) {
uint8_t eepromId = EEPROM.read(i);
if (eepromId == 0xFF | eepromId == 0x00) {
lastID = i;
//Serial1.print("EEPROM Write:");
//Serial1.println(i);
break;
}
}
if (lastID == EEPROM.length()) {
lastID = 0;
}
EEPROM.write(lastID, reqID);
EEPROM.write(lastID + 1, data);
}
void toggleLED() {
static bool led;
led = !led;
digitalWrite(LED_BUILTIN, led);
}
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
Serial1.begin(115200);
bool ret = CANInit(CAN_250KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12
//bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9
//bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1
//bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12
//bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9
//bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1
nodeID = eepromRead(1);
if (nodeID >= 0x3F) {
eepromWrite(1, NODE_ID_DEFAULT);
nodeID = NODE_ID_DEFAULT;
}
CAN1->FMR |= 0x1UL;
CAN1->FMR &= 0xFFFFC0FF;
CAN1->FMR |= 0x1C << 8;
CANSetFilter(0, 1, 0, 0, ((nodeID << 5) << 21), 0xFC000004);
CANSetFilter(1, 1, 0, 0, ((NODE_BC << 5) << 21), 0xFC000004);
CAN1->FMR &= ~(0x1UL);
}
void loop() {
if (CANMsgAvail()) {
CAN_msg_t CAN_RX_msg;
CANReceive(&CAN_RX_msg);
uint16_t cmdID = uint16_t(CAN_RX_msg.id & 0x1F);
uint16_t nodeIDrcv = CAN_RX_msg.id >> 5;
CAN_msg_t CAN_TX_msg;
memset(&CAN_TX_msg, 0, sizeof(CAN_msg_t));
CAN_TX_msg.type = DATA_FRAME;
CAN_TX_msg.format = STANDARD_FORMAT;
CAN_TX_msg.id = ((nodeID << 5) | cmdID);
if (nodeIDrcv == nodeID) {
switch (cmdID) {
// case 0x003: //Get Motor Error
// break;
// case 0x004: //Get Encoder Error
// break;
// case 0x005: //Get Sensorless Error
// break;
case 0x006: //Set Axis Node ID
if (CAN_RX_msg.type == DATA_FRAME) {
uint32_t reqNodeID = CAN_RX_msg.data[0] | CAN_RX_msg.data[1] << 8 | CAN_RX_msg.data[2] << 16 | CAN_RX_msg.data[3] << 24;
//Serial1.println(reqNodeID,HEX);
if ((reqNodeID < 0x3F) && (reqNodeID != eepromRead(1))) {
eepromWrite(1, reqNodeID);
//Serial1.println("nodeID update");
}
}
break;
// case 0x009: //Get Encoder Estimates
// break;
case 0x012: //Set Traj Accel Limits
if (CAN_RX_msg.type == DATA_FRAME) {
FLOAT_BYTE_UNION input;
memcpy(input.uint, CAN_RX_msg.data, 8);
//Serial1.println(input.value[0]);
//Serial1.println(input.value[1]);
}
break;
// case 0x014: //Get IQ
// break;
case 0x015: //Get Temperature(FET IEEE 754 Float, Motor IEEE 754 Float)
if (CAN_RX_msg.type == REMOTE_FRAME) {
FLOAT_BYTE_UNION output;
output.value[0] = (1.43 - analogRead(ATEMP) * 3.3 / 4095) / 0.0043 + 25;
output.value[1] = analogRead(ATEMP);
memcpy(CAN_TX_msg.data, output.uint, 8);
CAN_TX_msg.len = 8;
CANSend(&CAN_TX_msg);
}
break;
// case 0x017: //Get Vbus Voltage
// break;
// case 0x01c: //Get ADC Voltage
// break;
default:
break;
}
}
if (CAN_RX_msg.type == REMOTE_FRAME) {
switch (cmdID) {
case 0x000: //Get Version
CAN_TX_msg.data[0] = Protocol_Version;
CAN_TX_msg.data[1] = Hw_Version_Major;
CAN_TX_msg.data[2] = Hw_Version_Minor;
CAN_TX_msg.data[3] = Hw_Version_Variant;
CAN_TX_msg.data[4] = Fw_Version_Major;
CAN_TX_msg.data[5] = Fw_Version_Minor;
CAN_TX_msg.data[6] = Fw_Version_Revision;
CAN_TX_msg.data[7] = Fw_Version_Unreleased;
CAN_TX_msg.len = 8;
CANSend(&CAN_TX_msg);
break;
case 0x001: //Heartbeat Message
CAN_TX_msg.data[1] = nodeID >> 8;
CAN_TX_msg.data[0] = nodeID;
CAN_TX_msg.len = 8;
CANSend(&CAN_TX_msg);
break;
default:
break;
}
}
#ifdef DEBUG_CAN_RCV
if (CAN_RX_msg.format == EXTENDED_FORMAT) {
Serial1.print("Extended ID: 0x");
if (CAN_RX_msg.id < 0x10000000) Serial1.print("0");
if (CAN_RX_msg.id < 0x1000000) Serial1.print("0");
if (CAN_RX_msg.id < 0x100000) Serial1.print("0");
if (CAN_RX_msg.id < 0x10000) Serial1.print("0");
if (CAN_RX_msg.id < 0x1000) Serial1.print("0");
if (CAN_RX_msg.id < 0x100) Serial1.print("0");
if (CAN_RX_msg.id < 0x10) Serial1.print("0");
Serial1.print(CAN_RX_msg.id, HEX);
} else {
Serial1.print("Standard ID: 0x");
if (CAN_RX_msg.id < 0x100) Serial1.print("0");
if (CAN_RX_msg.id < 0x10) Serial1.print("0");
Serial1.print(CAN_RX_msg.id, HEX);
Serial1.print(" ");
}
Serial1.print(" DLC: ");
Serial1.print(CAN_RX_msg.len);
if (CAN_RX_msg.type == DATA_FRAME) {
Serial1.print(" Data: ");
for (int i = 0; i < CAN_RX_msg.len; i++) {
Serial1.print("0x");
Serial1.print(CAN_RX_msg.data[i], HEX);
if (i != (CAN_RX_msg.len - 1)) Serial1.print(" ");
}
Serial1.println();
} else {
Serial1.println(" Data: REMOTE REQUEST FRAME");
}
#endif
toggleLED();
}
}ポートのRemapとかもわかりやすかった。フィルタの設定はbank1にフィルタ、bank2にマスクみたいな感じ？例も上げてくれているので助かった。しかしブロードキャスト対応にするためにフィルタを追加するところで少しハマった。
CANSetFilterの前後でちゃんとレジスタをいじってやらないとだめっぽい。setupに1行だけいれてちゃんと動いていたので気が付かなかった…
ということでこれで特定のノードIDとブロードキャスト以外はフィルタができるように設定できた。
とりあえずこんな感じのスケッチだとデバイスノードID(上位ビット)を設定するだけで、そのデバイス宛のコマンド(下位ビット)を受け取るみたいな感じに使えるっぽい。
FIFO0しか使っていないのでFIFO1を使おうと思ったんだけど、フィルタ設定してちゃんとsFIFOMailBox[1]に入ってきているようだけど、CANReceive関数とかを変更しないと行けないので今回は処理能力がギリギリとかじゃなかったのでFIFO0のみを使ってみた。簡単にやるならCANReceiveをごそっとコピーしてもう一つFIFO1用の関数にしてしまっても良さそう。
ちなみに今回のサンプル、何個か持ってるBluepillの中このスケッチが動かないBluepillがあったんだけど、乗ってるICがGD32F103っていう互換チップだった。よく3Dプリンタの制御ボードではSTM32F103の代わりとして導入されていて、Klipperとかのカスタムファームウェアでも普通にSTM32でビルドしたバイナリが動くらしい。GD32F303っていうICを使った3Dプリンタ制御ボードもあるらしいけど、これもSTM32F103のバイナリが動くっぽい。(どちらもピン配まで一緒のdrop-in compatible)
ちなみに前に使ったeXoCANライブラリはCKS32F103っていうSTM32F103互換ICでも動くっぽいので試してみたらGD32F103でも動作した。もしかしたら303でも動くかも。
互換ICが乗ってるBluepillが増えてきてるようなので注意が必要だな…
