dashboard-node.ino

// =============================================================================
// dashboard-node.ino
// Teensy 4.1 — Integrated CAN2 logger, I2C radio forwarder, and RTD signal
//
// Functions:
//  1. Receives CAN2 frames and forwards them over I2C (Wire2) to radio module
//  2. Logs all received CAN frames to SD card in CRTD format
//  3. On button press: sends Ready-To-Drive CAN signal (5x) and sounds buzzer
//     for exactly 3 seconds
//
// CAN library : ACAN_T4  (https://github.com/pierremolinaro/acan-t4)
// I2C bus     : Wire2
// I2C slave   : ESP32 radio board at address 0x08
//
// Wire format sent over I2C (per CAN frame):
//   Byte 0      : frame flags  bit0=ext, bit1=rtr
//   Bytes 1-4   : CAN ID (big-endian uint32)
//   Byte 5      : DLC  (0-8)
//   Bytes 6-13  : data (DLC bytes valid, rest 0xFF)
//   Bytes 14-17 : timestamp in milliseconds (big-endian uint32)
//   Total       : 18 bytes per frame
//
// CRTD line format:
//   <timestamp_s.us> <bus>R11/<bus>R29 <CAN_ID> <data bytes...>
//   Example: 1.020305 2R11 213 00 00 00 C0 01 00 00
//
// Pin assignments:
//   Button  : PIN_BUTTON  (active LOW, internal pull-up)
//   Buzzer  : PIN_BUZZER  (active HIGH)
// =============================================================================

#include <ACAN_T4.h>
#include <Wire.h>
#include <SD.h>
#include <SPI.h>

// ---------------------------------------------------------------------------
// Configuration
// ---------------------------------------------------------------------------
#define CAN_BITRATE          500000   // 500 kbit/s
#define I2C_BUS              Wire2    // SDA2/SCL2 on Teensy 4.1
#define I2C_CLOCK            100000   // 100 kHz (slow for ESP32)
#define I2C_TIMEOUT          5000     // Timeout to prevent hangs (ms)
#define I2C_SLAVE_ADDR       0x08     // Must match ESP32 slave address
#define PIN_SCL              24       // SCL2

#define HEARTBEAT_MS         2000     // Heartbeat print interval (ms)
#define ESP32_INIT_DELAY     3000     // Delay program start to wait for ESP32 to initialize (ms)

// I2C frame
#define I2C_FRAME_SIZE       18       // bytes per encoded CAN frame (with timestamp)
#define I2C_RETRY_DELAY      100      // how long to wait before sending on i2c again when transmit fails (ms). This prevents the whole node hanging when there are I2C errors. 
// SD / CRTD logging
#define CAN_BUS_NUM          1        // CRTD bus label
#define SD_FLUSH_INTERVAL_MS 200      // How often to flush the SD file

// Ready-To-Drive
#define PKT_DRIVE_ENABLE 0x0C        // Drive Enable command
#define INVERTER_NODE_ID 44          // Inverter S/N: IP000544, CAN ID: 44
#define CAN_ID_DRIVE_ENABLE ((PKT_DRIVE_ENABLE << 8) | INVERTER_NODE_ID)
#define RTD_BURST_COUNT      5        // How many RTD frames to send per button press
#define RTD_BURST_INTERVAL   50       // ms between each RTD frame in the burst

// Button & buzzer
#define PIN_BUTTON           27        // Active LOW with internal pull-up
#define PIN_BUZZER           28        // Active HIGH
#define BUZZER_DURATION_MS   2000     // Buzzer on-time in ms
#define BUTTON_DEBOUNCE_MS   30       // Debounce window

// Safe to drive check
#define BYTE_RELAY_STATUS            1
#define BIT_MPI_SIGNAL_STATUS        4
#define BIT_IS_CHARGING_INPUT_STATUS 7 

// ---------------------------------------------------------------------------
// Globals — SD logging
// ---------------------------------------------------------------------------
File     logFile;
bool     sdLogging    = false;
static uint32_t recordCount  = 0;
static uint32_t lastFlush    = 0;
static uint32_t startMicros  = 0;

// ---------------------------------------------------------------------------
// Globals — RTD / buzzer
// ---------------------------------------------------------------------------
// RTD burst state (non-blocking)
bool     rtdBurstActive   = false;
uint8_t  rtdBurstRemaining = 0;
uint32_t rtdBurstLastSent  = 0;

// Buzzer state (non-blocking)
bool     buzzerActive   = false;
uint32_t buzzerStartMs  = 0;

// Button debounce
bool     lastButtonState   = HIGH;   // pull-up -> idle HIGH
bool  debouncedButtonState = HIGH; // pull-up -> idle HIGH
uint32_t    lastButtonChange  = 0;

// Ready to drive states
bool readyToDrive = false;
bool safeToDrive = false;

// Timers and stats
static uint32_t lastHeartbeat  = 0;
static uint32_t framesReceived = 0;
static uint32_t framesSent     = 0;
static uint32_t i2cErrors      = 0;
static uint32_t lastI2CRetry   = 0;

// ---------------------------------------------------------------------------
// SD helpers
// ---------------------------------------------------------------------------

// Open the next available canlogXXX.crtd file on the SD card.
bool initSDLogging() {
  Serial.print("Initializing SD card... ");
  if (!SD.begin(BUILTIN_SDCARD)) {
    Serial.println("FAILED — check SD card.");
    return false;
  }
  Serial.println("OK.");

  char filename[20];
  for (int i = 0; i < 1000; i++) {
    snprintf(filename, sizeof(filename), "canlog%03d.crtd", i);
    if (!SD.exists(filename)) break;
  }

  logFile = SD.open(filename, FILE_WRITE);
  if (!logFile) {
    Serial.println("Error creating CRTD log file!");
    return false;
  }

  logFile.println("CXX CAN bus baud rate: 500000");
  logFile.flush();

  Serial.print("Logging CAN frames to: ");
  Serial.println(filename);
  return true;
}

// Write one CANMessage to the CRTD log file.
void logFrameCRTD(const CANMessage &msg) {
  if (!sdLogging) return;

  uint32_t elapsed  = micros() - startMicros;
  uint32_t seconds  = elapsed / 1000000UL;
  uint32_t fraction = elapsed % 1000000UL;

  char line[80];
  int  pos = 0;

  // Timestamp
  pos += snprintf(line + pos, sizeof(line) - pos, "%lu.%06lu ", seconds, fraction);

  // Record type
  if (msg.ext) {
    pos += snprintf(line + pos, sizeof(line) - pos, "%dR29", CAN_BUS_NUM);
  } else {
    pos += snprintf(line + pos, sizeof(line) - pos, "%dR11", CAN_BUS_NUM);
  }

  // CAN ID
  pos += snprintf(line + pos, sizeof(line) - pos, " %03lX", msg.id);

  // Data bytes
  for (uint8_t i = 0; i < msg.len; i++) {
    pos += snprintf(line + pos, sizeof(line) - pos, " %02X", msg.data[i]);
  }

  logFile.println(line);
  recordCount++;
}

// ---------------------------------------------------------------------------
// I2C helpers
// ---------------------------------------------------------------------------

void recoverI2CBus() {
  pinMode(PIN_SCL, OUTPUT);
  for (int i = 0; i < 9; i++) {
    digitalWrite(PIN_SCL, HIGH);
    delayMicroseconds(5);
    digitalWrite(PIN_SCL, LOW);
    delayMicroseconds(5);
  }
  pinMode(PIN_SCL, INPUT); // Hand back to Wire library
}

uint8_t encodeCAN(const CANMessage &msg, uint8_t *buf, uint32_t timestamp) {
  buf[0]  = (msg.ext ? 0x01 : 0x00) | (msg.rtr ? 0x02 : 0x00);
  buf[1]  = (msg.id >> 24) & 0xFF;
  buf[2]  = (msg.id >> 16) & 0xFF;
  buf[3]  = (msg.id >>  8) & 0xFF;
  buf[4]  =  msg.id        & 0xFF;
  buf[5]  = msg.len;
  for (uint8_t i = 0; i < 8; i++) {
    buf[6 + i] = (i < msg.len) ? msg.data[i] : 0xFF;
  }
  buf[14] = (timestamp >> 24) & 0xFF;
  buf[15] = (timestamp >> 16) & 0xFF;
  buf[16] = (timestamp >>  8) & 0xFF;
  buf[17] =  timestamp        & 0xFF;
  return I2C_FRAME_SIZE;
}

uint8_t sendFrameI2C(const uint8_t *buf, uint8_t len) {
  I2C_BUS.beginTransmission(I2C_SLAVE_ADDR);
  I2C_BUS.write(buf, len);
  return I2C_BUS.endTransmission();
}

// ---------------------------------------------------------------------------
// RTD helpers
// ---------------------------------------------------------------------------

// Build and send one RTD CAN frame.
void sendRTDFrame() {
  CANMessage msg;
  msg.id = CAN_ID_DRIVE_ENABLE;
  msg.len = 8;
  msg.ext = true;

  msg.data[0] = readyToDrive ? 0x01 : 0x00;  // Drive enable only if ready to drive
  msg.data[1] = 0xFF;
  msg.data[2] = 0xFF;
  msg.data[3] = 0xFF;
  msg.data[4] = 0xFF;
  msg.data[5] = 0xFF;
  msg.data[6] = 0xFF;
  msg.data[7] = 0xFF;

  bool sent = ACAN_T4::can2.tryToSend(msg);

  Serial.print("[RTD] frame sent: ");
  Serial.println(sent ? "OK" : "FAILED (TX queue full)");
}

// Kick off a non-blocking RTD burst + buzzer on button press.
void startRTDBurst(bool rtd) {
  Serial.println("[RTD] Button pressed — starting RTD burst + buzzer.");
  // set new state
  readyToDrive = rtd;
  // Begin RTD burst
  rtdBurstRemaining = RTD_BURST_COUNT;
  rtdBurstLastSent  = millis() - RTD_BURST_INTERVAL; // fire first frame immediately
  rtdBurstActive    = true;

  // Start buzzer
  if (rtd) {
    digitalWrite(PIN_BUZZER, HIGH);
    buzzerStartMs  = millis();
    buzzerActive   = true;
  }
}

// Call every loop iteration to advance the non-blocking RTD burst.
void updateRTDBurst() {
  if (!rtdBurstActive) return;

  if (millis() - rtdBurstLastSent >= (uint32_t)RTD_BURST_INTERVAL) {
    sendRTDFrame();
    rtdBurstLastSent = millis();
    rtdBurstRemaining--;
    if (rtdBurstRemaining == 0) {
      rtdBurstActive = false;
    }
  }
}

// Call every loop iteration to turn the buzzer off after BUZZER_DURATION_MS.
void updateBuzzer() {
  if (!buzzerActive) return;

  if (millis() - buzzerStartMs >= BUZZER_DURATION_MS) {
    digitalWrite(PIN_BUZZER, LOW);
    buzzerActive = false;
    Serial.println("[Buzzer] OFF.");
  }
}

// ---------------------------------------------------------------------------
// Button polling with debounce (non-blocking)
// Triggers only on the falling edge (idle HIGH → pressed LOW).
// ---------------------------------------------------------------------------
void pollButton() {
  bool rawState = digitalRead(PIN_BUTTON);

  // 1. If the switch changed (noise or actual press), reset the timer
  if (rawState != lastButtonState) {
    lastButtonChange = millis();
  }

  // 2. If the state has been stable longer than the debounce window...
  if ((millis() - lastButtonChange) >= BUTTON_DEBOUNCE_MS) {
    
    // 3. And if that stable state is DIFFERENT from our accepted debounced state...
    if (rawState != debouncedButtonState) {
      debouncedButtonState = rawState; // Accept the new stable state

      // 4. Trigger the burst ONLY on the falling edge (HIGH -> LOW)
      if (debouncedButtonState == LOW) {
        if (!rtdBurstActive && !buzzerActive && safeToDrive) {
          startRTDBurst(true);
        }
      }
    }
  }
  
  // Save the raw reading for the next loop's noise check
  lastButtonState = rawState;
}

// Function to handle specific processing for certain CAN messages
void processSpecialMessages(const CANMessage &msg) {
  switch (msg.id) {
    // verify special safety bits
    case 0x60: {
      bool newSafeToDrive = (msg.data[BYTE_RELAY_STATUS] & (1 << BIT_MPI_SIGNAL_STATUS)) && !(msg.data[BYTE_RELAY_STATUS] & (1 << BIT_IS_CHARGING_INPUT_STATUS));
      if (!newSafeToDrive && newSafeToDrive != safeToDrive) {
        startRTDBurst(false);
      }
      safeToDrive = newSafeToDrive;
      break;
    }
    default:
      break;
  }
}

// ---------------------------------------------------------------------------
// Setup
// ---------------------------------------------------------------------------
void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PIN_BUTTON,  INPUT_PULLUP);
  pinMode(PIN_BUZZER,  OUTPUT);
  digitalWrite(PIN_BUZZER, LOW);

  Serial.begin(115200);
  uint32_t t0 = millis();
  // wait for esp to initialize
  while ((millis() - t0 < ESP32_INIT_DELAY)) {
    delay(50);
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }

  Serial.println("\n=== Teensy 4.1  CAN2 → I2C + SD Logger + RTD ===");

  // --- I2C init ---
  recoverI2CBus();
  I2C_BUS.begin();
  I2C_BUS.setClock(I2C_CLOCK);
  I2C_BUS.setTimeout(I2C_TIMEOUT); // 5ms timeout prevents hangs if radio dies
  Serial.print("I2C (Wire2) @ ");
  Serial.print(I2C_CLOCK / 1000);
  Serial.print(" kHz, slave 0x");
  Serial.println(I2C_SLAVE_ADDR, HEX);

  Serial.println("Scanning I2C bus...");
  bool found = false;
  for (byte addr = 1; addr < 127; addr++) {
    I2C_BUS.beginTransmission(addr);
    if (I2C_BUS.endTransmission() == 0) {
      Serial.print("  Device @ 0x");
      Serial.println(addr, HEX);
      found = true;
    }
  }
  if (!found) Serial.println("  No devices found — check wiring!");

  // --- CAN2 init ---
  ACAN_T4_Settings settings(CAN_BITRATE);
  uint32_t err = ACAN_T4::can2.begin(settings);
  if (err == 0) {
    Serial.print("CAN2 OK @ ");
    Serial.print(settings.actualBitRate());
    Serial.println(" bit/s");
  } else {
    Serial.print("CAN2 FAILED, code 0x");
    Serial.println(err, HEX);
    while (1) { delay(300); digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); }
  }

  // --- SD init ---
  startMicros = micros();
  sdLogging   = initSDLogging();

  digitalWrite(LED_BUILTIN, HIGH);
  Serial.println("=== Running ===");
  Serial.println("  Press button (pin " + String(PIN_BUTTON) + ") to send RTD signal + buzzer.");
  Serial.println();
}

// ---------------------------------------------------------------------------
// Main loop
// ---------------------------------------------------------------------------
void loop() {

  // ---- Button / RTD / buzzer (non-blocking) ----
  pollButton();
  updateRTDBurst();
  updateBuzzer();

  // ---- Drain the entire CAN RX FIFO each iteration ----
  CANMessage msg;
  while (ACAN_T4::can2.receive(msg)) {
    framesReceived++;
    // extract data from CAN if needed
    processSpecialMessages(msg);
    // 1. Log to SD card in CRTD format
    logFrameCRTD(msg);

    // 2. Forward over I2C to radio module only if no recent i2c error
    if (millis() - lastI2CRetry > I2C_RETRY_DELAY) {
      uint8_t buf[I2C_FRAME_SIZE];
      encodeCAN(msg, buf, millis());
      uint8_t result = sendFrameI2C(buf, I2C_FRAME_SIZE);

      if (result == 0) {
        framesSent++;
      } else {
        i2cErrors++;
        lastI2CRetry = millis();
        Serial.print("[I2C ERR] code=");
        Serial.print(result);
        Serial.print("  CAN ID=0x");
        Serial.println(msg.id, HEX);
      }
    }
  }

  // Background: Flush SD card periodically
  if (sdLogging && (millis() - lastFlush > SD_FLUSH_INTERVAL_MS)) {
    logFile.flush();
    lastFlush = millis();
  }

  // ---- Heartbeat / stats ----
  if (millis() - lastHeartbeat >= HEARTBEAT_MS) {
    Serial.print("[Heartbeat] CAN RX: ");
    Serial.print(framesReceived);
    Serial.print("  I2C sent: ");
    Serial.print(framesSent);
    Serial.print("  I2C errors: ");
    Serial.print(i2cErrors);
    Serial.print("  SD frames: ");
    Serial.println(recordCount);
    lastHeartbeat = millis();
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  }
}