515 lines
14 KiB
Arduino
515 lines
14 KiB
Arduino
//
|
|
// 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);
|
|
}
|