1312 lines
38 KiB
Arduino
1312 lines
38 KiB
Arduino
//
|
|
// Auto_Seat
|
|
// Automatic seat controller with EEPROM settings and optional serial tools.
|
|
//
|
|
#include <EEPROM.h>
|
|
#include <SPI.h>
|
|
#include <mcp_can.h>
|
|
|
|
#define seatRelayD8 8 // HIGH = OFF, LOW = ON (up relay)
|
|
#define seatRelayD9 9 // HIGH = OFF, LOW = ON (down relay)
|
|
|
|
#define parkingPin A0 // PARKING raw analog value, 0~1023
|
|
#define ignitionPin 7 // HIGH = IG1 ON, LOW = IG1 OFF
|
|
|
|
// ============================================================
|
|
// !!! SAFETY LIMIT !!!
|
|
//
|
|
// Physical maximum travel time from bottom to the highest allowed point.
|
|
// User settings saved in EEPROM must stay inside 0..6000ms.
|
|
// ============================================================
|
|
const uint16_t SEAT_HARD_LIMIT_UP_MS = 6000;
|
|
|
|
// Parking raw threshold hysteresis.
|
|
// P turns ON at 950 or higher, and turns OFF at 940 or lower.
|
|
const int PARKING_ON_THRESHOLD = 950;
|
|
const int PARKING_OFF_THRESHOLD = 940;
|
|
|
|
// Down movement is estimated as 1.1x faster than up movement.
|
|
const int DOWN_SPEED_NUM = 11;
|
|
const int DOWN_SPEED_DEN = 10;
|
|
|
|
// Input and relay protection.
|
|
const unsigned long IGNITION_STABILIZE_MS = 1000UL;
|
|
const unsigned long DRIVE_CONFIRM_MS = 80UL;
|
|
const unsigned long PARK_CONFIRM_MS = 250UL;
|
|
const unsigned long RELAY_DEADTIME_MS = 120UL;
|
|
const unsigned long DOWN_EDGE_MARGIN_MS = 300UL;
|
|
|
|
// After 10 ignition OFF events, wait 5 minutes while ignition is OFF,
|
|
// then auto-calibrate: up 7 seconds -> down 6 seconds -> set position to bottom.
|
|
const byte USES_BEFORE_AUTO_CALIBRATION = 10;
|
|
const unsigned long AUTO_CALIBRATION_DELAY_MS = 300000UL;
|
|
const unsigned long AUTO_CALIBRATION_UP_MS = 7000UL;
|
|
const unsigned long AUTO_CALIBRATION_DOWN_MS = 6000UL;
|
|
|
|
// Serial debug output.
|
|
const unsigned long DEBUG_INTERVAL_MS = 1000UL;
|
|
const long MANUAL_STEP_MS = 500;
|
|
|
|
// Chaegeon-CAN heartbeat. MCP2515 module uses an 8MHz crystal.
|
|
const byte CAN_CS_PIN = 10;
|
|
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 CAN_HEARTBEAT_INTERVAL_MS = 250UL;
|
|
|
|
// Center-console node protocol. Nano stores memory and sends pulse commands.
|
|
const unsigned long CONSOLE_CAN_TIMEOUT_MS = 2000UL;
|
|
const unsigned long CONSOLE_COMMAND_HOLD_MS = 2500UL;
|
|
const unsigned long CONSOLE_MANUAL_LEARN_SETTLE_MS = 700UL;
|
|
const unsigned long CONSOLE_STARTUP_DELAY_MS = 3000UL;
|
|
const unsigned long CONSOLE_MEMORY_REFRESH_INTERVAL_MS = 1170000UL; // 19m 30s
|
|
const unsigned long CONSOLE_STEERING_RESTART_INTERVAL_MS = 1170000UL;
|
|
const unsigned int CONSOLE_SHORT_PRESS_MS = 300;
|
|
const unsigned int CONSOLE_SHORT_PRESS_GAP_MS = 450;
|
|
const unsigned int CONSOLE_CLIMATE_LONG_OFF_MS = 2100;
|
|
const unsigned int CONSOLE_CLIMATE_LONG_OFF_GAP_MS = 700;
|
|
const unsigned int CONSOLE_STEERING_RESTART_GAP_MS = 1200;
|
|
|
|
const byte CONSOLE_CHANNEL_COUNT = 7;
|
|
const byte CONSOLE_CLIMATE_COUNT = 4;
|
|
const byte CONSOLE_TOGGLE_COUNT = 3;
|
|
const byte CONSOLE_PULSE_QUEUE_SIZE = 24;
|
|
|
|
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;
|
|
|
|
const byte TG_STEERING_HEAT = 0;
|
|
const byte TG_FRONT_SENSOR = 1;
|
|
const byte TG_VESS = 2;
|
|
|
|
const byte CONSOLE_OPPOSITE_CLIMATE[CONSOLE_CLIMATE_COUNT] = {
|
|
CH_DRIVER_HEAT,
|
|
CH_DRIVER_VENT,
|
|
CH_PASSENGER_HEAT,
|
|
CH_PASSENGER_VENT
|
|
};
|
|
|
|
// EEPROM map.
|
|
const int EEPROM_MAGIC_ADDR = 0;
|
|
const byte EEPROM_MAGIC_VALUE = 0x64;
|
|
const int EEPROM_USE_COUNT_ADDR = 1;
|
|
const int EEPROM_SEAT_POSITION_ADDR = 2; // uint16_t, low byte first
|
|
const int EEPROM_CONSOLE_MAGIC_ADDR = 16;
|
|
const byte EEPROM_CONSOLE_MAGIC_VALUE = 0xC2;
|
|
const int EEPROM_CONSOLE_CLIMATE_TARGET_ADDR = 17; // 4 bytes, driver vent/heat/passenger vent/heat
|
|
const int EEPROM_CONSOLE_TOGGLE_TARGET_ADDR = 21; // 3 bytes, steering/front sensor/VESS
|
|
|
|
const byte CALIBRATION_IDLE = 0;
|
|
const byte CALIBRATION_UP = 1;
|
|
const byte CALIBRATION_DOWN = 2;
|
|
|
|
boolean ignitionState = LOW;
|
|
boolean parkingState = LOW;
|
|
boolean prevIgnitionState = LOW;
|
|
boolean isStabilizing = false;
|
|
boolean confirmedDriveMode = LOW;
|
|
boolean pendingDriveMode = LOW;
|
|
|
|
byte useCount = 0;
|
|
byte autoCalibrationState = CALIBRATION_IDLE;
|
|
bool autoCalibrationPending = false;
|
|
bool forceCalibration = false;
|
|
bool manualMode = false;
|
|
|
|
int parkingRawValue = 0;
|
|
uint16_t storedSeatPositionMs = 0;
|
|
long currentPosMs = 0;
|
|
long targetPosMs = 0;
|
|
long downSpeedRemainder = 0;
|
|
unsigned long downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
bool downEdgeMarginActive = false;
|
|
|
|
unsigned long lastTime = 0;
|
|
unsigned long stabilizeStartTime = 0;
|
|
unsigned long ignitionOffStartTime = 0;
|
|
unsigned long calibrationPhaseStartTime = 0;
|
|
unsigned long pendingDriveModeStartTime = 0;
|
|
unsigned long relayDeadtimeStartTime = 0;
|
|
unsigned long lastDebugPrintTime = 0;
|
|
|
|
int currentSeatAction = 0;
|
|
int requestedSeatAction = 0;
|
|
int lastStoppedSeatAction = 0;
|
|
|
|
bool serialIntroPrinted = false;
|
|
bool debugEnabled = false;
|
|
bool manualStepStatusPending = false;
|
|
|
|
MCP_CAN chaegeonCan(CAN_CS_PIN);
|
|
bool chaegeonCanReady = false;
|
|
byte chaegeonCanSeq = 0;
|
|
unsigned long lastCanHeartbeatTime = 0;
|
|
|
|
bool consoleAlive = false;
|
|
bool consoleBusy = false;
|
|
bool consoleStartupApplyDone = false;
|
|
bool previousConsoleIgnitionOn = false;
|
|
bool consoleIgnitionApplyPending = false;
|
|
bool consoleCommandInFlight = false;
|
|
byte consoleCommandSeq = 0;
|
|
byte consoleCommandInFlightSeq = 0;
|
|
byte consoleLastAcceptedSeq = 0;
|
|
byte consoleActualClimatePack = 0;
|
|
byte consoleActualToggleBits = 0;
|
|
byte consoleButtonBits = 0;
|
|
byte consolePreviousButtonBits = 0;
|
|
byte consoleClimateTarget[CONSOLE_CLIMATE_COUNT] = {0, 0, 0, 0};
|
|
bool consoleToggleTarget[CONSOLE_TOGGLE_COUNT] = {false, false, false};
|
|
bool consoleManualLearnPending[CONSOLE_CHANNEL_COUNT] = {false};
|
|
unsigned long consoleManualLearnTime[CONSOLE_CHANNEL_COUNT] = {0};
|
|
unsigned long lastConsoleStatusTime = 0;
|
|
unsigned long lastConsoleCommandTime = 0;
|
|
unsigned long lastConsoleClimateRefreshTime = 0;
|
|
unsigned long lastConsoleSteeringRestartTime = 0;
|
|
byte consolePulseQueueChannel[CONSOLE_PULSE_QUEUE_SIZE] = {0};
|
|
unsigned int consolePulseQueueOnMs[CONSOLE_PULSE_QUEUE_SIZE] = {0};
|
|
unsigned int consolePulseQueueGapMs[CONSOLE_PULSE_QUEUE_SIZE] = {0};
|
|
byte consolePulseQueueHead = 0;
|
|
byte consolePulseQueueTail = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Serial.setTimeout(25);
|
|
|
|
pinMode(parkingPin, INPUT);
|
|
pinMode(ignitionPin, INPUT);
|
|
pinMode(seatRelayD8, OUTPUT);
|
|
pinMode(seatRelayD9, OUTPUT);
|
|
|
|
applySeatAction(0);
|
|
seat(0);
|
|
setupChaegeonCan();
|
|
loadSettings();
|
|
|
|
measurement();
|
|
confirmedDriveMode = getRawDriveMode();
|
|
pendingDriveMode = confirmedDriveMode;
|
|
targetPosMs = getTargetForConfirmedMode();
|
|
if (confirmedDriveMode == HIGH) {
|
|
// USB serial reconnect can reset the board while the car is already out of P.
|
|
// In that case, assume the seat is already at the saved driving position.
|
|
currentPosMs = storedSeatPositionMs;
|
|
targetPosMs = storedSeatPositionMs;
|
|
}
|
|
if (ignitionState == LOW) {
|
|
ignitionOffStartTime = millis();
|
|
}
|
|
|
|
lastTime = millis();
|
|
}
|
|
|
|
void loop() {
|
|
measurement();
|
|
handleSerial();
|
|
|
|
unsigned long currentTime = millis();
|
|
unsigned long deltaTime = currentTime - lastTime;
|
|
lastTime = currentTime;
|
|
|
|
if (prevIgnitionState == LOW && ignitionState == HIGH) {
|
|
isStabilizing = true;
|
|
stabilizeStartTime = currentTime;
|
|
} else if (prevIgnitionState == HIGH && ignitionState == LOW) {
|
|
registerIgnitionOffUse(currentTime);
|
|
}
|
|
prevIgnitionState = ignitionState;
|
|
updateChaegeonCan(currentTime);
|
|
|
|
if (autoCalibrationState != CALIBRATION_IDLE) {
|
|
updateAutoCalibration(currentTime);
|
|
updateRelayOutput(currentTime);
|
|
updateDebugOutput(currentTime);
|
|
return;
|
|
}
|
|
|
|
if (autoCalibrationPending && ignitionState == LOW &&
|
|
currentTime - ignitionOffStartTime >= AUTO_CALIBRATION_DELAY_MS) {
|
|
startAutoCalibration(currentTime);
|
|
updateRelayOutput(currentTime);
|
|
updateDebugOutput(currentTime);
|
|
return;
|
|
}
|
|
|
|
if (isStabilizing) {
|
|
if (currentTime - stabilizeStartTime < IGNITION_STABILIZE_MS) {
|
|
seat(0);
|
|
updateRelayOutput(currentTime);
|
|
updateDebugOutput(currentTime);
|
|
return;
|
|
}
|
|
isStabilizing = false;
|
|
}
|
|
|
|
updateConfirmedDriveMode(currentTime);
|
|
if (!manualMode) {
|
|
targetPosMs = getTargetForConfirmedMode();
|
|
}
|
|
normalizeMotionState();
|
|
updateMovement(deltaTime);
|
|
normalizeMotionState();
|
|
updateRelayOutput(currentTime);
|
|
updateManualStepStatus();
|
|
updateDebugOutput(currentTime);
|
|
}
|
|
|
|
void measurement() {
|
|
ignitionState = digitalRead(ignitionPin);
|
|
|
|
parkingRawValue = analogRead(parkingPin);
|
|
if (parkingState == LOW && parkingRawValue >= PARKING_ON_THRESHOLD) {
|
|
parkingState = HIGH;
|
|
} else if (parkingState == HIGH && parkingRawValue <= PARKING_OFF_THRESHOLD) {
|
|
parkingState = LOW;
|
|
}
|
|
}
|
|
|
|
boolean getRawDriveMode() {
|
|
if (ignitionState == HIGH && parkingState == LOW) {
|
|
return HIGH;
|
|
}
|
|
return LOW;
|
|
}
|
|
|
|
long getTargetForConfirmedMode() {
|
|
if (confirmedDriveMode == HIGH) {
|
|
return storedSeatPositionMs;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void updateConfirmedDriveMode(unsigned long currentTime) {
|
|
boolean rawDriveMode = getRawDriveMode();
|
|
|
|
if (rawDriveMode != pendingDriveMode) {
|
|
pendingDriveMode = rawDriveMode;
|
|
pendingDriveModeStartTime = currentTime;
|
|
}
|
|
|
|
unsigned long confirmMs = pendingDriveMode == HIGH ? DRIVE_CONFIRM_MS : PARK_CONFIRM_MS;
|
|
if (pendingDriveMode != confirmedDriveMode &&
|
|
currentTime - pendingDriveModeStartTime >= confirmMs) {
|
|
confirmedDriveMode = pendingDriveMode;
|
|
manualMode = false;
|
|
manualStepStatusPending = false;
|
|
}
|
|
}
|
|
|
|
void updateMovement(unsigned long deltaTime) {
|
|
if (downEdgeMarginActive) {
|
|
if (currentPosMs == 0 && targetPosMs == 0) {
|
|
seat(-1);
|
|
downEdgeMarginTime += deltaTime;
|
|
if (downEdgeMarginTime >= DOWN_EDGE_MARGIN_MS) {
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
downEdgeMarginActive = false;
|
|
seat(0);
|
|
}
|
|
return;
|
|
}
|
|
|
|
downEdgeMarginActive = false;
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
}
|
|
|
|
if (currentPosMs < targetPosMs) {
|
|
seat(1);
|
|
currentPosMs += deltaTime;
|
|
if (currentPosMs >= targetPosMs) {
|
|
currentPosMs = targetPosMs;
|
|
seat(0);
|
|
}
|
|
} else if (currentPosMs > targetPosMs) {
|
|
seat(-1);
|
|
long downMoveScaled = deltaTime * DOWN_SPEED_NUM + downSpeedRemainder;
|
|
currentPosMs -= downMoveScaled / DOWN_SPEED_DEN;
|
|
downSpeedRemainder = downMoveScaled % DOWN_SPEED_DEN;
|
|
if (currentPosMs <= targetPosMs) {
|
|
currentPosMs = targetPosMs;
|
|
downSpeedRemainder = 0;
|
|
if (targetPosMs == 0) {
|
|
startDownEdgeMargin();
|
|
} else {
|
|
seat(0);
|
|
}
|
|
}
|
|
} else {
|
|
seat(0);
|
|
}
|
|
}
|
|
|
|
void startDownEdgeMargin() {
|
|
downEdgeMarginActive = true;
|
|
downEdgeMarginTime = 0;
|
|
seat(-1);
|
|
}
|
|
|
|
void registerIgnitionOffUse(unsigned long currentTime) {
|
|
ignitionOffStartTime = currentTime;
|
|
|
|
if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
|
|
useCount++;
|
|
EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
|
|
}
|
|
|
|
if (useCount >= USES_BEFORE_AUTO_CALIBRATION) {
|
|
autoCalibrationPending = true;
|
|
}
|
|
}
|
|
|
|
void startAutoCalibration(unsigned long currentTime) {
|
|
manualMode = false;
|
|
manualStepStatusPending = false;
|
|
downSpeedRemainder = 0;
|
|
downEdgeMarginActive = false;
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
autoCalibrationState = CALIBRATION_UP;
|
|
calibrationPhaseStartTime = currentTime;
|
|
seat(1);
|
|
}
|
|
|
|
void updateAutoCalibration(unsigned long currentTime) {
|
|
if (ignitionState == HIGH && !forceCalibration) {
|
|
seat(0);
|
|
autoCalibrationState = CALIBRATION_IDLE;
|
|
manualStepStatusPending = false;
|
|
downSpeedRemainder = 0;
|
|
downEdgeMarginActive = false;
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
ignitionOffStartTime = currentTime;
|
|
return;
|
|
}
|
|
|
|
if (autoCalibrationState == CALIBRATION_UP) {
|
|
if (currentTime - calibrationPhaseStartTime >= AUTO_CALIBRATION_UP_MS) {
|
|
autoCalibrationState = CALIBRATION_DOWN;
|
|
calibrationPhaseStartTime = currentTime;
|
|
seat(-1);
|
|
} else {
|
|
seat(1);
|
|
}
|
|
} else if (autoCalibrationState == CALIBRATION_DOWN) {
|
|
if (currentTime - calibrationPhaseStartTime >= AUTO_CALIBRATION_DOWN_MS) {
|
|
seat(0);
|
|
autoCalibrationState = CALIBRATION_IDLE;
|
|
autoCalibrationPending = false;
|
|
forceCalibration = false;
|
|
useCount = 0;
|
|
EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
|
|
currentPosMs = 0;
|
|
targetPosMs = 0;
|
|
downSpeedRemainder = 0;
|
|
downEdgeMarginActive = false;
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
} else {
|
|
seat(-1);
|
|
}
|
|
}
|
|
}
|
|
|
|
void handleSerial() {
|
|
if (!Serial.available()) {
|
|
return;
|
|
}
|
|
|
|
String command = Serial.readStringUntil('\n');
|
|
command.trim();
|
|
|
|
if (debugEnabled) {
|
|
command.toLowerCase();
|
|
if (command == "x") {
|
|
debugEnabled = false;
|
|
Serial.println(F("DEBUG_STOPPED"));
|
|
printStatus(F("STATUS"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!serialIntroPrinted) {
|
|
if (command.length() == 0) {
|
|
printIntro();
|
|
serialIntroPrinted = true;
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (command.length() == 0) {
|
|
return;
|
|
}
|
|
|
|
processSerialCommand(command);
|
|
}
|
|
|
|
bool isSeatCommandBusy() {
|
|
return manualStepStatusPending ||
|
|
autoCalibrationState != CALIBRATION_IDLE ||
|
|
downEdgeMarginActive ||
|
|
currentPosMs != targetPosMs ||
|
|
requestedSeatAction != 0 ||
|
|
currentSeatAction != 0;
|
|
}
|
|
|
|
void processSerialCommand(String command) {
|
|
command.trim();
|
|
command.toLowerCase();
|
|
if (command.length() == 0) {
|
|
printStatus(F("EMPTY_INPUT_STATUS"));
|
|
return;
|
|
}
|
|
|
|
if (command.length() != 1) {
|
|
Serial.println(F("ERR: one-character command only. Send h for help."));
|
|
printStatus(F("ACK"));
|
|
return;
|
|
}
|
|
|
|
char name = command.charAt(0);
|
|
|
|
if (requiresParkingForCommand(name) && parkingState != HIGH) {
|
|
Serial.println(F("ERR: check gear state. Command requires P."));
|
|
printStatus(F("GEAR_CHECK_FAILED"));
|
|
return;
|
|
}
|
|
|
|
if (isSeatCommandBusy()) {
|
|
Serial.println(F("ERR: busy. Wait until seat movement is done."));
|
|
printStatus(F("BUSY"));
|
|
return;
|
|
}
|
|
|
|
if (name == 'h') {
|
|
printIntro();
|
|
} else if (name == 's') {
|
|
printStatus(F("STATUS"));
|
|
} else if (name == 'p') {
|
|
saveCurrentPositionCommand();
|
|
} else if (name == 'u') {
|
|
handleRelativeStep(1);
|
|
} else if (name == 'd') {
|
|
handleRelativeStep(-1);
|
|
} else if (name == 'g') {
|
|
debugEnabled = true;
|
|
lastDebugPrintTime = 0;
|
|
printStatus(F("DEBUG_STARTED_X_TO_STOP"));
|
|
} else if (name == 'a') {
|
|
manualStepStatusPending = false;
|
|
manualMode = false;
|
|
targetPosMs = getTargetForConfirmedMode();
|
|
printStatus(F("AUTO_MODE_RESTORED"));
|
|
} else if (name == 'c') {
|
|
manualStepStatusPending = false;
|
|
forceCalibration = true;
|
|
autoCalibrationPending = false;
|
|
startAutoCalibration(millis());
|
|
printStatus(F("CALIBRATION_STARTED_UP7_DOWN6"));
|
|
} else if (name == 'r') {
|
|
useCount = 0;
|
|
autoCalibrationPending = false;
|
|
EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
|
|
printStatus(F("USE_COUNT_RESET"));
|
|
} else {
|
|
Serial.println(F("Unknown command. Send h for help."));
|
|
printStatus(F("ACK"));
|
|
}
|
|
}
|
|
|
|
bool requiresParkingForCommand(char name) {
|
|
return name == 'a' ||
|
|
name == 'c' ||
|
|
name == 'r';
|
|
}
|
|
|
|
void saveCurrentPositionCommand() {
|
|
currentPosMs = clampPosition(currentPosMs);
|
|
targetPosMs = currentPosMs;
|
|
|
|
storedSeatPositionMs = (uint16_t)currentPosMs;
|
|
writeUint16(EEPROM_SEAT_POSITION_ADDR, storedSeatPositionMs);
|
|
manualMode = true;
|
|
targetPosMs = currentPosMs;
|
|
printStatus(F("CURRENT_POSITION_SAVED"));
|
|
}
|
|
|
|
void handleRelativeStep(int direction) {
|
|
long nextTarget = currentPosMs + (direction * MANUAL_STEP_MS);
|
|
if (nextTarget < 0 || nextTarget > SEAT_HARD_LIMIT_UP_MS) {
|
|
Serial.print(F("WARN: ignored. Requested target "));
|
|
Serial.print(nextTarget);
|
|
Serial.print(F("ms is outside 0.."));
|
|
Serial.print(SEAT_HARD_LIMIT_UP_MS);
|
|
Serial.println(F("ms."));
|
|
printStatus(F("ACK"));
|
|
return;
|
|
}
|
|
|
|
manualMode = true;
|
|
targetPosMs = nextTarget;
|
|
manualStepStatusPending = true;
|
|
}
|
|
|
|
void updateManualStepStatus() {
|
|
if (!manualStepStatusPending) {
|
|
return;
|
|
}
|
|
|
|
if (currentPosMs == targetPosMs && requestedSeatAction == 0 && currentSeatAction == 0) {
|
|
manualStepStatusPending = false;
|
|
printStatus(F("MANUAL_STEP_DONE"));
|
|
}
|
|
}
|
|
|
|
long clampPosition(long value) {
|
|
if (value < 0) {
|
|
return 0;
|
|
}
|
|
if (value > SEAT_HARD_LIMIT_UP_MS) {
|
|
return SEAT_HARD_LIMIT_UP_MS;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
void normalizeMotionState() {
|
|
currentPosMs = clampPosition(currentPosMs);
|
|
targetPosMs = clampPosition(targetPosMs);
|
|
|
|
if (storedSeatPositionMs > SEAT_HARD_LIMIT_UP_MS) {
|
|
storedSeatPositionMs = SEAT_HARD_LIMIT_UP_MS;
|
|
}
|
|
|
|
if (targetPosMs != 0 && downEdgeMarginActive) {
|
|
downEdgeMarginActive = false;
|
|
downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
|
|
}
|
|
|
|
if (currentPosMs <= targetPosMs) {
|
|
downSpeedRemainder = 0;
|
|
}
|
|
}
|
|
|
|
void updateDebugOutput(unsigned long currentTime) {
|
|
if (!debugEnabled) {
|
|
return;
|
|
}
|
|
|
|
if (lastDebugPrintTime == 0 || currentTime - lastDebugPrintTime >= DEBUG_INTERVAL_MS) {
|
|
lastDebugPrintTime = currentTime;
|
|
printDebugLine();
|
|
}
|
|
}
|
|
|
|
void loadSettings() {
|
|
if (EEPROM.read(EEPROM_MAGIC_ADDR) != EEPROM_MAGIC_VALUE) {
|
|
EEPROM.update(EEPROM_MAGIC_ADDR, EEPROM_MAGIC_VALUE);
|
|
EEPROM.update(EEPROM_USE_COUNT_ADDR, 0);
|
|
writeUint16(EEPROM_SEAT_POSITION_ADDR, SEAT_HARD_LIMIT_UP_MS);
|
|
}
|
|
|
|
useCount = EEPROM.read(EEPROM_USE_COUNT_ADDR);
|
|
if (useCount > USES_BEFORE_AUTO_CALIBRATION) {
|
|
useCount = 0;
|
|
EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
|
|
}
|
|
autoCalibrationPending = (useCount >= USES_BEFORE_AUTO_CALIBRATION);
|
|
|
|
storedSeatPositionMs = readUint16(EEPROM_SEAT_POSITION_ADDR);
|
|
if (storedSeatPositionMs > SEAT_HARD_LIMIT_UP_MS) {
|
|
storedSeatPositionMs = SEAT_HARD_LIMIT_UP_MS;
|
|
writeUint16(EEPROM_SEAT_POSITION_ADDR, storedSeatPositionMs);
|
|
}
|
|
|
|
loadConsoleSettings();
|
|
}
|
|
|
|
uint16_t readUint16(int addr) {
|
|
byte lowByte = EEPROM.read(addr);
|
|
byte highByte = EEPROM.read(addr + 1);
|
|
return (uint16_t)lowByte | ((uint16_t)highByte << 8);
|
|
}
|
|
|
|
void writeUint16(int addr, uint16_t value) {
|
|
EEPROM.update(addr, value & 0xFF);
|
|
EEPROM.update(addr + 1, (value >> 8) & 0xFF);
|
|
}
|
|
|
|
void printIntro() {
|
|
Serial.println();
|
|
Serial.println(F("+--------------------------------------------------+"));
|
|
Serial.println(F("| Auto Seat Serial Console |"));
|
|
Serial.println(F("+--------------------------------------------------+"));
|
|
Serial.println(F("| Units: milliseconds. 0 = bottom. 6000 = top cap. |"));
|
|
Serial.println(F("| Normal auto move target is saved in EEPROM. |"));
|
|
Serial.println(F("| a/c/r commands require gear P for safety. |"));
|
|
Serial.println(F("| Serial output is quiet unless a command is used. |"));
|
|
Serial.println(F("+--------------------------------------------------+"));
|
|
Serial.println(F("Commands:"));
|
|
Serial.println(F(" s Print current state once"));
|
|
Serial.println(F(" p Save current position to EEPROM"));
|
|
Serial.println(F(" u Manual up +500ms if inside 0..6000"));
|
|
Serial.println(F(" d Manual down -500ms if inside 0..6000"));
|
|
Serial.println(F(" g Debug every 1s; only x works in debug"));
|
|
Serial.println(F(" x Stop debug mode only"));
|
|
Serial.println(F(" d=down, g=debug to avoid command overlap"));
|
|
Serial.println(F(" a Return to gear-based automatic target"));
|
|
Serial.println(F(" c Run calibration now: up 7s, down 6s, set 0"));
|
|
Serial.println(F(" r Clear 10-use auto calibration counter"));
|
|
Serial.println(F(" h Print this help"));
|
|
Serial.println(F("+--------------------------------------------------+"));
|
|
printStatus(F("CURRENT_SETTINGS"));
|
|
}
|
|
|
|
void printStatus(const __FlashStringHelper *tag) {
|
|
Serial.println();
|
|
Serial.print(F("["));
|
|
Serial.print(tag);
|
|
Serial.println(F("]"));
|
|
printUptime();
|
|
printNamedInt(F("parkingRawValue"), parkingRawValue);
|
|
printNamedBool(F("ignition"), ignitionState == HIGH);
|
|
printNamedBool(F("parking"), parkingState == HIGH);
|
|
printNamedBool(F("rawDriveMode"), getRawDriveMode() == HIGH);
|
|
printNamedBool(F("confirmedDriveMode"), confirmedDriveMode == HIGH);
|
|
printNamedLong(F("currentPositionMs"), currentPosMs);
|
|
printNamedLong(F("targetPositionMs"), targetPosMs);
|
|
printNamedInt(F("savedPositionMs"), storedSeatPositionMs);
|
|
printNamedInt(F("hardLimitPositionMs"), SEAT_HARD_LIMIT_UP_MS);
|
|
printNamedInt(F("requestedRelayAction"), requestedSeatAction);
|
|
printNamedInt(F("actualRelayAction"), currentSeatAction);
|
|
printNamedBool(F("manualMode"), manualMode);
|
|
printNamedBool(F("stabilizing"), isStabilizing);
|
|
Serial.print(F("useCount="));
|
|
Serial.print(useCount);
|
|
Serial.print(F("/"));
|
|
Serial.println(USES_BEFORE_AUTO_CALIBRATION);
|
|
printNamedBool(F("autoCalibrationPending"), autoCalibrationPending);
|
|
printNamedBool(F("forceCalibration"), forceCalibration);
|
|
printNamedInt(F("autoCalibrationState"), autoCalibrationState);
|
|
printAutoCalibrationDelayStatus();
|
|
printConsoleStatusFields();
|
|
}
|
|
|
|
void printDebugLine() {
|
|
Serial.println();
|
|
Serial.println(F("[DEBUG]"));
|
|
printUptime();
|
|
printNamedInt(F("parkingRawValue"), parkingRawValue);
|
|
printNamedBool(F("ignition"), ignitionState == HIGH);
|
|
printNamedBool(F("parking"), parkingState == HIGH);
|
|
printNamedBool(F("rawDriveMode"), getRawDriveMode() == HIGH);
|
|
printNamedBool(F("confirmedDriveMode"), confirmedDriveMode == HIGH);
|
|
printNamedLong(F("currentPositionMs"), currentPosMs);
|
|
printNamedLong(F("targetPositionMs"), targetPosMs);
|
|
printNamedInt(F("savedPositionMs"), storedSeatPositionMs);
|
|
printNamedInt(F("hardLimitPositionMs"), SEAT_HARD_LIMIT_UP_MS);
|
|
printNamedInt(F("requestedRelayAction"), requestedSeatAction);
|
|
printNamedInt(F("actualRelayAction"), currentSeatAction);
|
|
printNamedBool(F("manualMode"), manualMode);
|
|
printNamedBool(F("stabilizing"), isStabilizing);
|
|
Serial.print(F("useCount="));
|
|
Serial.print(useCount);
|
|
Serial.print(F("/"));
|
|
Serial.println(USES_BEFORE_AUTO_CALIBRATION);
|
|
printNamedBool(F("autoCalibrationPending"), autoCalibrationPending);
|
|
printNamedBool(F("forceCalibration"), forceCalibration);
|
|
printNamedInt(F("autoCalibrationState"), autoCalibrationState);
|
|
printAutoCalibrationDelayStatus();
|
|
printConsoleStatusFields();
|
|
}
|
|
|
|
void printUptime() {
|
|
Serial.print(F("uptime="));
|
|
printDuration(millis());
|
|
Serial.println();
|
|
}
|
|
|
|
void printUptimeInline() {
|
|
Serial.print(F("uptime="));
|
|
printDuration(millis());
|
|
}
|
|
|
|
void printBool(bool value) {
|
|
Serial.print(value ? F("true") : F("false"));
|
|
}
|
|
|
|
void printNamedBool(const __FlashStringHelper *name, bool value) {
|
|
Serial.print(name);
|
|
Serial.print(F("="));
|
|
printBool(value);
|
|
Serial.println();
|
|
}
|
|
|
|
void printNamedInt(const __FlashStringHelper *name, long value) {
|
|
Serial.print(name);
|
|
Serial.print(F("="));
|
|
Serial.println(value);
|
|
}
|
|
|
|
void printNamedLong(const __FlashStringHelper *name, long value) {
|
|
printNamedInt(name, value);
|
|
}
|
|
|
|
void printDuration(unsigned long totalMs) {
|
|
unsigned long hours = totalMs / 3600000UL;
|
|
totalMs %= 3600000UL;
|
|
unsigned long minutes = totalMs / 60000UL;
|
|
totalMs %= 60000UL;
|
|
unsigned long seconds = totalMs / 1000UL;
|
|
unsigned long milliseconds = totalMs % 1000UL;
|
|
|
|
Serial.print(hours);
|
|
Serial.print(F("h "));
|
|
Serial.print(minutes);
|
|
Serial.print(F("m "));
|
|
Serial.print(seconds);
|
|
Serial.print(F("s "));
|
|
Serial.print(milliseconds);
|
|
Serial.print(F("ms"));
|
|
}
|
|
|
|
void printAutoCalibrationDelayStatusInline() {
|
|
if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
|
|
return;
|
|
}
|
|
|
|
Serial.print(F("autoCalibrationOffTimer="));
|
|
if (ignitionState == HIGH) {
|
|
Serial.print(F("waiting_for_ig_off"));
|
|
return;
|
|
}
|
|
|
|
unsigned long elapsedMs = millis() - ignitionOffStartTime;
|
|
unsigned long remainingMs = 0;
|
|
if (elapsedMs < AUTO_CALIBRATION_DELAY_MS) {
|
|
remainingMs = AUTO_CALIBRATION_DELAY_MS - elapsedMs;
|
|
}
|
|
|
|
Serial.print(F("elapsed "));
|
|
printDuration(elapsedMs);
|
|
Serial.print(F(" remaining "));
|
|
printDuration(remainingMs);
|
|
}
|
|
|
|
void printAutoCalibrationDelayStatus() {
|
|
if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
|
|
return;
|
|
}
|
|
|
|
printAutoCalibrationDelayStatusInline();
|
|
Serial.println();
|
|
}
|
|
|
|
void printConsoleStatusFields() {
|
|
printNamedBool(F("consoleCanReady"), chaegeonCanReady);
|
|
printNamedBool(F("consoleAlive"), consoleAlive);
|
|
printNamedBool(F("consoleBusy"), consoleBusy);
|
|
printNamedInt(F("consolePulseQueueLength"), consolePulseQueueLength());
|
|
printNamedInt(F("consoleDriverVentTarget"), consoleClimateTarget[CH_DRIVER_VENT]);
|
|
printNamedInt(F("consoleDriverHeatTarget"), consoleClimateTarget[CH_DRIVER_HEAT]);
|
|
printNamedInt(F("consolePassengerVentTarget"), consoleClimateTarget[CH_PASSENGER_VENT]);
|
|
printNamedInt(F("consolePassengerHeatTarget"), consoleClimateTarget[CH_PASSENGER_HEAT]);
|
|
printNamedBool(F("consoleSteeringHeatTarget"), consoleToggleTarget[TG_STEERING_HEAT]);
|
|
printNamedBool(F("consoleFrontSensorTarget"), consoleToggleTarget[TG_FRONT_SENSOR]);
|
|
printNamedBool(F("consoleVessTarget"), consoleToggleTarget[TG_VESS]);
|
|
printNamedInt(F("consoleDriverVentActual"), getConsoleClimateActual(CH_DRIVER_VENT));
|
|
printNamedInt(F("consoleDriverHeatActual"), getConsoleClimateActual(CH_DRIVER_HEAT));
|
|
printNamedInt(F("consolePassengerVentActual"), getConsoleClimateActual(CH_PASSENGER_VENT));
|
|
printNamedInt(F("consolePassengerHeatActual"), getConsoleClimateActual(CH_PASSENGER_HEAT));
|
|
printNamedBool(F("consoleSteeringHeatActual"), getConsoleToggleActual(TG_STEERING_HEAT));
|
|
printNamedBool(F("consoleFrontSensorActual"), getConsoleToggleActual(TG_FRONT_SENSOR));
|
|
printNamedBool(F("consoleVessActual"), getConsoleToggleActual(TG_VESS));
|
|
}
|
|
|
|
void updateChaegeonCan(unsigned long currentTime) {
|
|
updateChaegeonCanReceive(currentTime);
|
|
updateConsoleAlive(currentTime);
|
|
updateChaegeonCanHeartbeat(currentTime);
|
|
updateConsoleManualLearn(currentTime);
|
|
updateConsoleAutomation(currentTime);
|
|
updateConsoleCommandSender(currentTime);
|
|
}
|
|
|
|
void updateChaegeonCanReceive(unsigned long currentTime) {
|
|
if (!chaegeonCanReady) {
|
|
return;
|
|
}
|
|
|
|
while (chaegeonCan.checkReceive() == CAN_MSGAVAIL) {
|
|
unsigned long rxId = 0;
|
|
byte len = 0;
|
|
byte payload[8] = {0};
|
|
if (chaegeonCan.readMsgBuf(&rxId, &len, payload) != CAN_OK) {
|
|
return;
|
|
}
|
|
|
|
if (rxId == CAN_ID_CONSOLE_STATUS && len >= 6 && payload[0] == 1) {
|
|
consoleBusy = (payload[1] & 0x02) != 0;
|
|
consoleActualClimatePack = payload[2];
|
|
consoleActualToggleBits = payload[3];
|
|
consoleLastAcceptedSeq = payload[5];
|
|
lastConsoleStatusTime = currentTime;
|
|
consoleAlive = true;
|
|
|
|
if (consoleCommandInFlight &&
|
|
consoleLastAcceptedSeq == consoleCommandInFlightSeq) {
|
|
consoleCommandInFlight = false;
|
|
}
|
|
|
|
handleConsoleButtonBits(payload[4], currentTime);
|
|
}
|
|
}
|
|
}
|
|
|
|
void updateConsoleAlive(unsigned long currentTime) {
|
|
bool aliveNow = chaegeonCanReady &&
|
|
lastConsoleStatusTime != 0 &&
|
|
currentTime - lastConsoleStatusTime <= CONSOLE_CAN_TIMEOUT_MS;
|
|
|
|
if (!aliveNow && consoleAlive) {
|
|
consoleStartupApplyDone = false;
|
|
consoleCommandInFlight = false;
|
|
clearConsolePulseQueue();
|
|
}
|
|
|
|
consoleAlive = aliveNow;
|
|
}
|
|
|
|
void handleConsoleButtonBits(byte newButtonBits, unsigned long currentTime) {
|
|
consolePreviousButtonBits = consoleButtonBits;
|
|
consoleButtonBits = newButtonBits;
|
|
|
|
for (byte channel = 0; channel < CONSOLE_CHANNEL_COUNT; channel++) {
|
|
bool wasPressed = (consolePreviousButtonBits & (1 << channel)) != 0;
|
|
bool isPressed = (consoleButtonBits & (1 << channel)) != 0;
|
|
if (wasPressed && !isPressed) {
|
|
consoleManualLearnPending[channel] = true;
|
|
consoleManualLearnTime[channel] = currentTime + CONSOLE_MANUAL_LEARN_SETTLE_MS;
|
|
}
|
|
}
|
|
}
|
|
|
|
void updateConsoleManualLearn(unsigned long currentTime) {
|
|
for (byte channel = 0; channel < CONSOLE_CHANNEL_COUNT; channel++) {
|
|
if (!consoleManualLearnPending[channel]) {
|
|
continue;
|
|
}
|
|
|
|
if ((long)(currentTime - consoleManualLearnTime[channel]) < 0) {
|
|
continue;
|
|
}
|
|
|
|
consoleManualLearnPending[channel] = false;
|
|
learnConsoleManualChannel(channel);
|
|
}
|
|
}
|
|
|
|
void learnConsoleManualChannel(byte channel) {
|
|
if (channel <= CH_PASSENGER_HEAT) {
|
|
setConsoleClimateTarget(channel, getConsoleClimateActual(channel));
|
|
return;
|
|
}
|
|
|
|
byte toggleIndex = channel - CH_STEERING_HEAT;
|
|
if (toggleIndex < CONSOLE_TOGGLE_COUNT) {
|
|
setConsoleToggleTarget(toggleIndex, getConsoleToggleActual(toggleIndex));
|
|
}
|
|
}
|
|
|
|
void updateConsoleAutomation(unsigned long currentTime) {
|
|
bool ignitionOn = ignitionState == HIGH;
|
|
if (ignitionOn && !previousConsoleIgnitionOn) {
|
|
consoleIgnitionApplyPending = true;
|
|
}
|
|
previousConsoleIgnitionOn = ignitionOn;
|
|
|
|
if (!consoleAlive || !consoleReadyForNewSequence()) {
|
|
return;
|
|
}
|
|
|
|
if (!consoleStartupApplyDone && currentTime >= CONSOLE_STARTUP_DELAY_MS) {
|
|
applyConsoleClimateTargets(true);
|
|
if (ignitionOn) {
|
|
applyConsoleIgnitionTargets();
|
|
consoleIgnitionApplyPending = false;
|
|
}
|
|
consoleStartupApplyDone = true;
|
|
lastConsoleClimateRefreshTime = currentTime;
|
|
lastConsoleSteeringRestartTime = currentTime;
|
|
return;
|
|
}
|
|
|
|
if (ignitionOn && consoleIgnitionApplyPending) {
|
|
applyConsoleIgnitionTargets();
|
|
consoleIgnitionApplyPending = false;
|
|
lastConsoleSteeringRestartTime = currentTime;
|
|
previousConsoleIgnitionOn = ignitionOn;
|
|
return;
|
|
}
|
|
|
|
if (currentTime - lastConsoleClimateRefreshTime >= CONSOLE_MEMORY_REFRESH_INTERVAL_MS) {
|
|
applyConsoleClimateTargets(true);
|
|
if (ignitionOn) {
|
|
applyConsoleIgnitionTargets();
|
|
consoleIgnitionApplyPending = false;
|
|
}
|
|
lastConsoleClimateRefreshTime = currentTime;
|
|
return;
|
|
}
|
|
|
|
if (ignitionOn &&
|
|
consoleToggleTarget[TG_STEERING_HEAT] &&
|
|
currentTime - lastConsoleSteeringRestartTime >= CONSOLE_STEERING_RESTART_INTERVAL_MS) {
|
|
restartConsoleSteeringHeat();
|
|
lastConsoleSteeringRestartTime = currentTime;
|
|
}
|
|
}
|
|
|
|
bool consoleReadyForNewSequence() {
|
|
return !consoleBusy &&
|
|
!consoleCommandInFlight &&
|
|
consolePulseQueueHead == consolePulseQueueTail;
|
|
}
|
|
|
|
void applyConsoleClimateTargets(bool force) {
|
|
for (byte climate = 0; climate < CONSOLE_CLIMATE_COUNT; climate++) {
|
|
ensureConsoleClimateTarget(climate, force);
|
|
}
|
|
}
|
|
|
|
void applyConsoleIgnitionTargets() {
|
|
for (byte toggle = 0; toggle < CONSOLE_TOGGLE_COUNT; toggle++) {
|
|
ensureConsoleToggleTarget(toggle);
|
|
}
|
|
}
|
|
|
|
void ensureConsoleClimateTarget(byte climate, bool force) {
|
|
byte target = consoleClimateTarget[climate];
|
|
byte actual = getConsoleClimateActual(climate);
|
|
|
|
if (target == 0) {
|
|
if (actual != 0) {
|
|
enqueueConsolePulse(climate, CONSOLE_CLIMATE_LONG_OFF_MS, CONSOLE_CLIMATE_LONG_OFF_GAP_MS);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!force && actual == target) {
|
|
return;
|
|
}
|
|
|
|
enqueueConsoleClimateSet(climate, target);
|
|
}
|
|
|
|
void ensureConsoleToggleTarget(byte toggle) {
|
|
if (getConsoleToggleActual(toggle) == consoleToggleTarget[toggle]) {
|
|
return;
|
|
}
|
|
|
|
enqueueConsolePulse(toggle + CH_STEERING_HEAT,
|
|
CONSOLE_SHORT_PRESS_MS,
|
|
CONSOLE_SHORT_PRESS_GAP_MS);
|
|
}
|
|
|
|
void restartConsoleSteeringHeat() {
|
|
if (getConsoleToggleActual(TG_STEERING_HEAT)) {
|
|
enqueueConsolePulse(CH_STEERING_HEAT,
|
|
CONSOLE_SHORT_PRESS_MS,
|
|
CONSOLE_STEERING_RESTART_GAP_MS);
|
|
enqueueConsolePulse(CH_STEERING_HEAT,
|
|
CONSOLE_SHORT_PRESS_MS,
|
|
CONSOLE_SHORT_PRESS_GAP_MS);
|
|
} else {
|
|
enqueueConsolePulse(CH_STEERING_HEAT,
|
|
CONSOLE_SHORT_PRESS_MS,
|
|
CONSOLE_SHORT_PRESS_GAP_MS);
|
|
}
|
|
}
|
|
|
|
void enqueueConsoleClimateSet(byte climate, byte targetLevel) {
|
|
enqueueConsolePulse(climate, CONSOLE_CLIMATE_LONG_OFF_MS, CONSOLE_CLIMATE_LONG_OFF_GAP_MS);
|
|
|
|
byte shortPresses = getConsoleShortPressCount(targetLevel);
|
|
for (byte i = 0; i < shortPresses; i++) {
|
|
enqueueConsolePulse(climate, CONSOLE_SHORT_PRESS_MS, CONSOLE_SHORT_PRESS_GAP_MS);
|
|
}
|
|
}
|
|
|
|
byte getConsoleShortPressCount(byte level) {
|
|
if (level == 3) {
|
|
return 1;
|
|
}
|
|
if (level == 2) {
|
|
return 2;
|
|
}
|
|
if (level == 1) {
|
|
return 3;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
byte getConsoleClimateActual(byte climate) {
|
|
return (consoleActualClimatePack >> (climate * 2)) & 0x03;
|
|
}
|
|
|
|
bool getConsoleToggleActual(byte toggle) {
|
|
return (consoleActualToggleBits & (1 << toggle)) != 0;
|
|
}
|
|
|
|
void setConsoleClimateTarget(byte climate, byte level) {
|
|
if (level > 3) {
|
|
level = 3;
|
|
}
|
|
|
|
bool changed = false;
|
|
if (consoleClimateTarget[climate] != level) {
|
|
consoleClimateTarget[climate] = level;
|
|
changed = true;
|
|
}
|
|
|
|
if (level > 0) {
|
|
byte opposite = CONSOLE_OPPOSITE_CLIMATE[climate];
|
|
if (consoleClimateTarget[opposite] != 0) {
|
|
consoleClimateTarget[opposite] = 0;
|
|
changed = true;
|
|
}
|
|
}
|
|
|
|
if (changed) {
|
|
saveConsoleSettings();
|
|
}
|
|
}
|
|
|
|
void setConsoleToggleTarget(byte toggle, bool enabled) {
|
|
if (consoleToggleTarget[toggle] == enabled) {
|
|
return;
|
|
}
|
|
|
|
consoleToggleTarget[toggle] = enabled;
|
|
saveConsoleSettings();
|
|
}
|
|
|
|
void updateConsoleCommandSender(unsigned long currentTime) {
|
|
if (!chaegeonCanReady || !consoleAlive || consoleBusy) {
|
|
return;
|
|
}
|
|
|
|
if (consoleCommandInFlight) {
|
|
if (currentTime - lastConsoleCommandTime >= CONSOLE_COMMAND_HOLD_MS) {
|
|
consoleCommandInFlight = false;
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (consolePulseQueueHead == consolePulseQueueTail) {
|
|
return;
|
|
}
|
|
|
|
byte channel = consolePulseQueueChannel[consolePulseQueueHead];
|
|
unsigned int onMs = consolePulseQueueOnMs[consolePulseQueueHead];
|
|
unsigned int gapMs = consolePulseQueueGapMs[consolePulseQueueHead];
|
|
|
|
byte seq = nextConsoleCommandSeq();
|
|
byte payload[8] = {
|
|
1,
|
|
1,
|
|
channel,
|
|
(byte)(onMs & 0xFF),
|
|
(byte)(onMs >> 8),
|
|
(byte)(gapMs & 0xFF),
|
|
(byte)(gapMs >> 8),
|
|
seq
|
|
};
|
|
|
|
if (chaegeonCan.sendMsgBuf(CAN_ID_CONSOLE_COMMAND, 0, 8, payload) == CAN_OK) {
|
|
consolePulseQueueHead = nextConsoleQueueIndex(consolePulseQueueHead);
|
|
consoleCommandInFlight = true;
|
|
consoleCommandInFlightSeq = seq;
|
|
lastConsoleCommandTime = currentTime;
|
|
}
|
|
}
|
|
|
|
byte nextConsoleCommandSeq() {
|
|
consoleCommandSeq++;
|
|
if (consoleCommandSeq == 0) {
|
|
consoleCommandSeq = 1;
|
|
}
|
|
return consoleCommandSeq;
|
|
}
|
|
|
|
void enqueueConsolePulse(byte channel, unsigned int onMs, unsigned int gapMs) {
|
|
byte nextTail = nextConsoleQueueIndex(consolePulseQueueTail);
|
|
if (nextTail == consolePulseQueueHead || channel >= CONSOLE_CHANNEL_COUNT) {
|
|
return;
|
|
}
|
|
|
|
consolePulseQueueChannel[consolePulseQueueTail] = channel;
|
|
consolePulseQueueOnMs[consolePulseQueueTail] = onMs;
|
|
consolePulseQueueGapMs[consolePulseQueueTail] = gapMs;
|
|
consolePulseQueueTail = nextTail;
|
|
}
|
|
|
|
void clearConsolePulseQueue() {
|
|
consolePulseQueueHead = consolePulseQueueTail;
|
|
}
|
|
|
|
byte consolePulseQueueLength() {
|
|
if (consolePulseQueueTail >= consolePulseQueueHead) {
|
|
return consolePulseQueueTail - consolePulseQueueHead;
|
|
}
|
|
return CONSOLE_PULSE_QUEUE_SIZE - consolePulseQueueHead + consolePulseQueueTail;
|
|
}
|
|
|
|
byte nextConsoleQueueIndex(byte index) {
|
|
index++;
|
|
if (index >= CONSOLE_PULSE_QUEUE_SIZE) {
|
|
index = 0;
|
|
}
|
|
return index;
|
|
}
|
|
|
|
void loadConsoleSettings() {
|
|
if (EEPROM.read(EEPROM_CONSOLE_MAGIC_ADDR) != EEPROM_CONSOLE_MAGIC_VALUE) {
|
|
EEPROM.update(EEPROM_CONSOLE_MAGIC_ADDR, EEPROM_CONSOLE_MAGIC_VALUE);
|
|
for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
|
|
EEPROM.update(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i, 0);
|
|
}
|
|
for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
|
|
EEPROM.update(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i, 0);
|
|
}
|
|
}
|
|
|
|
for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
|
|
byte value = EEPROM.read(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i);
|
|
consoleClimateTarget[i] = value <= 3 ? value : 0;
|
|
}
|
|
for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
|
|
consoleToggleTarget[i] = EEPROM.read(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i) != 0;
|
|
}
|
|
|
|
if (consoleClimateTarget[CH_DRIVER_VENT] > 0 &&
|
|
consoleClimateTarget[CH_DRIVER_HEAT] > 0) {
|
|
consoleClimateTarget[CH_DRIVER_HEAT] = 0;
|
|
}
|
|
if (consoleClimateTarget[CH_PASSENGER_VENT] > 0 &&
|
|
consoleClimateTarget[CH_PASSENGER_HEAT] > 0) {
|
|
consoleClimateTarget[CH_PASSENGER_HEAT] = 0;
|
|
}
|
|
saveConsoleSettings();
|
|
}
|
|
|
|
void saveConsoleSettings() {
|
|
for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
|
|
EEPROM.update(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i, consoleClimateTarget[i]);
|
|
}
|
|
for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
|
|
EEPROM.update(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i, consoleToggleTarget[i] ? 1 : 0);
|
|
}
|
|
}
|
|
|
|
void setupChaegeonCan() {
|
|
if (chaegeonCan.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ) == CAN_OK) {
|
|
chaegeonCan.setMode(MCP_NORMAL);
|
|
chaegeonCanReady = true;
|
|
}
|
|
}
|
|
|
|
void updateChaegeonCanHeartbeat(unsigned long currentTime) {
|
|
if (!chaegeonCanReady) {
|
|
return;
|
|
}
|
|
|
|
if (currentTime - lastCanHeartbeatTime < CAN_HEARTBEAT_INTERVAL_MS) {
|
|
return;
|
|
}
|
|
lastCanHeartbeatTime = currentTime;
|
|
|
|
byte flags = 0;
|
|
if (ignitionState == HIGH) {
|
|
flags |= 0x01;
|
|
}
|
|
if (parkingState == HIGH) {
|
|
flags |= 0x02;
|
|
}
|
|
if (getRawDriveMode() == HIGH) {
|
|
flags |= 0x04;
|
|
}
|
|
if (confirmedDriveMode == HIGH) {
|
|
flags |= 0x08;
|
|
}
|
|
if (isStabilizing) {
|
|
flags |= 0x10;
|
|
}
|
|
if (currentSeatAction != 0 || requestedSeatAction != 0) {
|
|
flags |= 0x20;
|
|
}
|
|
if (autoCalibrationPending || autoCalibrationState != CALIBRATION_IDLE) {
|
|
flags |= 0x40;
|
|
}
|
|
|
|
uint16_t raw = (uint16_t)parkingRawValue;
|
|
uint16_t uptimeSeconds = (uint16_t)(currentTime / 1000UL);
|
|
byte payload[8] = {
|
|
1,
|
|
flags,
|
|
chaegeonCanSeq++,
|
|
useCount,
|
|
(byte)(raw & 0xFF),
|
|
(byte)(raw >> 8),
|
|
(byte)(uptimeSeconds & 0xFF),
|
|
(byte)(uptimeSeconds >> 8)
|
|
};
|
|
|
|
chaegeonCan.sendMsgBuf(CAN_ID_MAIN_HEARTBEAT, 0, 8, payload);
|
|
}
|
|
|
|
void setRelay(int relayPin, boolean state) {
|
|
digitalWrite(relayPin, state ? LOW : HIGH);
|
|
}
|
|
|
|
void seat(int action) {
|
|
requestedSeatAction = action;
|
|
}
|
|
|
|
void updateRelayOutput(unsigned long currentTime) {
|
|
if (requestedSeatAction == currentSeatAction) {
|
|
applySeatAction(currentSeatAction);
|
|
return;
|
|
}
|
|
|
|
if (currentSeatAction != 0) {
|
|
lastStoppedSeatAction = currentSeatAction;
|
|
currentSeatAction = 0;
|
|
relayDeadtimeStartTime = currentTime;
|
|
applySeatAction(0);
|
|
return;
|
|
}
|
|
|
|
bool sameDirectionResume = requestedSeatAction != 0 && requestedSeatAction == lastStoppedSeatAction;
|
|
bool deadtimeDone = currentTime - relayDeadtimeStartTime >= RELAY_DEADTIME_MS;
|
|
if (sameDirectionResume || deadtimeDone) {
|
|
currentSeatAction = requestedSeatAction;
|
|
}
|
|
|
|
applySeatAction(currentSeatAction);
|
|
}
|
|
|
|
void applySeatAction(int action) {
|
|
if (action == 1) {
|
|
setRelay(seatRelayD8, 1);
|
|
setRelay(seatRelayD9, 0);
|
|
} else if (action == -1) {
|
|
setRelay(seatRelayD8, 0);
|
|
setRelay(seatRelayD9, 1);
|
|
} else {
|
|
setRelay(seatRelayD8, 0);
|
|
setRelay(seatRelayD9, 0);
|
|
}
|
|
}
|