791 lines
19 KiB
C++
791 lines
19 KiB
C++
#include <SPI.h>
|
|
#include "mcp_can.h"
|
|
|
|
const uint8_t CAN_CS_PIN = 8;
|
|
const uint8_t CAN_INT_PIN = 2;
|
|
|
|
MCP_CAN CAN(CAN_CS_PIN);
|
|
|
|
const byte CAN_SPEED = CAN_500KBPS;
|
|
const byte OSC = MCP_8MHZ;
|
|
|
|
// REDUCED: 64 IDs (saves memory)
|
|
const size_t MAX_IDS = 64;
|
|
uint32_t seenIds[MAX_IDS];
|
|
uint16_t seenCounts[MAX_IDS];
|
|
uint16_t changeCounts[MAX_IDS]; // NEW: Track number of changes
|
|
size_t idsUsed = 0;
|
|
|
|
// Store last seen data for change detection
|
|
byte lastData[MAX_IDS][8];
|
|
byte lastDataLen[MAX_IDS];
|
|
|
|
// REDUCED: 16 filter IDs (saves 64 bytes)
|
|
const size_t MAX_FILTER_IDS = 16;
|
|
unsigned long filterIds[MAX_FILTER_IDS];
|
|
size_t filterCount = 0;
|
|
bool filterEnabled = false;
|
|
bool filterReverse = false;
|
|
|
|
// REDUCED: 48 bytes linebuf (saves 16 bytes)
|
|
char lineBuf[48];
|
|
size_t lineLen = 0;
|
|
|
|
bool showTxOutput = true;
|
|
bool showChangeOnly = false;
|
|
uint8_t frameTypeFilter = 0;
|
|
|
|
// ========== Send Task Structure ==========
|
|
struct SendTask {
|
|
unsigned long id;
|
|
unsigned long interval_ms;
|
|
unsigned long last_send_ms;
|
|
byte data[8];
|
|
byte len;
|
|
bool active;
|
|
bool extended;
|
|
};
|
|
|
|
const size_t MAX_SEND_TASKS = 4;
|
|
SendTask sendTasks[MAX_SEND_TASKS];
|
|
bool canSendMode = false;
|
|
|
|
// ========== Functions ==========
|
|
|
|
inline uint32_t packId(unsigned long id, bool extended) {
|
|
return (extended ? 0x80000000UL : 0) | (id & 0x1FFFFFFFUL);
|
|
}
|
|
inline bool isExtendedPacked(uint32_t packed) { return (packed & 0x80000000UL) != 0; }
|
|
inline unsigned long unpackId(uint32_t packed) { return packed & 0x1FFFFFFFUL; }
|
|
|
|
int16_t findSeenIndex(uint32_t packed) {
|
|
for (size_t i = 0; i < idsUsed; i++) {
|
|
if (seenIds[i] == packed) return (int16_t)i;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
// UPDATED: Store OLD data before updating, return change status
|
|
// Returns: 0=new ID, 1=no change, 2=data changed
|
|
uint8_t checkAndUpdateData(uint32_t packed, byte* newData, byte newLen, int16_t &idx, byte* oldData, byte &oldLen) {
|
|
idx = findSeenIndex(packed);
|
|
|
|
if (idx >= 0) {
|
|
if (seenCounts[idx] != 0xFFFF) seenCounts[idx]++;
|
|
|
|
// Store old data
|
|
oldLen = lastDataLen[idx];
|
|
for (byte i = 0; i < oldLen; i++) {
|
|
oldData[i] = lastData[idx][i];
|
|
}
|
|
|
|
// Check if data changed
|
|
bool changed = false;
|
|
if (lastDataLen[idx] != newLen) {
|
|
changed = true;
|
|
} else {
|
|
for (byte i = 0; i < newLen; i++) {
|
|
if (lastData[idx][i] != newData[i]) {
|
|
changed = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// NEW: Increment change counter if data changed
|
|
if (changed && changeCounts[idx] != 0xFFFF) {
|
|
changeCounts[idx]++;
|
|
}
|
|
|
|
// Update stored data
|
|
lastDataLen[idx] = newLen;
|
|
for (byte i = 0; i < newLen; i++) {
|
|
lastData[idx][i] = newData[i];
|
|
}
|
|
|
|
return changed ? 2 : 1;
|
|
}
|
|
|
|
// New ID
|
|
if (idsUsed < MAX_IDS) {
|
|
idx = idsUsed;
|
|
seenIds[idsUsed] = packed;
|
|
seenCounts[idsUsed] = 1;
|
|
changeCounts[idsUsed] = 0; // NEW: Initialize change count to 0
|
|
lastDataLen[idsUsed] = newLen;
|
|
for (byte i = 0; i < newLen; i++) {
|
|
lastData[idsUsed][i] = newData[i];
|
|
}
|
|
idsUsed++;
|
|
return 0; // New ID
|
|
}
|
|
|
|
return 1; // Array full
|
|
}
|
|
|
|
void printSummary() {
|
|
Serial.print(F("\nSum: "));
|
|
Serial.print(idsUsed);
|
|
Serial.println(F(" IDs"));
|
|
for (size_t i = 0; i < idsUsed; i++) {
|
|
bool ext = isExtendedPacked(seenIds[i]);
|
|
unsigned long id = unpackId(seenIds[i]);
|
|
Serial.print(F(" 0x"));
|
|
Serial.print(id, HEX);
|
|
if (ext) Serial.print(F("(E)"));
|
|
Serial.print(F(" n: "));
|
|
Serial.print(seenCounts[i]);
|
|
Serial.print(F(" chg:")); // NEW: Show change count
|
|
Serial.print(changeCounts[i]);
|
|
Serial.print(F(" ["));
|
|
for (byte j = 0; j < lastDataLen[i]; j++) {
|
|
if (lastData[i][j] < 16) Serial.print('0');
|
|
Serial.print(lastData[i][j], HEX);
|
|
if (j < lastDataLen[i] - 1) Serial.print(' ');
|
|
}
|
|
Serial.println(F("]"));
|
|
}
|
|
Serial.println();
|
|
}
|
|
|
|
bool isIdInList(unsigned long id) {
|
|
for (size_t i = 0; i < filterCount; i++) {
|
|
if (filterIds[i] == id) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool shouldDisplayId(unsigned long id) {
|
|
if (! filterEnabled) return true;
|
|
if (filterCount == 0) return false;
|
|
bool inList = isIdInList(id);
|
|
return filterReverse ? !inList : inList;
|
|
}
|
|
|
|
bool shouldDisplayFrameType(bool extended) {
|
|
if (frameTypeFilter == 0) return true;
|
|
if (frameTypeFilter == 1) return extended;
|
|
if (frameTypeFilter == 2) return !extended;
|
|
return true;
|
|
}
|
|
|
|
bool addFilterId(unsigned long id) {
|
|
for (size_t i = 0; i < filterCount; i++) {
|
|
if (filterIds[i] == id) return false;
|
|
}
|
|
if (filterCount >= MAX_FILTER_IDS) return false;
|
|
filterIds[filterCount++] = id;
|
|
return true;
|
|
}
|
|
|
|
void clearFilter() {
|
|
filterCount = 0;
|
|
}
|
|
|
|
void listFilter() {
|
|
Serial.print(F("Filt "));
|
|
Serial.print(filterEnabled ? F("ON") : F("OFF"));
|
|
Serial.print(F(" "));
|
|
Serial.print(filterReverse ? F("REV") : F("NOR"));
|
|
Serial.print(F(" ("));
|
|
Serial.print(filterCount);
|
|
Serial.println(F(")"));
|
|
for (size_t i = 0; i < filterCount; i++) {
|
|
Serial.print(F(" 0x"));
|
|
Serial.println(filterIds[i], HEX);
|
|
}
|
|
if (filterCount == 0 && filterEnabled) Serial.println(F(" (no output)"));
|
|
|
|
Serial.print(F("Frame: "));
|
|
if (frameTypeFilter == 0) Serial.println(F("BOTH"));
|
|
else if (frameTypeFilter == 1) Serial.println(F("EXT"));
|
|
else Serial.println(F("NORM"));
|
|
|
|
Serial.print(F("Change: "));
|
|
Serial.println(showChangeOnly ? F("ON") : F("OFF"));
|
|
}
|
|
|
|
static void trimInPlace(char* s) {
|
|
while (*s == ' ' || *s == '\t') { memmove(s, s+1, strlen(s)); }
|
|
size_t n = strlen(s);
|
|
while (n && (s[n-1] == ' ' || s[n-1] == '\t')) { s[--n] = 0; }
|
|
}
|
|
|
|
static char* nextToken(char* &p) {
|
|
while (*p == ' ' || *p == '\t') p++;
|
|
if (*p == 0) return nullptr;
|
|
char* start = p;
|
|
while (*p && *p != ' ' && *p != '\t') p++;
|
|
if (*p) { *p = 0; p++; }
|
|
return start;
|
|
}
|
|
|
|
bool parseHexToken(const char* tok, unsigned long &out) {
|
|
if (! tok || ! *tok) return false;
|
|
if ((tok[0] == '0') && (tok[1] == 'x' || tok[1] == 'X')) tok += 2;
|
|
char* endp = nullptr;
|
|
unsigned long val = strtoul(tok, &endp, 16);
|
|
if (endp == tok) return false;
|
|
out = val;
|
|
return true;
|
|
}
|
|
|
|
bool parseDecToken(const char* tok, unsigned long &out) {
|
|
if (! tok || !*tok) return false;
|
|
char* endp = nullptr;
|
|
unsigned long val = strtoul(tok, &endp, 10);
|
|
if (endp == tok) return false;
|
|
out = val;
|
|
return true;
|
|
}
|
|
|
|
// ========== Send Task Functions ==========
|
|
|
|
void initSendTasks() {
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
sendTasks[i].active = false;
|
|
}
|
|
}
|
|
|
|
void switchToSendMode() {
|
|
if (! canSendMode) {
|
|
CAN.setMode(MCP_NORMAL);
|
|
canSendMode = true;
|
|
Serial.println(F("Mode: NORM"));
|
|
}
|
|
}
|
|
|
|
void switchToListenMode() {
|
|
if (canSendMode) {
|
|
CAN.setMode(MCP_LISTENONLY);
|
|
canSendMode = false;
|
|
Serial. println(F("Mode: LIST"));
|
|
}
|
|
}
|
|
|
|
int8_t findFreeSendTaskSlot() {
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
if (!sendTasks[i].active) return (int8_t)i;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int8_t findTaskByIdIdx(unsigned long id) {
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
if (sendTasks[i].active && sendTasks[i].id == id) {
|
|
return (int8_t)i;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
bool addSendTask(unsigned long id, bool extended, unsigned long interval_ms, byte* data, byte len) {
|
|
int8_t existingSlot = findTaskByIdIdx(id);
|
|
int8_t slot;
|
|
|
|
if (existingSlot >= 0) {
|
|
slot = existingSlot;
|
|
Serial.print(F("Upd #"));
|
|
Serial.print(slot);
|
|
Serial.print(F(" 0x"));
|
|
Serial.println(id, HEX);
|
|
} else {
|
|
slot = findFreeSendTaskSlot();
|
|
if (slot < 0) {
|
|
Serial.println(F("Full"));
|
|
return false;
|
|
}
|
|
Serial.print(F("Task #"));
|
|
Serial.print(slot);
|
|
Serial.print(F(" 0x"));
|
|
Serial.print(id, HEX);
|
|
}
|
|
|
|
if (len > 8) len = 8;
|
|
|
|
sendTasks[slot].id = id;
|
|
sendTasks[slot].extended = extended;
|
|
sendTasks[slot].interval_ms = interval_ms;
|
|
sendTasks[slot].len = len;
|
|
for (byte i = 0; i < len; i++) {
|
|
sendTasks[slot].data[i] = data[i];
|
|
}
|
|
sendTasks[slot]. last_send_ms = millis();
|
|
sendTasks[slot]. active = true;
|
|
|
|
switchToSendMode();
|
|
|
|
if (extended) Serial.print(F("(E)"));
|
|
Serial.print(F(" @"));
|
|
Serial.print(interval_ms);
|
|
Serial.println(F("ms"));
|
|
|
|
return true;
|
|
}
|
|
|
|
void stopSendTask(size_t index) {
|
|
if (index >= MAX_SEND_TASKS || ! sendTasks[index].active) {
|
|
Serial.println(F("Invalid"));
|
|
return;
|
|
}
|
|
sendTasks[index].active = false;
|
|
Serial.print(F("Stop #"));
|
|
Serial.println(index);
|
|
|
|
bool anyActive = false;
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
if (sendTasks[i].active) {
|
|
anyActive = true;
|
|
break;
|
|
}
|
|
}
|
|
if (! anyActive) switchToListenMode();
|
|
}
|
|
|
|
void stopAllSendTasks() {
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
sendTasks[i].active = false;
|
|
}
|
|
Serial.println(F("Stop all"));
|
|
switchToListenMode();
|
|
}
|
|
|
|
void listSendTasks() {
|
|
Serial.println(F("\n=== Tasks ==="));
|
|
bool anyActive = false;
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
if (sendTasks[i]. active) {
|
|
anyActive = true;
|
|
Serial.print(F("#"));
|
|
Serial.print(i);
|
|
Serial.print(F(" 0x"));
|
|
Serial.print(sendTasks[i].id, HEX);
|
|
if (sendTasks[i].extended) Serial.print(F("(E)"));
|
|
Serial.print(F(" @"));
|
|
Serial.print(sendTasks[i].interval_ms);
|
|
Serial.print(F("ms ["));
|
|
for (byte j = 0; j < sendTasks[i].len; j++) {
|
|
if (sendTasks[i].data[j] < 16) Serial.print('0');
|
|
Serial.print(sendTasks[i].data[j], HEX);
|
|
if (j < sendTasks[i]. len - 1) Serial.print(' ');
|
|
}
|
|
Serial.println(F("]"));
|
|
}
|
|
}
|
|
if (!anyActive) Serial.println(F("<none>"));
|
|
Serial.print(F("TX: "));
|
|
Serial.println(showTxOutput ? F("ON") : F("OFF"));
|
|
Serial.println();
|
|
}
|
|
|
|
void processSendTasks() {
|
|
unsigned long now = millis();
|
|
for (size_t i = 0; i < MAX_SEND_TASKS; i++) {
|
|
if (sendTasks[i]. active) {
|
|
if (now - sendTasks[i].last_send_ms >= sendTasks[i].interval_ms) {
|
|
byte sndStat = sendTasks[i].extended ?
|
|
CAN.sendMsgBuf(sendTasks[i]. id, 1, sendTasks[i].len, sendTasks[i].data) :
|
|
CAN.sendMsgBuf(sendTasks[i]. id, 0, sendTasks[i].len, sendTasks[i].data);
|
|
|
|
if (showTxOutput) {
|
|
if (sndStat == CAN_OK) {
|
|
Serial.print(F("[TX#"));
|
|
Serial.print(i);
|
|
Serial.print(F("] 0x"));
|
|
Serial.println(sendTasks[i].id, HEX);
|
|
} else {
|
|
Serial.print(F("[ERR#"));
|
|
Serial.print(i);
|
|
Serial.print(F(": "));
|
|
Serial.print(sndStat);
|
|
Serial. println(F("]"));
|
|
}
|
|
}
|
|
|
|
sendTasks[i].last_send_ms = now;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool sendSinglePacket(unsigned long id, bool extended, byte* data, byte len) {
|
|
if (! canSendMode) switchToSendMode();
|
|
|
|
byte sndStat = extended ?
|
|
CAN.sendMsgBuf(id, 1, len, data) :
|
|
CAN.sendMsgBuf(id, 0, len, data);
|
|
|
|
if (showTxOutput) {
|
|
if (sndStat == CAN_OK) {
|
|
Serial.print(F("[TX] 0x"));
|
|
Serial.print(id, HEX);
|
|
if (extended) Serial.print(F("(E)"));
|
|
Serial.print(F(" ["));
|
|
for (byte i = 0; i < len; i++) {
|
|
if (data[i] < 16) Serial.print('0');
|
|
Serial.print(data[i], HEX);
|
|
if (i < len - 1) Serial.print(' ');
|
|
}
|
|
Serial.println(F("]"));
|
|
return true;
|
|
} else {
|
|
Serial.print(F("[TX ERR: "));
|
|
Serial.print(sndStat);
|
|
Serial.println(F("]"));
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return (sndStat == CAN_OK);
|
|
}
|
|
|
|
// ========== Help ==========
|
|
|
|
void printHelp() {
|
|
Serial.println(F("\n=== Log ==="));
|
|
Serial.println(F("s summary"));
|
|
Serial.println(F("c clear"));
|
|
Serial.println(F("fe/fd filt on/off"));
|
|
Serial.println(F("fr toggle rev"));
|
|
Serial.println(F("fl list filt"));
|
|
Serial.println(F("fx clear filt"));
|
|
Serial.println(F("f/a <id> set/add"));
|
|
Serial.println(F("fb/fext/fnorm"));
|
|
Serial.println(F("ce/cd change on/off"));
|
|
|
|
Serial.println(F("\n=== Send ==="));
|
|
Serial.println(F("w <id> <data>"));
|
|
Serial.println(F("t <id> <ms> <data>"));
|
|
Serial.println(F("tl/ts/tx"));
|
|
Serial.println(F("txe/txd"));
|
|
Serial.println(F("mn/ml"));
|
|
}
|
|
|
|
// ========== Command Handler ==========
|
|
|
|
void handleCommandLine(char* line) {
|
|
trimInPlace(line);
|
|
if (line[0] == 0) return;
|
|
|
|
Serial.print(F("> "));
|
|
Serial.println(line);
|
|
|
|
char* p = line;
|
|
char* cmd = nextToken(p);
|
|
|
|
if (strcmp(cmd, "s") == 0 || strcmp(cmd, "S") == 0) {
|
|
printSummary();
|
|
return;
|
|
}
|
|
if (strcmp(cmd, "c") == 0 || strcmp(cmd, "C") == 0) {
|
|
idsUsed = 0;
|
|
Serial.println(F("Cleared"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "ce") == 0) {
|
|
showChangeOnly = true;
|
|
Serial.println(F("Change ON"));
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "cd") == 0) {
|
|
showChangeOnly = false;
|
|
Serial.println(F("Change OFF"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "fb") == 0) {
|
|
frameTypeFilter = 0;
|
|
Serial.println(F("Frame: BOTH"));
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "fext") == 0) {
|
|
frameTypeFilter = 1;
|
|
Serial.println(F("Frame: EXT"));
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "fnorm") == 0) {
|
|
frameTypeFilter = 2;
|
|
Serial.println(F("Frame: NORM"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "fe") == 0) {
|
|
filterEnabled = true;
|
|
Serial.println(F("Filter ON"));
|
|
listFilter();
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "fd") == 0) {
|
|
filterEnabled = false;
|
|
Serial.println(F("Filter OFF"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "fr") == 0) {
|
|
filterReverse = ! filterReverse;
|
|
Serial. print(F("Mode: "));
|
|
Serial.println(filterReverse ? F("REV") : F("NORM"));
|
|
listFilter();
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "fl") == 0) {
|
|
listFilter();
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "fx") == 0) {
|
|
clearFilter();
|
|
Serial.println(F("Filt clear"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "f") == 0) {
|
|
char* tok = nextToken(p);
|
|
unsigned long id;
|
|
if (parseHexToken(tok, id)) {
|
|
clearFilter();
|
|
addFilterId(id);
|
|
filterEnabled = true;
|
|
Serial. print(F("Filt: 0x"));
|
|
Serial.println(id, HEX);
|
|
return;
|
|
}
|
|
Serial.println(F("Bad ID"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "a") == 0) {
|
|
char* tok = nextToken(p);
|
|
unsigned long id;
|
|
if (parseHexToken(tok, id)) {
|
|
if (addFilterId(id)) {
|
|
Serial.print(F("Add 0x"));
|
|
Serial.println(id, HEX);
|
|
} else {
|
|
Serial. println(F("Fail"));
|
|
}
|
|
filterEnabled = true;
|
|
return;
|
|
}
|
|
Serial.println(F("Bad ID"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "mn") == 0) {
|
|
switchToSendMode();
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "ml") == 0) {
|
|
stopAllSendTasks();
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "txe") == 0) {
|
|
showTxOutput = true;
|
|
Serial.println(F("TX ON"));
|
|
return;
|
|
}
|
|
if (strcasecmp(cmd, "txd") == 0) {
|
|
showTxOutput = false;
|
|
Serial.println(F("TX OFF"));
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "w") == 0) {
|
|
char* idTok = nextToken(p);
|
|
unsigned long id;
|
|
if (! parseHexToken(idTok, id)) {
|
|
Serial.println(F("Bad ID"));
|
|
return;
|
|
}
|
|
|
|
byte data[8];
|
|
byte len = 0;
|
|
char* dataTok;
|
|
while ((dataTok = nextToken(p)) != nullptr && len < 8) {
|
|
unsigned long byteVal;
|
|
if (parseHexToken(dataTok, byteVal)) {
|
|
data[len++] = (byte)byteVal;
|
|
}
|
|
}
|
|
|
|
if (len == 0) {
|
|
Serial.println(F("No data"));
|
|
return;
|
|
}
|
|
|
|
sendSinglePacket(id, false, data, len);
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "t") == 0) {
|
|
char* idTok = nextToken(p);
|
|
char* intervalTok = nextToken(p);
|
|
|
|
unsigned long id, interval;
|
|
if (! parseHexToken(idTok, id) || !parseDecToken(intervalTok, interval)) {
|
|
Serial.println(F("Bad args"));
|
|
return;
|
|
}
|
|
|
|
byte data[8];
|
|
byte len = 0;
|
|
char* dataTok;
|
|
while ((dataTok = nextToken(p)) != nullptr && len < 8) {
|
|
unsigned long byteVal;
|
|
if (parseHexToken(dataTok, byteVal)) {
|
|
data[len++] = (byte)byteVal;
|
|
}
|
|
}
|
|
|
|
if (len == 0) {
|
|
Serial.println(F("No data"));
|
|
return;
|
|
}
|
|
|
|
addSendTask(id, false, interval, data, len);
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "tl") == 0) {
|
|
listSendTasks();
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "ts") == 0) {
|
|
char* indexTok = nextToken(p);
|
|
unsigned long index;
|
|
if (!parseDecToken(indexTok, index)) {
|
|
Serial.println(F("Bad idx"));
|
|
return;
|
|
}
|
|
stopSendTask((size_t)index);
|
|
return;
|
|
}
|
|
|
|
if (strcasecmp(cmd, "tx") == 0) {
|
|
stopAllSendTasks();
|
|
return;
|
|
}
|
|
|
|
printHelp();
|
|
}
|
|
|
|
// ========== Setup ==========
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(50);
|
|
|
|
pinMode(CAN_INT_PIN, INPUT);
|
|
initSendTasks();
|
|
|
|
if (CAN. begin(MCP_ANY, CAN_SPEED, OSC) != CAN_OK) {
|
|
Serial.println(F("CAN fail"));
|
|
while (1) { delay(1000); }
|
|
}
|
|
|
|
CAN.setMode(MCP_LISTENONLY);
|
|
Serial.println(F("CAN Ready"));
|
|
printHelp();
|
|
listFilter();
|
|
}
|
|
|
|
// ========== Loop ==========
|
|
|
|
void loop() {
|
|
// Serial commands
|
|
while (Serial.available() > 0) {
|
|
char ch = (char)Serial.read();
|
|
if (ch == '\r') {
|
|
if (lineLen > 0) {
|
|
lineBuf[lineLen] = 0;
|
|
handleCommandLine(lineBuf);
|
|
lineLen = 0;
|
|
}
|
|
} else if (ch == '\n') {
|
|
lineBuf[lineLen] = 0;
|
|
handleCommandLine(lineBuf);
|
|
lineLen = 0;
|
|
} else {
|
|
if (lineLen < sizeof(lineBuf) - 1) {
|
|
lineBuf[lineLen++] = ch;
|
|
} else {
|
|
lineBuf[lineLen] = 0;
|
|
handleCommandLine(lineBuf);
|
|
lineLen = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Process send tasks
|
|
processSendTasks();
|
|
|
|
// Receive CAN
|
|
while (CAN_MSGAVAIL == CAN.checkReceive()) {
|
|
unsigned long id = 0;
|
|
byte ext = 0;
|
|
byte len = 0;
|
|
byte buf[8];
|
|
if (CAN.readMsgBuf(&id, &ext, &len, buf) != CAN_OK) continue;
|
|
|
|
bool extended = (ext != 0);
|
|
uint32_t packed = packId(id, extended);
|
|
|
|
// Check and update data, store old data
|
|
byte oldData[8];
|
|
byte oldLen;
|
|
int16_t idx;
|
|
uint8_t changeStatus = checkAndUpdateData(packed, buf, len, idx, oldData, oldLen);
|
|
|
|
// Check if should display based on filters
|
|
bool passesFilters = shouldDisplayId(id) && shouldDisplayFrameType(extended);
|
|
|
|
// If change detection is ON, only show if data changed
|
|
if (showChangeOnly && changeStatus != 2) {
|
|
continue; // Skip if not changed (status 0=new, 1=no change, 2=changed)
|
|
}
|
|
|
|
if (passesFilters) {
|
|
// Show change indicator and old->new
|
|
if (showChangeOnly && changeStatus == 2) {
|
|
Serial.print(F("[CHG] 0x"));
|
|
Serial.print(id, HEX);
|
|
if (extended) Serial.print(F("(E)"));
|
|
Serial.print(F(" ["));
|
|
|
|
// Show OLD data
|
|
for (byte i = 0; i < oldLen; i++) {
|
|
if (oldData[i] < 16) Serial.print('0');
|
|
Serial.print(oldData[i], HEX);
|
|
if (i < oldLen - 1) Serial.print(' ');
|
|
}
|
|
|
|
Serial.print(F("]->["));
|
|
|
|
// Show NEW data
|
|
for (byte i = 0; i < len; i++) {
|
|
if (buf[i] < 16) Serial.print('0');
|
|
Serial.print(buf[i], HEX);
|
|
if (i < len - 1) Serial.print(' ');
|
|
}
|
|
Serial.println(F("]"));
|
|
} else {
|
|
// Normal display
|
|
Serial.print(millis());
|
|
Serial.print(F("ms 0x"));
|
|
Serial.print(id, HEX);
|
|
if (extended) Serial.print(F("(E)"));
|
|
Serial.print(F(" ["));
|
|
for (byte i = 0; i < len; i++) {
|
|
if (buf[i] < 16) Serial.print('0');
|
|
Serial.print(buf[i], HEX);
|
|
if (i < len - 1) Serial.print(' ');
|
|
}
|
|
Serial.println(F("]"));
|
|
}
|
|
}
|
|
}
|
|
}
|