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] = analogRead(AVREF); 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用の関数にしてしまっても良さそう。
0 件のコメント:
コメントを投稿