Add console CAN controller node

Console_Node/Console_Node.ino

@@ -0,0 +1,514 @@
+//
+// Console_Node
+// Chaegeon-CAN center-console I/O node for Arduino Mega.
+//
+#include <SPI.h>
+#include <mcp_can.h>
+
+// ============================================================
+// CAN
+// ============================================================
+const byte CAN_CS_PIN = 53;
+const byte CAN_INT_PIN = 2;
+const unsigned long CAN_ID_MAIN_HEARTBEAT = 0x100UL;
+const unsigned long CAN_ID_CONSOLE_COMMAND = 0x101UL;
+const unsigned long CAN_ID_CONSOLE_STATUS = 0x200UL;
+const unsigned long MAIN_HEARTBEAT_TIMEOUT_MS = 2000UL;
+const unsigned long STATUS_INTERVAL_MS = 100UL;
+
+// ============================================================
+// I/O
+// ============================================================
+const byte CHANNEL_COUNT = 7;
+const byte CH_DRIVER_VENT = 0;
+const byte CH_DRIVER_HEAT = 1;
+const byte CH_PASSENGER_VENT = 2;
+const byte CH_PASSENGER_HEAT = 3;
+const byte CH_STEERING_HEAT = 4;
+const byte CH_FRONT_SENSOR = 5;
+const byte CH_VESS = 6;
+
+// YYNMOS-8 style low-side MOSFET board. Change these two if the board is inverted.
+const byte OUTPUT_ACTIVE_LEVEL = HIGH;
+const byte OUTPUT_INACTIVE_LEVEL = LOW;
+const byte OUTPUT_PINS[CHANNEL_COUNT] = {
+  22, 23, 24, 25, 26, 27, 28
+};
+
+// Button press detect inputs. PC817 output modules are commonly active-low.
+const bool BUTTON_USE_INTERNAL_PULLUP = true;
+const byte BUTTON_INPUT_PINS[CHANNEL_COUNT] = {
+  30, 31, 32, 33, 34, 35, 36
+};
+const bool BUTTON_ACTIVE_LOW[CHANNEL_COUNT] = {
+  true, true, true, true, true, true, true
+};
+const unsigned long BUTTON_DEBOUNCE_MS = 40UL;
+const unsigned long OWN_PRESS_MASK_MS = 1200UL;
+
+// STATE_LED analog inputs.
+// 0..2   driver vent high/mid/low
+// 3..5   driver heat high/mid/low
+// 6..8   passenger vent high/mid/low
+// 9..11  passenger heat high/mid/low
+// 12     steering heat
+// 13     front sensor
+// 14     VESS
+const byte LED_COUNT = 15;
+const byte LED_PINS[LED_COUNT] = {
+  A0, A1, A2, A3, A4, A5, A6, A7,
+  A8, A9, A10, A11, A12, A13, A14
+};
+
+// 5V ADC reference: 1.8V ~= 368, 3.3V ~= 675.
+// Tune each channel after raw measurements. For inverted PC817 outputs, set false.
+const int LED_ON_THRESHOLD[LED_COUNT] = {
+  300, 300, 300, 300, 300,
+  300, 300, 300, 300, 300,
+  300, 300, 300, 300, 300
+};
+const bool LED_ON_WHEN_ABOVE[LED_COUNT] = {
+  true, true, true, true, true,
+  true, true, true, true, true,
+  true, true, true, true, true
+};
+
+const byte CLIMATE_COUNT = 4;
+const byte TOGGLE_COUNT = 3;
+const byte CLIMATE_LED_BASE[CLIMATE_COUNT] = {
+  0, 3, 6, 9
+};
+const byte TOGGLE_LED_INDEX[TOGGLE_COUNT] = {
+  12, 13, 14
+};
+
+// ============================================================
+// Pulse execution
+// ============================================================
+const byte PULSE_QUEUE_SIZE = 16;
+byte pulseQueueChannel[PULSE_QUEUE_SIZE] = {0};
+unsigned int pulseQueueOnMs[PULSE_QUEUE_SIZE] = {0};
+unsigned int pulseQueueGapMs[PULSE_QUEUE_SIZE] = {0};
+byte pulseQueueHead = 0;
+byte pulseQueueTail = 0;
+
+bool pulseOnPhase = false;
+bool pulseGapPhase = false;
+byte activePulseChannel = CHANNEL_COUNT;
+unsigned int activePulseOnMs = 0;
+unsigned int activePulseGapMs = 0;
+unsigned long pulsePhaseStartTime = 0;
+unsigned long buttonMaskUntil[CHANNEL_COUNT] = {0};
+
+// ============================================================
+// State
+// ============================================================
+MCP_CAN consoleCan(CAN_CS_PIN);
+
+bool canReady = false;
+bool mainAlive = false;
+bool mainIgnitionOn = false;
+bool mainParkingOn = false;
+byte mainHeartbeatSeq = 0;
+byte lastAcceptedCommandSeq = 0;
+bool commandSeqSeen = false;
+bool pulseQueueOverflow = false;
+unsigned long lastMainHeartbeatTime = 0;
+unsigned long lastStatusTime = 0;
+
+bool lastButtonRaw[CHANNEL_COUNT] = {false};
+bool buttonStable[CHANNEL_COUNT] = {false};
+unsigned long buttonChangeTime[CHANNEL_COUNT] = {0};
+
+void setup() {
+  Serial.begin(115200);
+  setupPins();
+  setupCan();
+  Serial.println(F("Console_Node I/O ready. Send h for help."));
+}
+
+void loop() {
+  unsigned long currentTime = millis();
+
+  updateCanReceive(currentTime);
+  updateMainAlive(currentTime);
+  updateButtonInputs(currentTime);
+  updatePulseOutput(currentTime);
+  updateStatusSend(currentTime);
+  handleSerial();
+}
+
+void setupPins() {
+  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
+    pinMode(OUTPUT_PINS[channel], OUTPUT);
+    digitalWrite(OUTPUT_PINS[channel], OUTPUT_INACTIVE_LEVEL);
+  }
+
+  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
+    pinMode(BUTTON_INPUT_PINS[channel], BUTTON_USE_INTERNAL_PULLUP ? INPUT_PULLUP : INPUT);
+    bool pressed = readButtonPressedRaw(channel);
+    lastButtonRaw[channel] = pressed;
+    buttonStable[channel] = pressed;
+    buttonChangeTime[channel] = millis();
+  }
+
+  for (byte led = 0; led < LED_COUNT; led++) {
+    pinMode(LED_PINS[led], INPUT);
+  }
+}
+
+void setupCan() {
+  pinMode(CAN_INT_PIN, INPUT);
+  if (consoleCan.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ) == CAN_OK) {
+    consoleCan.setMode(MCP_NORMAL);
+    canReady = true;
+  }
+}
+
+void updateCanReceive(unsigned long currentTime) {
+  if (!canReady) {
+    return;
+  }
+
+  while (consoleCan.checkReceive() == CAN_MSGAVAIL) {
+    unsigned long rxId = 0;
+    byte len = 0;
+    byte payload[8] = {0};
+    if (consoleCan.readMsgBuf(&rxId, &len, payload) != CAN_OK) {
+      return;
+    }
+
+    if (rxId == CAN_ID_MAIN_HEARTBEAT && len >= 3 && payload[0] == 1) {
+      byte flags = payload[1];
+      mainIgnitionOn = (flags & 0x01) != 0;
+      mainParkingOn = (flags & 0x02) != 0;
+      mainHeartbeatSeq = payload[2];
+      mainAlive = true;
+      lastMainHeartbeatTime = currentTime;
+    } else if (rxId == CAN_ID_CONSOLE_COMMAND && len == 8 && payload[0] == 1) {
+      processCommand(payload, currentTime);
+    }
+  }
+}
+
+void processCommand(byte payload[8], unsigned long currentTime) {
+  byte command = payload[1];
+  byte seq = payload[7];
+
+  if (commandSeqSeen && seq == lastAcceptedCommandSeq) {
+    return;
+  }
+
+  if (command == 1) {
+    byte channel = payload[2];
+    unsigned int onMs = (unsigned int)payload[3] | ((unsigned int)payload[4] << 8);
+    unsigned int gapMs = (unsigned int)payload[5] | ((unsigned int)payload[6] << 8);
+    if (enqueuePulse(channel, onMs, gapMs)) {
+      lastAcceptedCommandSeq = seq;
+      commandSeqSeen = true;
+    }
+  } else if (command == 2) {
+    clearPulseQueue();
+    allOutputsOff();
+    lastAcceptedCommandSeq = seq;
+    commandSeqSeen = true;
+  }
+
+  (void)currentTime;
+}
+
+void updateMainAlive(unsigned long currentTime) {
+  bool aliveNow = lastMainHeartbeatTime != 0 &&
+                  currentTime - lastMainHeartbeatTime <= MAIN_HEARTBEAT_TIMEOUT_MS;
+  if (!aliveNow && mainAlive) {
+    clearPulseQueue();
+    allOutputsOff();
+  }
+
+  mainAlive = aliveNow;
+}
+
+void updateStatusSend(unsigned long currentTime) {
+  if (!canReady || currentTime - lastStatusTime < STATUS_INTERVAL_MS) {
+    return;
+  }
+  lastStatusTime = currentTime;
+
+  byte flags = 0;
+  if (mainAlive) {
+    flags |= 0x01;
+  }
+  if (isPulseBusy()) {
+    flags |= 0x02;
+  }
+  if (pulseQueueOverflow) {
+    flags |= 0x04;
+  }
+  if (mainIgnitionOn) {
+    flags |= 0x08;
+  }
+  if (mainParkingOn) {
+    flags |= 0x10;
+  }
+
+  byte payload[8] = {
+    1,
+    flags,
+    packClimateLevels(),
+    packToggleStates(),
+    packButtonStates(),
+    lastAcceptedCommandSeq,
+    queueLength(),
+    mainHeartbeatSeq
+  };
+
+  consoleCan.sendMsgBuf(CAN_ID_CONSOLE_STATUS, 0, 8, payload);
+}
+
+byte packClimateLevels() {
+  byte packed = 0;
+  for (byte climate = 0; climate < CLIMATE_COUNT; climate++) {
+    packed |= (readClimateLevel(climate) & 0x03) << (climate * 2);
+  }
+  return packed;
+}
+
+byte packToggleStates() {
+  byte packed = 0;
+  for (byte toggle = 0; toggle < TOGGLE_COUNT; toggle++) {
+    if (readLedOn(TOGGLE_LED_INDEX[toggle])) {
+      packed |= (1 << toggle);
+    }
+  }
+  return packed;
+}
+
+byte packButtonStates() {
+  byte packed = 0;
+  unsigned long currentTime = millis();
+  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
+    if (isButtonMasked(channel, currentTime)) {
+      continue;
+    }
+    if (buttonStable[channel]) {
+      packed |= (1 << channel);
+    }
+  }
+  return packed;
+}
+
+void updateButtonInputs(unsigned long currentTime) {
+  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
+    bool rawPressed = readButtonPressedRaw(channel);
+    if (rawPressed != lastButtonRaw[channel]) {
+      lastButtonRaw[channel] = rawPressed;
+      buttonChangeTime[channel] = currentTime;
+    }
+
+    if (currentTime - buttonChangeTime[channel] >= BUTTON_DEBOUNCE_MS) {
+      buttonStable[channel] = rawPressed;
+    }
+  }
+}
+
+bool readButtonPressedRaw(byte channel) {
+  bool high = digitalRead(BUTTON_INPUT_PINS[channel]) == HIGH;
+  if (BUTTON_ACTIVE_LOW[channel]) {
+    return !high;
+  }
+  return high;
+}
+
+bool isButtonMasked(byte channel, unsigned long currentTime) {
+  return (long)(buttonMaskUntil[channel] - currentTime) > 0;
+}
+
+byte readClimateLevel(byte climate) {
+  byte base = CLIMATE_LED_BASE[climate];
+  if (readLedOn(base)) {
+    return 3;
+  }
+  if (readLedOn(base + 1)) {
+    return 2;
+  }
+  if (readLedOn(base + 2)) {
+    return 1;
+  }
+  return 0;
+}
+
+bool readLedOn(byte ledIndex) {
+  int raw = analogRead(LED_PINS[ledIndex]);
+  bool above = raw >= LED_ON_THRESHOLD[ledIndex];
+  return LED_ON_WHEN_ABOVE[ledIndex] ? above : !above;
+}
+
+int readLedRaw(byte ledIndex) {
+  return analogRead(LED_PINS[ledIndex]);
+}
+
+bool enqueuePulse(byte channel, unsigned int onMs, unsigned int gapMs) {
+  byte nextTail = nextQueueIndex(pulseQueueTail);
+  if (channel >= CHANNEL_COUNT || nextTail == pulseQueueHead) {
+    pulseQueueOverflow = true;
+    return false;
+  }
+
+  pulseQueueChannel[pulseQueueTail] = channel;
+  pulseQueueOnMs[pulseQueueTail] = onMs;
+  pulseQueueGapMs[pulseQueueTail] = gapMs;
+  pulseQueueTail = nextTail;
+
+  unsigned long maskUntil = millis() + onMs + gapMs + OWN_PRESS_MASK_MS;
+  if ((long)(maskUntil - buttonMaskUntil[channel]) > 0) {
+    buttonMaskUntil[channel] = maskUntil;
+  }
+
+  return true;
+}
+
+void updatePulseOutput(unsigned long currentTime) {
+  if (!pulseOnPhase && !pulseGapPhase) {
+    startNextPulse(currentTime);
+    return;
+  }
+
+  if (pulseOnPhase && currentTime - pulsePhaseStartTime >= activePulseOnMs) {
+    setOutput(activePulseChannel, false);
+    pulseOnPhase = false;
+    pulseGapPhase = true;
+    pulsePhaseStartTime = currentTime;
+    return;
+  }
+
+  if (pulseGapPhase && currentTime - pulsePhaseStartTime >= activePulseGapMs) {
+    pulseGapPhase = false;
+    activePulseChannel = CHANNEL_COUNT;
+    startNextPulse(currentTime);
+  }
+}
+
+void startNextPulse(unsigned long currentTime) {
+  if (pulseQueueHead == pulseQueueTail) {
+    return;
+  }
+
+  activePulseChannel = pulseQueueChannel[pulseQueueHead];
+  activePulseOnMs = pulseQueueOnMs[pulseQueueHead];
+  activePulseGapMs = pulseQueueGapMs[pulseQueueHead];
+  pulseQueueHead = nextQueueIndex(pulseQueueHead);
+
+  setOutput(activePulseChannel, true);
+  pulseOnPhase = true;
+  pulsePhaseStartTime = currentTime;
+}
+
+void setOutput(byte channel, bool active) {
+  if (channel >= CHANNEL_COUNT) {
+    return;
+  }
+  digitalWrite(OUTPUT_PINS[channel], active ? OUTPUT_ACTIVE_LEVEL : OUTPUT_INACTIVE_LEVEL);
+}
+
+void allOutputsOff() {
+  for (byte channel = 0; channel < CHANNEL_COUNT; channel++) {
+    setOutput(channel, false);
+  }
+  pulseOnPhase = false;
+  pulseGapPhase = false;
+  activePulseChannel = CHANNEL_COUNT;
+}
+
+void clearPulseQueue() {
+  pulseQueueHead = pulseQueueTail;
+}
+
+bool isPulseBusy() {
+  return pulseOnPhase || pulseGapPhase || pulseQueueHead != pulseQueueTail;
+}
+
+byte queueLength() {
+  if (pulseQueueTail >= pulseQueueHead) {
+    return pulseQueueTail - pulseQueueHead;
+  }
+  return PULSE_QUEUE_SIZE - pulseQueueHead + pulseQueueTail;
+}
+
+byte nextQueueIndex(byte index) {
+  index++;
+  if (index >= PULSE_QUEUE_SIZE) {
+    index = 0;
+  }
+  return index;
+}
+
+void handleSerial() {
+  while (Serial.available()) {
+    char command = Serial.read();
+    if (command == '\r' || command == '\n') {
+      continue;
+    }
+
+    if (command == 'h') {
+      printHelp();
+    } else if (command == 's') {
+      printStatus();
+    } else if (command == 'x') {
+      clearPulseQueue();
+      allOutputsOff();
+      Serial.println(F("ACK: outputs off"));
+    } else {
+      Serial.println(F("ERR: h/s/x only"));
+    }
+  }
+}
+
+void printHelp() {
+  Serial.println();
+  Serial.println(F("+------------------------------------------------------+"));
+  Serial.println(F("| Console_Node I/O                                     |"));
+  Serial.println(F("+------------------------------------------------------+"));
+  Serial.println(F("| h = help                                             |"));
+  Serial.println(F("| s = status and raw LED values                         |"));
+  Serial.println(F("| x = stop outputs and clear pulse queue                |"));
+  Serial.println(F("| Memory and automation live in Auto_Seat Nano.         |"));
+  Serial.println(F("+------------------------------------------------------+"));
+}
+
+void printStatus() {
+  Serial.println();
+  Serial.println(F("[CONSOLE_IO_STATUS]"));
+  printNamedBool(F("canReady"), canReady);
+  printNamedBool(F("mainAlive"), mainAlive);
+  printNamedBool(F("mainIgnitionOn"), mainIgnitionOn);
+  printNamedBool(F("mainParkingOn"), mainParkingOn);
+  printNamedBool(F("pulseBusy"), isPulseBusy());
+  printNamedBool(F("pulseQueueOverflow"), pulseQueueOverflow);
+  printNamedInt(F("queueLength"), queueLength());
+  printNamedInt(F("lastAcceptedCommandSeq"), lastAcceptedCommandSeq);
+  printNamedInt(F("driverVentLevel"), readClimateLevel(CH_DRIVER_VENT));
+  printNamedInt(F("driverHeatLevel"), readClimateLevel(CH_DRIVER_HEAT));
+  printNamedInt(F("passengerVentLevel"), readClimateLevel(CH_PASSENGER_VENT));
+  printNamedInt(F("passengerHeatLevel"), readClimateLevel(CH_PASSENGER_HEAT));
+  printNamedBool(F("steeringHeatOn"), readLedOn(12));
+  printNamedBool(F("frontSensorOn"), readLedOn(13));
+  printNamedBool(F("vessOn"), readLedOn(14));
+  for (byte led = 0; led < LED_COUNT; led++) {
+    Serial.print(F("ledRaw"));
+    Serial.print(led);
+    Serial.print(F("="));
+    Serial.println(readLedRaw(led));
+  }
+}
+
+void printNamedBool(const __FlashStringHelper *name, bool value) {
+  Serial.print(name);
+  Serial.print(F("="));
+  Serial.println(value ? F("true") : F("false"));
+}
+
+void printNamedInt(const __FlashStringHelper *name, long value) {
+  Serial.print(name);
+  Serial.print(F("="));
+  Serial.println(value);
+}