Matthieu614
#include <ESP32-TWAI-CAN.hpp>
#include <HardwareSerial.h>
#include <cmath>
// =========================================================================
// Configuration
// =========================================================================
#define CAN_TX 5
#define CAN_RX 4
const int CAN_ID_STATUS_DOORS_TEMP = 0x60D;
const int CAN_ID_ODOMETER = 0x5FD;
const int CAN_ID_RPM = 0x181;
const int CAN_ID_STEERING = 0x0C2;
const int CAN_ID_PARKING_BRAKE = 0x5C5;
const int CAN_BAUD_RATE = 500000;
// --- Steering Angle Calibration ---
const int STEERING_RAW_LEFT = 107;
const int STEERING_RAW_RIGHT = 148;
const int STEERING_MAPPED_LEFT = -540;
const int STEERING_MAPPED_RIGHT = 540;
const int UART_TX_PIN = 17;
const long UART_BAUD_RATE = 38400;
HardwareSerial UartPort(1);
const uint8_t CMD_STATUS = 0x41;
const uint8_t CMD_STEERING_WHEEL = 0x26;
const uint8_t SUB_CMD_DOOR_INFO = 0x01;
const uint8_t SUB_CMD_VEHICLE_INFO = 0x02;
const long VEHICLE_INFO_SEND_INTERVAL_MS = 500;
const long STEERING_SEND_INTERVAL_MS = 200;
const long STATUS_SEND_INTERVAL_MS = 250;
// --- Bitmasks for the status byte ---
const uint8_t MASK_PARKING_BRAKE_ON = 0x20;
// =========================================================================
// Global Variables
// =========================================================================
CanFrame rxFrame;
int outsideTemperature = 15;
uint32_t odometerValue = 0;
uint16_t engineRpm = 0;
int16_t steeringAngleMapped = 0;
uint8_t currentDoorStateMask = 0;
bool isParkingBrakeOn = false;
unsigned long lastVehicleInfoSendTime = 0;
unsigned long lastSteeringSendTime = 0;
unsigned long lastStatusSendTime = 0;
// =========================================================================
// Helper Functions
// =========================================================================
void sendCanboxMessage(uint8_t cmd, uint8_t* data, uint8_t len) {
uint8_t sum = cmd + len;
for (int i = 0; i < len; i++) { sum += data[i]; }
uint8_t checksum = sum ^ 0xFF;
uint8_t finalFrame[len + 4];
finalFrame[0] = 0x2E;
finalFrame[1] = cmd;
finalFrame[2] = len;
memcpy(&finalFrame[3], data, len);
finalFrame[3 + len] = checksum;
UartPort.write(finalFrame, sizeof(finalFrame));
}
void sendStatusByteCommand(uint8_t doorStateMask) {
uint8_t finalStateMask = doorStateMask;
if (isParkingBrakeOn) {
finalStateMask |= MASK_PARKING_BRAKE_ON;
}
uint8_t dataPayload[] = {SUB_CMD_DOOR_INFO, finalStateMask};
sendCanboxMessage(CMD_STATUS, dataPayload, sizeof(dataPayload));
}
void sendVehicleInfoCommand() {
uint16_t temp_encoded = outsideTemperature * 10;
uint8_t dataPayload[13] = {
SUB_CMD_VEHICLE_INFO,
(uint8_t)(engineRpm >> 8), (uint8_t)(engineRpm),
0, 0, 0, 0,
(uint8_t)(temp_encoded >> 8), (uint8_t)(temp_encoded),
(uint8_t)(odometerValue >> 16), (uint8_t)(odometerValue >> 8), (uint8_t)(odometerValue),
0
};
sendCanboxMessage(CMD_STATUS, dataPayload, sizeof(dataPayload));
}
void sendSteeringAngleCommand() {
uint8_t dataPayload[2] = {(uint8_t)(steeringAngleMapped), (uint8_t)(steeringAngleMapped >> 8)};
sendCanboxMessage(CMD_STEERING_WHEEL, dataPayload, sizeof(dataPayload));
}
// =========================================================================
// SETUP
// =========================================================================
void setup() {
// Serial.begin(115200); // Debugging disabled
UartPort.begin(UART_BAUD_RATE, SERIAL_8N1, -1, UART_TX_PIN);
ESP32Can.setPins(CAN_TX, CAN_RX);
ESP32Can.setSpeed(ESP32Can.convertSpeed(CAN_BAUD_RATE));
ESP32Can.begin();
}
// =========================================================================
// LOOP
// =========================================================================
void loop() {
if (ESP32Can.readFrame(rxFrame)) {
switch (rxFrame.identifier) {
case CAN_ID_STATUS_DOORS_TEMP:
if (rxFrame.data_length_code >= 5) {
outsideTemperature = round((float)rxFrame.data[4] - 40.0);
uint8_t byteA = rxFrame.data[0];
uint8_t newDoorStateMask = 0;
if (byteA & (1 << 4)) newDoorStateMask |= 0b00000001; // Driver (A.5)
if (byteA & (1 << 3)) newDoorStateMask |= 0b00000010; // Passenger (A.4)
currentDoorStateMask = newDoorStateMask;
}
break;
case CAN_ID_ODOMETER:
if (rxFrame.data_length_code >= 3) {
uint32_t rawOdo = ((uint32_t)rxFrame.data[0] << 16) | ((uint32_t)rxFrame.data[1] << 8) | rxFrame.data[2];
odometerValue = rawOdo / 16;
}
break;
case CAN_ID_RPM:
if (rxFrame.data_length_code >= 2) {
uint16_t rawRpm = ((uint16_t)rxFrame.data[0] << 8) | rxFrame.data[1];
// CORRECTED: RPM value is divided by 8 to get the real value
engineRpm = rawRpm / 8;
}
break;
case CAN_ID_STEERING:
if (rxFrame.data_length_code >= 1) {
int rawAngle = rxFrame.data[0];
steeringAngleMapped = map(rawAngle, STEERING_RAW_LEFT, STEERING_RAW_RIGHT, STEERING_MAPPED_LEFT, STEERING_MAPPED_RIGHT);
}
break;
case CAN_ID_PARKING_BRAKE:
if (rxFrame.data_length_code >= 1) {
isParkingBrakeOn = (rxFrame.data[0] & (1 << 2)) == 0;
}
break;
}
}
// --- Handle Timed Sends ---
unsigned long currentTime = millis();
if (currentTime - lastVehicleInfoSendTime >= VEHICLE_INFO_SEND_INTERVAL_MS) {
lastVehicleInfoSendTime = currentTime;
sendVehicleInfoCommand();
}
if (currentTime - lastSteeringSendTime >= STEERING_SEND_INTERVAL_MS) {
lastSteeringSendTime = currentTime;
sendSteeringAngleCommand();
}
if (currentTime - lastStatusSendTime >= STATUS_SEND_INTERVAL_MS) {
lastStatusSendTime = currentTime;
sendStatusByteCommand(currentDoorStateMask);
}
}
