diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index 22f260e7..71921d60 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -23,6 +23,6 @@ - Inverter communication protocol: `` - Hardware used for Battery-Emulator: `HW_LILYGO, HW_STARK, Custom` - CONTACTOR_CONTROL: `yes/no` -- DUAL_CAN: `yes/no` +- CAN_ADDON: `yes/no` - WEBSERVER: `yes/no` - MQTT: `yes/no` diff --git a/Software/Software.ino b/Software/Software.ino index 1712c70f..efcb4051 100644 --- a/Software/Software.ino +++ b/Software/Software.ino @@ -11,6 +11,12 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "src/charger/CHARGERS.h" +#include "src/communication/can/comm_can.h" +#include "src/communication/contactorcontrol/comm_contactorcontrol.h" +#include "src/communication/equipmentstopbutton/comm_equipmentstopbutton.h" +#include "src/communication/nvm/comm_nvm.h" +#include "src/communication/rs485/comm_rs485.h" +#include "src/communication/seriallink/comm_seriallink.h" #include "src/datalayer/datalayer.h" #include "src/devboard/utils/events.h" #include "src/devboard/utils/led_handler.h" @@ -41,17 +47,6 @@ #endif // MQTT #endif // WIFI -#ifndef CONTACTOR_CONTROL -#ifdef PWM_CONTACTOR_CONTROL -#error CONTACTOR_CONTROL needs to be enabled for PWM_CONTACTOR_CONTROL -#endif -#endif - -#ifdef EQUIPMENT_STOP_BUTTON -#include "src/devboard/utils/debounce_button.h" -#endif - -Preferences settings; // Store user settings // The current software version, shown on webserver const char* version_number = "8.0.dev"; @@ -60,33 +55,6 @@ uint16_t intervalUpdateValues = INTERVAL_1_S; // Interval at which to update in unsigned long previousMillis10ms = 0; unsigned long previousMillisUpdateVal = 0; -// CAN parameters -CAN_device_t CAN_cfg; // CAN Config -const int rx_queue_size = 10; // Receive Queue size -volatile bool send_ok = 0; - -#ifdef DUAL_CAN -#include "src/lib/pierremolinaro-acan2515/ACAN2515.h" -static const uint32_t QUARTZ_FREQUENCY = CRYSTAL_FREQUENCY_MHZ * 1000000UL; //MHZ configured in USER_SETTINGS.h -ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT); -static ACAN2515_Buffer16 gBuffer; -#endif //DUAL_CAN -#ifdef CAN_FD -#include "src/lib/pierremolinaro-ACAN2517FD/ACAN2517FD.h" -ACAN2517FD canfd(MCP2517_CS, SPI, MCP2517_INT); -#endif //CAN_FD - -// ModbusRTU parameters -#ifdef MODBUS_INVERTER_SELECTED -#define MB_RTU_NUM_VALUES 13100 -uint16_t mbPV[MB_RTU_NUM_VALUES]; // Process variable memory -// Create a ModbusRTU server instance listening on Serial2 with 2000ms timeout -ModbusServerRTU MBserver(Serial2, 2000); -#endif -#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) -#define SERIAL_LINK_BAUDRATE 112500 -#endif - // Common charger parameters volatile float charger_setpoint_HV_VDC = 0.0f; volatile float charger_setpoint_HV_IDC = 0.0f; @@ -113,61 +81,6 @@ MyTimer loop_task_timer_10s(INTERVAL_10_S); MyTimer check_pause_2s(INTERVAL_2_S); -// Contactor parameters -#ifdef CONTACTOR_CONTROL -enum State { DISCONNECTED, START_PRECHARGE, PRECHARGE, POSITIVE, PRECHARGE_OFF, COMPLETED, SHUTDOWN_REQUESTED }; -State contactorStatus = DISCONNECTED; - -#define ON 1 -#define OFF 0 - -#ifdef NC_CONTACTORS //Normally closed contactors use inverted logic -#undef ON -#define ON 0 -#undef OFF -#define OFF 1 -#endif //NC_CONTACTORS - -#define MAX_ALLOWED_FAULT_TICKS 1000 -#define NEGATIVE_CONTACTOR_TIME_MS \ - 500 // Time after negative contactor is turned on, to start precharge (not actual precharge time!) -#define PRECHARGE_COMPLETED_TIME_MS \ - 1000 // After successful precharge, resistor is turned off after this delay (and contactors are economized if PWM enabled) -#define PWM_Freq 20000 // 20 kHz frequency, beyond audible range -#define PWM_Res 10 // 10 Bit resolution 0 to 1023, maps 'nicely' to 0% 100% -#define PWM_HOLD_DUTY 250 -#define PWM_OFF_DUTY 0 -#define PWM_ON_DUTY 1023 -#define PWM_Positive_Channel 0 -#define PWM_Negative_Channel 1 -unsigned long prechargeStartTime = 0; -unsigned long negativeStartTime = 0; -unsigned long prechargeCompletedTime = 0; -unsigned long timeSpentInFaultedMode = 0; -#endif - -void set(uint8_t pin, bool direction, uint32_t pwm_freq = 0xFFFF) { -#ifdef PWM_CONTACTOR_CONTROL - if (pwm_freq != 0xFFFF) { - ledcWrite(pin, pwm_freq); - return; - } -#endif - if (direction == 1) { - digitalWrite(pin, HIGH); - } else { // 0 - digitalWrite(pin, LOW); - } -} - -#ifdef EQUIPMENT_STOP_BUTTON -const unsigned long equipment_button_long_press_duration = - 15000; // 15 seconds for long press in case of MOMENTARY_SWITCH -const unsigned long equipment_button_debounce_duration = 200; // 250ms for debouncing the button -unsigned long timeSincePress = 0; // Variable to store the time since the last press -DebouncedButton equipment_stop_button; // Debounced button object -#endif - TaskHandle_t main_loop_task; TaskHandle_t connectivity_loop_task; @@ -276,24 +189,24 @@ void core_loop(void* task_time_us) { // Input, Runs as fast as possible receive_can_native(); // Receive CAN messages from native CAN port -#ifdef CAN_FD - receive_canfd(); // Receive CAN-FD messages. -#endif -#ifdef DUAL_CAN - receive_can_addonMCP2515(); // Receive CAN messages on add-on MCP2515 chip -#endif +#ifdef CANFD_ADDON + receive_canfd_addon(); // Receive CAN-FD messages. +#endif // CANFD_ADDON +#ifdef CAN_ADDON + receive_can_addon(); // Receive CAN messages on add-on MCP2515 chip +#endif // CAN_ADDON #ifdef RS485_INVERTER_SELECTED receive_RS485(); // Process serial2 RS485 interface -#endif +#endif // RS485_INVERTER_SELECTED #if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) - runSerialDataLink(); -#endif + run_serialDataLink(); +#endif // SERIAL_LINK_RECEIVER || SERIAL_LINK_TRANSMITTER END_TIME_MEASUREMENT_MAX(comm, datalayer.system.status.time_comm_us); #ifdef WEBSERVER START_TIME_MEASUREMENT(ota); ElegantOTA.loop(); END_TIME_MEASUREMENT_MAX(ota, datalayer.system.status.time_ota_us); -#endif +#endif // WEBSERVER START_TIME_MEASUREMENT(time_10ms); // Process @@ -311,12 +224,12 @@ void core_loop(void* task_time_us) { #ifdef DOUBLE_BATTERY update_values_battery2(); check_interconnect_available(); -#endif +#endif // DOUBLE_BATTERY update_calculated_values(); #ifndef SERIAL_LINK_RECEIVER update_machineryprotection(); // Check safeties (Not on serial link reciever board) -#endif - update_values_inverter(); // Update values heading towards inverter +#endif // SERIAL_LINK_RECEIVER + update_values_inverter(); // Update values heading towards inverter if (DUMMY_EVENT_ENABLED) { set_event(EVENT_DUMMY_ERROR, (uint8_t)millis()); } @@ -330,7 +243,6 @@ void core_loop(void* task_time_us) { END_TIME_MEASUREMENT_MAX(cantx, datalayer.system.status.time_cantx_us); END_TIME_MEASUREMENT_MAX(all, datalayer.system.status.core_task_10s_max_us); #ifdef FUNCTION_TIME_MEASUREMENT - if (datalayer.system.status.core_task_10s_max_us > datalayer.system.status.core_task_max_us) { // Update worst case total time datalayer.system.status.core_task_max_us = datalayer.system.status.core_task_10s_max_us; @@ -352,8 +264,7 @@ void core_loop(void* task_time_us) { datalayer.system.status.time_cantx_us = 0; datalayer.system.status.core_task_10s_max_us = 0; } - -#endif +#endif // FUNCTION_TIME_MEASUREMENT if (check_pause_2s.elapsed()) { emulator_pause_state_send_CAN_battery(); } @@ -369,396 +280,9 @@ void init_serial() { while (!Serial) {} #ifdef DEBUG_VIA_USB Serial.println("__ OK __"); -#endif -} - -void init_stored_settings() { - static uint32_t temp = 0; - // ATTENTION ! The maximum length for settings keys is 15 characters - settings.begin("batterySettings", false); - - // Always get the equipment stop status - datalayer.system.settings.equipment_stop_active = settings.getBool("EQUIPMENT_STOP", false); - if (datalayer.system.settings.equipment_stop_active) { - set_event(EVENT_EQUIPMENT_STOP, 1); - } - -#ifndef LOAD_SAVED_SETTINGS_ON_BOOT - settings.clear(); // If this clear function is executed, no settings will be read from storage - - //always save the equipment stop status - settings.putBool("EQUIPMENT_STOP", datalayer.system.settings.equipment_stop_active); - -#endif //LOAD_SAVED_SETTINGS_ON_BOOT - -#ifdef WIFI - - char tempSSIDstring[63]; // Allocate buffer with sufficient size - size_t lengthSSID = settings.getString("SSID", tempSSIDstring, sizeof(tempSSIDstring)); - if (lengthSSID > 0) { // Successfully read the string from memory. Set it to SSID! - ssid = tempSSIDstring; - } else { // Reading from settings failed. Do nothing with SSID. Raise event? - } - char tempPasswordString[63]; // Allocate buffer with sufficient size - size_t lengthPassword = settings.getString("PASSWORD", tempPasswordString, sizeof(tempPasswordString)); - if (lengthPassword > 7) { // Successfully read the string from memory. Set it to password! - password = tempPasswordString; - } else { // Reading from settings failed. Do nothing with SSID. Raise event? - } -#endif //WIFI - - temp = settings.getUInt("BATTERY_WH_MAX", false); - if (temp != 0) { - datalayer.battery.info.total_capacity_Wh = temp; - } - temp = settings.getUInt("MAXPERCENTAGE", false); - if (temp != 0) { - datalayer.battery.settings.max_percentage = temp * 10; // Multiply by 10 for backwards compatibility - } - temp = settings.getUInt("MINPERCENTAGE", false); - if (temp != 0) { - datalayer.battery.settings.min_percentage = temp * 10; // Multiply by 10 for backwards compatibility - } - temp = settings.getUInt("MAXCHARGEAMP", false); - if (temp != 0) { - datalayer.battery.settings.max_user_set_charge_dA = temp; - } - temp = settings.getUInt("MAXDISCHARGEAMP", false); - if (temp != 0) { - datalayer.battery.settings.max_user_set_discharge_dA = temp; - } - datalayer.battery.settings.soc_scaling_active = settings.getBool("USE_SCALED_SOC", false); - temp = settings.getUInt("TARGETCHVOLT", false); - if (temp != 0) { - datalayer.battery.settings.max_user_set_charge_voltage_dV = temp; - } - temp = settings.getUInt("TARGETDISCHVOLT", false); - if (temp != 0) { - datalayer.battery.settings.max_user_set_discharge_voltage_dV = temp; - } - datalayer.battery.settings.user_set_voltage_limits_active = settings.getBool("USEVOLTLIMITS", false); - settings.end(); -} - -void init_CAN() { -// CAN pins -#ifdef CAN_SE_PIN - pinMode(CAN_SE_PIN, OUTPUT); - digitalWrite(CAN_SE_PIN, LOW); -#endif - CAN_cfg.speed = CAN_SPEED_500KBPS; -#ifdef NATIVECAN_250KBPS // Some component is requesting lower CAN speed - CAN_cfg.speed = CAN_SPEED_250KBPS; -#endif - CAN_cfg.tx_pin_id = CAN_TX_PIN; - CAN_cfg.rx_pin_id = CAN_RX_PIN; - CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); - // Init CAN Module - ESP32Can.CANInit(); - -#ifdef DUAL_CAN -#ifdef DEBUG_VIA_USB - Serial.println("Dual CAN Bus (ESP32+MCP2515) selected"); -#endif - gBuffer.initWithSize(25); - SPI.begin(MCP2515_SCK, MCP2515_MISO, MCP2515_MOSI); - ACAN2515Settings settings(QUARTZ_FREQUENCY, 500UL * 1000UL); // CAN bit rate 500 kb/s - settings.mRequestedMode = ACAN2515Settings::NormalMode; - const uint16_t errorCodeMCP = can.begin(settings, [] { can.isr(); }); - if (errorCodeMCP == 0) { -#ifdef DEBUG_VIA_USB - Serial.println("Can ok"); -#endif - } else { -#ifdef DEBUG_VIA_USB - Serial.print("Error Can: 0x"); - Serial.println(errorCodeMCP, HEX); -#endif - set_event(EVENT_CANMCP_INIT_FAILURE, (uint8_t)errorCodeMCP); - } -#endif - -#ifdef CAN_FD -#ifdef DEBUG_VIA_USB - Serial.println("CAN FD add-on (ESP32+MCP2517) selected"); -#endif - SPI.begin(MCP2517_SCK, MCP2517_SDO, MCP2517_SDI); - ACAN2517FDSettings settings(CAN_FD_CRYSTAL_FREQUENCY_MHZ, 500 * 1000, - DataBitRateFactor::x4); // Arbitration bit rate: 500 kbit/s, data bit rate: 2 Mbit/s -#ifdef USE_CANFD_INTERFACE_AS_CLASSIC_CAN - settings.mRequestedMode = ACAN2517FDSettings::Normal20B; // ListenOnly / Normal20B / NormalFD -#else - settings.mRequestedMode = ACAN2517FDSettings::NormalFD; // ListenOnly / Normal20B / NormalFD -#endif - const uint32_t errorCode = canfd.begin(settings, [] { canfd.isr(); }); - canfd.poll(); - if (errorCode == 0) { -#ifdef DEBUG_VIA_USB - Serial.print("Bit Rate prescaler: "); - Serial.println(settings.mBitRatePrescaler); - Serial.print("Arbitration Phase segment 1: "); - Serial.println(settings.mArbitrationPhaseSegment1); - Serial.print("Arbitration Phase segment 2: "); - Serial.println(settings.mArbitrationPhaseSegment2); - Serial.print("Arbitration SJW:"); - Serial.println(settings.mArbitrationSJW); - Serial.print("Actual Arbitration Bit Rate: "); - Serial.print(settings.actualArbitrationBitRate()); - Serial.println(" bit/s"); - Serial.print("Exact Arbitration Bit Rate ? "); - Serial.println(settings.exactArbitrationBitRate() ? "yes" : "no"); - Serial.print("Arbitration Sample point: "); - Serial.print(settings.arbitrationSamplePointFromBitStart()); - Serial.println("%"); -#endif - } else { -#ifdef DEBUG_VIA_USB - Serial.print("CAN-FD Configuration error 0x"); - Serial.println(errorCode, HEX); -#endif - set_event(EVENT_CANFD_INIT_FAILURE, (uint8_t)errorCode); - } -#endif -} - -void init_contactors() { - // Init contactor pins -#ifdef CONTACTOR_CONTROL -#ifdef PWM_CONTACTOR_CONTROL - // Setup PWM Channel Frequency and Resolution - ledcAttachChannel(POSITIVE_CONTACTOR_PIN, PWM_Freq, PWM_Res, PWM_Positive_Channel); - ledcAttachChannel(NEGATIVE_CONTACTOR_PIN, PWM_Freq, PWM_Res, PWM_Negative_Channel); - // Set all pins OFF (0% PWM) - ledcWrite(POSITIVE_CONTACTOR_PIN, PWM_OFF_DUTY); - ledcWrite(NEGATIVE_CONTACTOR_PIN, PWM_OFF_DUTY); -#else //Normal CONTACTOR_CONTROL - pinMode(POSITIVE_CONTACTOR_PIN, OUTPUT); - set(POSITIVE_CONTACTOR_PIN, OFF); - pinMode(NEGATIVE_CONTACTOR_PIN, OUTPUT); - set(NEGATIVE_CONTACTOR_PIN, OFF); -#endif // Precharge never has PWM regardless of setting - pinMode(PRECHARGE_PIN, OUTPUT); - set(PRECHARGE_PIN, OFF); -#endif //CONTACTOR_CONTROL -#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY - pinMode(SECOND_POSITIVE_CONTACTOR_PIN, OUTPUT); - set(SECOND_POSITIVE_CONTACTOR_PIN, OFF); - pinMode(SECOND_NEGATIVE_CONTACTOR_PIN, OUTPUT); - set(SECOND_NEGATIVE_CONTACTOR_PIN, OFF); -#endif //CONTACTOR_CONTROL_DOUBLE_BATTERY -// Init BMS contactor -#ifdef HW_STARK // TODO: Rewrite this so LilyGo can also handle this BMS contactor - pinMode(BMS_POWER, OUTPUT); - digitalWrite(BMS_POWER, HIGH); -#endif //HW_STARK -} - -void init_rs485() { -// Set up Modbus RTU Server -#ifdef RS485_EN_PIN - pinMode(RS485_EN_PIN, OUTPUT); - digitalWrite(RS485_EN_PIN, HIGH); -#endif -#ifdef RS485_SE_PIN - pinMode(RS485_SE_PIN, OUTPUT); - digitalWrite(RS485_SE_PIN, HIGH); -#endif -#ifdef PIN_5V_EN - pinMode(PIN_5V_EN, OUTPUT); - digitalWrite(PIN_5V_EN, HIGH); -#endif -#ifdef RS485_INVERTER_SELECTED - Serial2.begin(57600, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); -#endif -#ifdef MODBUS_INVERTER_SELECTED -#ifdef BYD_MODBUS - // Init Static data to the RTU Modbus - handle_static_data_modbus_byd(); -#endif - - // Init Serial2 connected to the RTU Modbus - RTUutils::prepareHardwareSerial(Serial2); - Serial2.begin(9600, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); - // Register served function code worker for server - MBserver.registerWorker(MBTCP_ID, READ_HOLD_REGISTER, &FC03); - MBserver.registerWorker(MBTCP_ID, WRITE_HOLD_REGISTER, &FC06); - MBserver.registerWorker(MBTCP_ID, WRITE_MULT_REGISTERS, &FC16); - MBserver.registerWorker(MBTCP_ID, R_W_MULT_REGISTERS, &FC23); - // Start ModbusRTU background task - MBserver.begin(Serial2, MODBUS_CORE); -#endif -} - -#ifdef EQUIPMENT_STOP_BUTTON - -void monitor_equipment_stop_button() { - - ButtonState changed_state = debounceButton(equipment_stop_button, timeSincePress); - - if (equipment_stop_behavior == LATCHING_SWITCH) { - if (changed_state == PRESSED) { - // Changed to ON – initiating equipment stop. - setBatteryPause(true, false, true); - } else if (changed_state == RELEASED) { - // Changed to OFF – ending equipment stop. - setBatteryPause(false, false, false); - } - } else if (equipment_stop_behavior == MOMENTARY_SWITCH) { - if (changed_state == RELEASED) { // button is released - - if (timeSincePress < equipment_button_long_press_duration) { - // Short press detected, trigger equipment stop - setBatteryPause(true, false, true); - } else { - // Long press detected, reset equipment stop state - setBatteryPause(false, false, false); - } - } - } -} - -void init_equipment_stop_button() { - //using external pullup resistors NC - pinMode(EQUIPMENT_STOP_PIN, INPUT); - // Initialize the debounced button with NC switch type and equipment_button_debounce_duration debounce time - initDebouncedButton(equipment_stop_button, EQUIPMENT_STOP_PIN, NC, equipment_button_debounce_duration); -} - -#endif - -enum frameDirection { MSG_RX, MSG_TX }; //RX = 0, TX = 1 -void print_can_frame(CAN_frame frame, frameDirection msgDir); -void print_can_frame(CAN_frame frame, frameDirection msgDir) { -#ifdef DEBUG_CAN_DATA // If enabled in user settings, print out the CAN messages via USB - uint8_t i = 0; - Serial.print("("); - Serial.print(millis() / 1000.0); - (msgDir == MSG_RX) ? Serial.print(") RX0 ") : Serial.print(") TX1 "); - Serial.print(frame.ID, HEX); - Serial.print(" ["); - Serial.print(frame.DLC); - Serial.print("] "); - for (i = 0; i < frame.DLC; i++) { - Serial.print(frame.data.u8[i] < 16 ? "0" : ""); - Serial.print(frame.data.u8[i], HEX); - if (i < frame.DLC - 1) - Serial.print(" "); - } - Serial.println(""); -#endif //#DEBUG_CAN_DATA - - if (datalayer.system.info.can_logging_active) { // If user clicked on CAN Logging page in webserver, start recording - char* message_string = datalayer.system.info.logged_can_messages; - int offset = datalayer.system.info.logged_can_messages_offset; // Keeps track of the current position in the buffer - size_t message_string_size = sizeof(datalayer.system.info.logged_can_messages); - - if (offset + 128 > sizeof(datalayer.system.info.logged_can_messages)) { - // Not enough space, reset and start from the beginning - offset = 0; - } - unsigned long currentTime = millis(); - // Add timestamp - offset += snprintf(message_string + offset, message_string_size - offset, "(%lu.%03lu) ", currentTime / 1000, - currentTime % 1000); - - // Add direction. The 0 and 1 after RX and TX ensures that SavvyCAN puts TX and RX in a different bus. - offset += - snprintf(message_string + offset, message_string_size - offset, "%s ", (msgDir == MSG_RX) ? "RX0" : "TX1"); - - // Add ID and DLC - offset += snprintf(message_string + offset, message_string_size - offset, "%X [%u] ", frame.ID, frame.DLC); - - // Add data bytes - for (uint8_t i = 0; i < frame.DLC; i++) { - if (i < frame.DLC - 1) { - offset += snprintf(message_string + offset, message_string_size - offset, "%02X ", frame.data.u8[i]); - } else { - offset += snprintf(message_string + offset, message_string_size - offset, "%02X", frame.data.u8[i]); - } - } - // Add linebreak - offset += snprintf(message_string + offset, message_string_size - offset, "\n"); - - datalayer.system.info.logged_can_messages_offset = offset; // Update offset in buffer - } -} - -#ifdef CAN_FD -// Functions -void receive_canfd() { // This section checks if we have a complete CAN-FD message incoming - CANFDMessage frame; - int count = 0; - while (canfd.available() && count++ < 16) { - canfd.receive(frame); - - CAN_frame rx_frame; - rx_frame.ID = frame.id; - rx_frame.ext_ID = frame.ext; - rx_frame.DLC = frame.len; - memcpy(rx_frame.data.u8, frame.data, MIN(rx_frame.DLC, 64)); - //message incoming, pass it on to the handler - receive_can(&rx_frame, CAN_ADDON_FD_MCP2518); - receive_can(&rx_frame, CANFD_NATIVE); - } -} -#endif - -void receive_can_native() { // This section checks if we have a complete CAN message incoming on native CAN port - CAN_frame_t rx_frame_native; - if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame_native, 0) == pdTRUE) { - CAN_frame rx_frame; - rx_frame.ID = rx_frame_native.MsgID; - if (rx_frame_native.FIR.B.FF == CAN_frame_std) { - rx_frame.ext_ID = false; - } else { //CAN_frame_ext == 1 - rx_frame.ext_ID = true; - } - rx_frame.DLC = rx_frame_native.FIR.B.DLC; - for (uint8_t i = 0; i < rx_frame.DLC && i < 8; i++) { - rx_frame.data.u8[i] = rx_frame_native.data.u8[i]; - } - //message incoming, pass it on to the handler - receive_can(&rx_frame, CAN_NATIVE); - } -} - -void send_can() { - if (!allowed_to_send_CAN) { - return; - } - - send_can_battery(); - -#ifdef CAN_INVERTER_SELECTED - send_can_inverter(); -#endif // CAN_INVERTER_SELECTED - -#ifdef CHARGER_SELECTED - send_can_charger(); -#endif // CHARGER_SELECTED +#endif // DEBUG_VIA_USB } -#ifdef DUAL_CAN -void receive_can_addonMCP2515() { // This section checks if we have a complete CAN message incoming on add-on CAN port - CAN_frame rx_frame; // Struct with our CAN format - CANMessage MCP2515Frame; // Struct with ACAN2515 library format, needed to use the MCP2515 library - - if (can.available()) { - can.receive(MCP2515Frame); - - rx_frame.ID = MCP2515Frame.id; - rx_frame.ext_ID = MCP2515Frame.ext ? CAN_frame_ext : CAN_frame_std; - rx_frame.DLC = MCP2515Frame.len; - for (uint8_t i = 0; i < MCP2515Frame.len && i < 8; i++) { - rx_frame.data.u8[i] = MCP2515Frame.data[i]; - } - - //message incoming, pass it on to the handler - receive_can(&rx_frame, CAN_ADDON_MCP2515); - } -} -#endif // DUAL_CAN - #ifdef DOUBLE_BATTERY void check_interconnect_available() { if (datalayer.battery.status.voltage_dV == 0 || datalayer.battery2.status.voltage_dV == 0) { @@ -779,121 +303,7 @@ void check_interconnect_available() { set_event(EVENT_VOLTAGE_DIFFERENCE, (uint8_t)(voltage_diff / 10)); } } -#endif //DOUBLE_BATTERY - -void handle_contactors() { -#ifdef BYD_SMA - datalayer.system.status.inverter_allows_contactor_closing = digitalRead(INVERTER_CONTACTOR_ENABLE_PIN); -#endif - -#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY - handle_contactors_battery2(); -#endif - -#ifdef CONTACTOR_CONTROL - // First check if we have any active errors, incase we do, turn off the battery - if (datalayer.battery.status.bms_status == FAULT) { - timeSpentInFaultedMode++; - } else { - timeSpentInFaultedMode = 0; - } - - //handle contactor control SHUTDOWN_REQUESTED - if (timeSpentInFaultedMode > MAX_ALLOWED_FAULT_TICKS) { - contactorStatus = SHUTDOWN_REQUESTED; - } - - if (contactorStatus == SHUTDOWN_REQUESTED) { - set(PRECHARGE_PIN, OFF); - set(NEGATIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); - set(POSITIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); - set_event(EVENT_ERROR_OPEN_CONTACTOR, 0); - datalayer.system.status.contactors_engaged = false; - return; // A fault scenario latches the contactor control. It is not possible to recover without a powercycle (and investigation why fault occured) - } - - // After that, check if we are OK to start turning on the battery - if (contactorStatus == DISCONNECTED) { - set(PRECHARGE_PIN, OFF); - set(NEGATIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); - set(POSITIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); - - if (datalayer.system.status.battery_allows_contactor_closing && - datalayer.system.status.inverter_allows_contactor_closing && !datalayer.system.settings.equipment_stop_active) { - contactorStatus = START_PRECHARGE; - } - } - - // In case the inverter requests contactors to open, set the state accordingly - if (contactorStatus == COMPLETED) { - //Incase inverter (or estop) requests contactors to open, make state machine jump to Disconnected state (recoverable) - if (!datalayer.system.status.inverter_allows_contactor_closing || datalayer.system.settings.equipment_stop_active) { - contactorStatus = DISCONNECTED; - } - // Skip running the state machine below if it has already completed - return; - } - - unsigned long currentTime = millis(); - - if (currentTime < INTERVAL_10_S) { - // Skip running the state machine before system has started up. - // Gives the system some time to detect any faults from battery before blindly just engaging the contactors - return; - } - - // Handle actual state machine. This first turns on Negative, then Precharge, then Positive, and finally turns OFF precharge - switch (contactorStatus) { - case START_PRECHARGE: - set(NEGATIVE_CONTACTOR_PIN, ON, PWM_ON_DUTY); - prechargeStartTime = currentTime; - contactorStatus = PRECHARGE; - break; - - case PRECHARGE: - if (currentTime - prechargeStartTime >= NEGATIVE_CONTACTOR_TIME_MS) { - set(PRECHARGE_PIN, ON); - negativeStartTime = currentTime; - contactorStatus = POSITIVE; - } - break; - - case POSITIVE: - if (currentTime - negativeStartTime >= PRECHARGE_TIME_MS) { - set(POSITIVE_CONTACTOR_PIN, ON, PWM_ON_DUTY); - prechargeCompletedTime = currentTime; - contactorStatus = PRECHARGE_OFF; - } - break; - - case PRECHARGE_OFF: - if (currentTime - prechargeCompletedTime >= PRECHARGE_COMPLETED_TIME_MS) { - set(PRECHARGE_PIN, OFF); - set(NEGATIVE_CONTACTOR_PIN, ON, PWM_HOLD_DUTY); - set(POSITIVE_CONTACTOR_PIN, ON, PWM_HOLD_DUTY); - contactorStatus = COMPLETED; - datalayer.system.status.contactors_engaged = true; - } - break; - default: - break; - } -#endif // CONTACTOR_CONTROL -} - -#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY -void handle_contactors_battery2() { - if ((contactorStatus == COMPLETED) && datalayer.system.status.battery2_allows_contactor_closing) { - set(SECOND_NEGATIVE_CONTACTOR_PIN, ON); - set(SECOND_POSITIVE_CONTACTOR_PIN, ON); - datalayer.system.status.contactors_battery2_engaged = true; - } else { // Closing contactors on secondary battery not allowed - set(SECOND_NEGATIVE_CONTACTOR_PIN, OFF); - set(SECOND_POSITIVE_CONTACTOR_PIN, OFF); - datalayer.system.status.contactors_battery2_engaged = false; - } -} -#endif //CONTACTOR_CONTROL_DOUBLE_BATTERY +#endif // DOUBLE_BATTERY void update_calculated_values() { /* Calculate allowed charge/discharge currents*/ @@ -983,7 +393,7 @@ void update_calculated_values() { } else { datalayer.battery2.status.reported_remaining_capacity_Wh = datalayer.battery2.status.remaining_capacity_Wh; } -#endif +#endif // DOUBLE_BATTERY } else { // soc_scaling_active == false. No SOC window wanted. Set scaled to same as real. datalayer.battery.status.reported_soc = datalayer.battery.status.real_soc; @@ -1012,94 +422,19 @@ void update_calculated_values() { datalayer.battery.status.reported_soc = datalayer.battery2.status.real_soc; datalayer.battery.status.reported_remaining_capacity_Wh = datalayer.battery2.status.remaining_capacity_Wh; } -#endif //DOUBLE_BATTERY +#endif // DOUBLE_BATTERY } void update_values_inverter() { #ifdef CAN_INVERTER_SELECTED update_values_can_inverter(); -#endif +#endif // CAN_INVERTER_SELECTED #ifdef MODBUS_INVERTER_SELECTED update_modbus_registers_inverter(); -#endif +#endif // CAN_INVERTER_SELECTED #ifdef RS485_INVERTER_SELECTED update_RS485_registers_inverter(); -#endif -} - -#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) -void runSerialDataLink() { - static unsigned long updateTime = 0; - unsigned long currentMillis = millis(); - - if ((currentMillis - updateTime) > 1) { //Every 2ms - updateTime = currentMillis; -#ifdef SERIAL_LINK_RECEIVER - manageSerialLinkReceiver(); -#endif -#ifdef SERIAL_LINK_TRANSMITTER - manageSerialLinkTransmitter(); -#endif - } -} -#endif - -void init_serialDataLink() { -#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) - Serial2.begin(SERIAL_LINK_BAUDRATE, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); -#endif -} - -void store_settings_equipment_stop() { - settings.begin("batterySettings", false); - settings.putBool("EQUIPMENT_STOP", datalayer.system.settings.equipment_stop_active); - settings.end(); -} - -void storeSettings() { - // ATTENTION ! The maximum length for settings keys is 15 characters - if (!settings.begin("batterySettings", false)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 0); - return; - } - -#ifdef WIFI - if (!settings.putString("SSID", String(ssid.c_str()))) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 1); - } - if (!settings.putString("PASSWORD", String(password.c_str()))) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 2); - } -#endif - - if (!settings.putUInt("BATTERY_WH_MAX", datalayer.battery.info.total_capacity_Wh)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 3); - } - if (!settings.putBool("USE_SCALED_SOC", datalayer.battery.settings.soc_scaling_active)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 4); - } - if (!settings.putUInt("MAXPERCENTAGE", datalayer.battery.settings.max_percentage / 10)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 5); - } - if (!settings.putUInt("MINPERCENTAGE", datalayer.battery.settings.min_percentage / 10)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 6); - } - if (!settings.putUInt("MAXCHARGEAMP", datalayer.battery.settings.max_user_set_charge_dA)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 7); - } - if (!settings.putUInt("MAXDISCHARGEAMP", datalayer.battery.settings.max_user_set_discharge_dA)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 8); - } - if (!settings.putBool("USEVOLTLIMITS", datalayer.battery.settings.user_set_voltage_limits_active)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 9); - } - if (!settings.putUInt("TARGETCHVOLT", datalayer.battery.settings.max_user_set_charge_voltage_dV)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 10); - } - if (!settings.putUInt("TARGETDISCHVOLT", datalayer.battery.settings.max_user_set_discharge_voltage_dV)) { - set_event(EVENT_PERSISTENT_SAVE_INFO, 11); - } - settings.end(); // Close preferences handle +#endif // CAN_INVERTER_SELECTED } /** Reset reason numbering and description @@ -1178,92 +513,3 @@ void check_reset_reason() { break; } } - -void transmit_can(CAN_frame* tx_frame, int interface) { - if (!allowed_to_send_CAN) { - return; - } - print_can_frame(*tx_frame, frameDirection(MSG_TX)); - - switch (interface) { - case CAN_NATIVE: - CAN_frame_t frame; - frame.MsgID = tx_frame->ID; - frame.FIR.B.FF = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; - frame.FIR.B.DLC = tx_frame->DLC; - frame.FIR.B.RTR = CAN_no_RTR; - for (uint8_t i = 0; i < tx_frame->DLC; i++) { - frame.data.u8[i] = tx_frame->data.u8[i]; - } - ESP32Can.CANWriteFrame(&frame); - break; - case CAN_ADDON_MCP2515: { -#ifdef DUAL_CAN - //Struct with ACAN2515 library format, needed to use the MCP2515 library for CAN2 - CANMessage MCP2515Frame; - MCP2515Frame.id = tx_frame->ID; - MCP2515Frame.ext = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; - MCP2515Frame.len = tx_frame->DLC; - MCP2515Frame.rtr = false; - for (uint8_t i = 0; i < MCP2515Frame.len; i++) { - MCP2515Frame.data[i] = tx_frame->data.u8[i]; - } - can.tryToSend(MCP2515Frame); -#else // Interface not compiled, and settings try to use it - set_event(EVENT_INTERFACE_MISSING, interface); -#endif //DUAL_CAN - } break; - case CANFD_NATIVE: - case CAN_ADDON_FD_MCP2518: { -#ifdef CAN_FD - CANFDMessage MCP2518Frame; - if (tx_frame->FD) { - MCP2518Frame.type = CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH; - } else { //Classic CAN message - MCP2518Frame.type = CANFDMessage::CAN_DATA; - } - MCP2518Frame.id = tx_frame->ID; - MCP2518Frame.ext = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; - MCP2518Frame.len = tx_frame->DLC; - for (uint8_t i = 0; i < MCP2518Frame.len; i++) { - MCP2518Frame.data[i] = tx_frame->data.u8[i]; - } - send_ok = canfd.tryToSend(MCP2518Frame); - if (!send_ok) { - set_event(EVENT_CANFD_BUFFER_FULL, interface); - } -#else // Interface not compiled, and settings try to use it - set_event(EVENT_INTERFACE_MISSING, interface); -#endif //CAN_FD - } break; - default: - // Invalid interface sent with function call. TODO: Raise event that coders messed up - break; - } -} -void receive_can(CAN_frame* rx_frame, int interface) { - - print_can_frame(*rx_frame, frameDirection(MSG_RX)); - - if (interface == can_config.battery) { - receive_can_battery(*rx_frame); -#ifdef CHADEMO_BATTERY - ISA_handleFrame(rx_frame); -#endif - } - if (interface == can_config.inverter) { -#ifdef CAN_INVERTER_SELECTED - receive_can_inverter(*rx_frame); -#endif - } - if (interface == can_config.battery_double) { -#ifdef DOUBLE_BATTERY - receive_can_battery2(*rx_frame); -#endif - } - if (interface == can_config.charger) { -#ifdef CHARGER_SELECTED - receive_can_charger(*rx_frame); -#endif - } -} diff --git a/Software/USER_SETTINGS.cpp b/Software/USER_SETTINGS.cpp index 7e916e68..7c2309b4 100644 --- a/Software/USER_SETTINGS.cpp +++ b/Software/USER_SETTINGS.cpp @@ -9,7 +9,7 @@ CAN_NATIVE = Native CAN port on the LilyGo & Stark hardware CANFD_NATIVE = Native CANFD port on the Stark CMR hardware CAN_ADDON_MCP2515 = Add-on CAN MCP2515 connected to GPIO pins -CAN_ADDON_FD_MCP2518 = Add-on CAN-FD MCP2518 connected to GPIO pins +CANFD_ADDON_MCP2518 = Add-on CAN-FD MCP2518 connected to GPIO pins */ volatile CAN_Configuration can_config = { diff --git a/Software/USER_SETTINGS.h b/Software/USER_SETTINGS.h index 792db0bf..d31582dd 100644 --- a/Software/USER_SETTINGS.h +++ b/Software/USER_SETTINGS.h @@ -34,7 +34,7 @@ //#define TESLA_MODEL_SX_BATTERY //#define VOLVO_SPA_BATTERY //#define TEST_FAKE_BATTERY -//#define DOUBLE_BATTERY //Enable this line if you use two identical batteries at the same time (requires DUAL_CAN setup) +//#define DOUBLE_BATTERY //Enable this line if you use two identical batteries at the same time (requires CAN_ADDON setup) /* Select inverter communication protocol. See Wiki for which to use with your inverter: https://github.com/dalathegreat/BYD-Battery-Emulator-For-Gen24/wiki */ //#define AFORE_CAN //Enable this line to emulate an "Afore battery" over CAN bus @@ -65,17 +65,17 @@ //#define NC_CONTACTORS //Enable this line to control normally closed contactors. CONTACTOR_CONTROL must be enabled for this option. Extremely rare setting! /* Other options */ -//#define DEBUG_VIA_USB //Enable this line to have the USB port output serial diagnostic data while program runs (WARNING, raises CPU load, do not use for production) -//#define DEBUG_CAN_DATA //Enable this line to print incoming/outgoing CAN & CAN-FD messages to USB serial (WARNING, raises CPU load, do not use for production) -//#define INTERLOCK_REQUIRED //Nissan LEAF specific setting, if enabled requires both high voltage conenctors to be seated before starting -//#define DUAL_CAN //Enable this line to activate an isolated secondary CAN Bus using add-on MCP2515 chip (Needed for some inverters / double battery) -#define CRYSTAL_FREQUENCY_MHZ 8 //DUAL_CAN option, what is your MCP2515 add-on boards crystal frequency? -//#define CAN_FD //Enable this line to activate an isolated secondary CAN-FD bus using add-on MCP2518FD chip / Native CANFD on Stark board -#ifdef CAN_FD // CAN_FD additional options if enabled -#define CAN_FD_CRYSTAL_FREQUENCY_MHZ \ - ACAN2517FDSettings:: \ - OSC_40MHz //CAN_FD option, what is your MCP2518 add-on boards crystal frequency? (Default OSC_40MHz) -#endif +//#define DEBUG_VIA_USB //Enable this line to have the USB port output serial diagnostic data while program runs (WARNING, raises CPU load, do not use for production) +//#define DEBUG_CAN_DATA //Enable this line to print incoming/outgoing CAN & CAN-FD messages to USB serial (WARNING, raises CPU load, do not use for production) +//#define INTERLOCK_REQUIRED //Nissan LEAF specific setting, if enabled requires both high voltage conenctors to be seated before starting +//#define CAN_ADDON //Enable this line to activate an isolated secondary CAN Bus using add-on MCP2515 chip (Needed for some inverters / double battery) +#define CRYSTAL_FREQUENCY_MHZ 8 //CAN_ADDON option, what is your MCP2515 add-on boards crystal frequency? +//#define CANFD_ADDON //Enable this line to activate an isolated secondary CAN-FD bus using add-on MCP2518FD chip / Native CANFD on Stark board +#ifdef CANFD_ADDON // CANFD_ADDON additional options if enabled +#define CANFD_ADDON_CRYSTAL_FREQUENCY_MHZ \ + ACAN2517FDSettings:: \ + OSC_40MHz //CANFD_ADDON option, what is your MCP2518 add-on boards crystal frequency? (Default OSC_40MHz) +#endif // CANFD_ADDON //#define USE_CANFD_INTERFACE_AS_CLASSIC_CAN // Enable this line if you intend to use the CANFD as normal CAN //#define SERIAL_LINK_RECEIVER //Enable this line to receive battery data over RS485 pins from another Lilygo (This LilyGo interfaces with inverter) //#define SERIAL_LINK_TRANSMITTER //Enable this line to send battery data over RS485 pins to another Lilygo (This LilyGo interfaces with battery) @@ -138,7 +138,7 @@ /* Do not change any code below this line unless you are sure what you are doing */ /* Only change battery specific settings in "USER_SETTINGS.h" */ -typedef enum { CAN_NATIVE = 0, CANFD_NATIVE = 1, CAN_ADDON_MCP2515 = 2, CAN_ADDON_FD_MCP2518 = 3 } CAN_Interface; +typedef enum { CAN_NATIVE = 0, CANFD_NATIVE = 1, CAN_ADDON_MCP2515 = 2, CANFD_ADDON_MCP2518 = 3 } CAN_Interface; typedef struct { CAN_Interface battery; CAN_Interface inverter; diff --git a/Software/src/communication/can/comm_can.cpp b/Software/src/communication/can/comm_can.cpp new file mode 100644 index 00000000..8d7923fd --- /dev/null +++ b/Software/src/communication/can/comm_can.cpp @@ -0,0 +1,324 @@ +#include "comm_can.h" +#include "../../include.h" + +// Parameters + +CAN_device_t CAN_cfg; // CAN Config +const int rx_queue_size = 10; // Receive Queue size +volatile bool send_ok = 0; + +#ifdef CAN_ADDON +static const uint32_t QUARTZ_FREQUENCY = CRYSTAL_FREQUENCY_MHZ * 1000000UL; //MHZ configured in USER_SETTINGS.h +ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT); +static ACAN2515_Buffer16 gBuffer; +#endif //CAN_ADDON +#ifdef CANFD_ADDON +ACAN2517FD canfd(MCP2517_CS, SPI, MCP2517_INT); +#endif //CANFD_ADDON + +// Initialization functions + +void init_CAN() { +// CAN pins +#ifdef CAN_SE_PIN + pinMode(CAN_SE_PIN, OUTPUT); + digitalWrite(CAN_SE_PIN, LOW); +#endif // CAN_SE_PIN + CAN_cfg.speed = CAN_SPEED_500KBPS; +#ifdef NATIVECAN_250KBPS // Some component is requesting lower CAN speed + CAN_cfg.speed = CAN_SPEED_250KBPS; +#endif // NATIVECAN_250KBPS + CAN_cfg.tx_pin_id = CAN_TX_PIN; + CAN_cfg.rx_pin_id = CAN_RX_PIN; + CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t)); + // Init CAN Module + ESP32Can.CANInit(); + +#ifdef CAN_ADDON +#ifdef DEBUG_VIA_USB + Serial.println("Dual CAN Bus (ESP32+MCP2515) selected"); +#endif // DEBUG_VIA_USB + gBuffer.initWithSize(25); + SPI.begin(MCP2515_SCK, MCP2515_MISO, MCP2515_MOSI); + ACAN2515Settings settings(QUARTZ_FREQUENCY, 500UL * 1000UL); // CAN bit rate 500 kb/s + settings.mRequestedMode = ACAN2515Settings::NormalMode; + const uint16_t errorCodeMCP = can.begin(settings, [] { can.isr(); }); + if (errorCodeMCP == 0) { +#ifdef DEBUG_VIA_USB + Serial.println("Can ok"); +#endif // DEBUG_VIA_USB + } else { +#ifdef DEBUG_VIA_USB + Serial.print("Error Can: 0x"); + Serial.println(errorCodeMCP, HEX); +#endif // DEBUG_VIA_USB + set_event(EVENT_CANMCP_INIT_FAILURE, (uint8_t)errorCodeMCP); + } +#endif // CAN_ADDON + +#ifdef CANFD_ADDON +#ifdef DEBUG_VIA_USB + Serial.println("CAN FD add-on (ESP32+MCP2517) selected"); +#endif // DEBUG_VIA_USB + SPI.begin(MCP2517_SCK, MCP2517_SDO, MCP2517_SDI); + ACAN2517FDSettings settings(CANFD_ADDON_CRYSTAL_FREQUENCY_MHZ, 500 * 1000, + DataBitRateFactor::x4); // Arbitration bit rate: 500 kbit/s, data bit rate: 2 Mbit/s +#ifdef USE_CANFD_INTERFACE_AS_CLASSIC_CAN + settings.mRequestedMode = ACAN2517FDSettings::Normal20B; // ListenOnly / Normal20B / NormalFD +#else // not USE_CANFD_INTERFACE_AS_CLASSIC_CAN + settings.mRequestedMode = ACAN2517FDSettings::NormalFD; // ListenOnly / Normal20B / NormalFD +#endif // USE_CANFD_INTERFACE_AS_CLASSIC_CAN + const uint32_t errorCode = canfd.begin(settings, [] { canfd.isr(); }); + canfd.poll(); + if (errorCode == 0) { +#ifdef DEBUG_VIA_USB + Serial.print("Bit Rate prescaler: "); + Serial.println(settings.mBitRatePrescaler); + Serial.print("Arbitration Phase segment 1: "); + Serial.println(settings.mArbitrationPhaseSegment1); + Serial.print("Arbitration Phase segment 2: "); + Serial.println(settings.mArbitrationPhaseSegment2); + Serial.print("Arbitration SJW:"); + Serial.println(settings.mArbitrationSJW); + Serial.print("Actual Arbitration Bit Rate: "); + Serial.print(settings.actualArbitrationBitRate()); + Serial.println(" bit/s"); + Serial.print("Exact Arbitration Bit Rate ? "); + Serial.println(settings.exactArbitrationBitRate() ? "yes" : "no"); + Serial.print("Arbitration Sample point: "); + Serial.print(settings.arbitrationSamplePointFromBitStart()); + Serial.println("%"); +#endif // DEBUG_VIA_USB + } else { +#ifdef DEBUG_VIA_USB + Serial.print("CAN-FD Configuration error 0x"); + Serial.println(errorCode, HEX); +#endif // DEBUG_VIA_USB + set_event(EVENT_CANFD_INIT_FAILURE, (uint8_t)errorCode); + } +#endif // CANFD_ADDON +} + +// Transmit functions + +void transmit_can(CAN_frame* tx_frame, int interface) { + if (!allowed_to_send_CAN) { + return; + } + print_can_frame(*tx_frame, frameDirection(MSG_TX)); + + switch (interface) { + case CAN_NATIVE: + CAN_frame_t frame; + frame.MsgID = tx_frame->ID; + frame.FIR.B.FF = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; + frame.FIR.B.DLC = tx_frame->DLC; + frame.FIR.B.RTR = CAN_no_RTR; + for (uint8_t i = 0; i < tx_frame->DLC; i++) { + frame.data.u8[i] = tx_frame->data.u8[i]; + } + ESP32Can.CANWriteFrame(&frame); + break; + case CAN_ADDON_MCP2515: { +#ifdef CAN_ADDON + //Struct with ACAN2515 library format, needed to use the MCP2515 library for CAN2 + CANMessage MCP2515Frame; + MCP2515Frame.id = tx_frame->ID; + MCP2515Frame.ext = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; + MCP2515Frame.len = tx_frame->DLC; + MCP2515Frame.rtr = false; + for (uint8_t i = 0; i < MCP2515Frame.len; i++) { + MCP2515Frame.data[i] = tx_frame->data.u8[i]; + } + can.tryToSend(MCP2515Frame); +#else // Interface not compiled, and settings try to use it + set_event(EVENT_INTERFACE_MISSING, interface); +#endif //CAN_ADDON + } break; + case CANFD_NATIVE: + case CANFD_ADDON_MCP2518: { +#ifdef CANFD_ADDON + CANFDMessage MCP2518Frame; + if (tx_frame->FD) { + MCP2518Frame.type = CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH; + } else { //Classic CAN message + MCP2518Frame.type = CANFDMessage::CAN_DATA; + } + MCP2518Frame.id = tx_frame->ID; + MCP2518Frame.ext = tx_frame->ext_ID ? CAN_frame_ext : CAN_frame_std; + MCP2518Frame.len = tx_frame->DLC; + for (uint8_t i = 0; i < MCP2518Frame.len; i++) { + MCP2518Frame.data[i] = tx_frame->data.u8[i]; + } + send_ok = canfd.tryToSend(MCP2518Frame); + if (!send_ok) { + set_event(EVENT_CANFD_BUFFER_FULL, interface); + } +#else // Interface not compiled, and settings try to use it + set_event(EVENT_INTERFACE_MISSING, interface); +#endif //CANFD_ADDON + } break; + default: + // Invalid interface sent with function call. TODO: Raise event that coders messed up + break; + } +} + +void send_can() { + if (!allowed_to_send_CAN) { + return; + } + send_can_battery(); + +#ifdef CAN_INVERTER_SELECTED + send_can_inverter(); +#endif // CAN_INVERTER_SELECTED + +#ifdef CHARGER_SELECTED + send_can_charger(); +#endif // CHARGER_SELECTED +} + +// Receive functions + +void receive_can(CAN_frame* rx_frame, int interface) { + print_can_frame(*rx_frame, frameDirection(MSG_RX)); + + if (interface == can_config.battery) { + receive_can_battery(*rx_frame); +#ifdef CHADEMO_BATTERY + ISA_handleFrame(rx_frame); +#endif + } + if (interface == can_config.inverter) { +#ifdef CAN_INVERTER_SELECTED + receive_can_inverter(*rx_frame); +#endif + } + if (interface == can_config.battery_double) { +#ifdef DOUBLE_BATTERY + receive_can_battery2(*rx_frame); +#endif + } + if (interface == can_config.charger) { +#ifdef CHARGER_SELECTED + receive_can_charger(*rx_frame); +#endif + } +} + +void receive_can_native() { // This section checks if we have a complete CAN message incoming on native CAN port + CAN_frame_t rx_frame_native; + if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame_native, 0) == pdTRUE) { + CAN_frame rx_frame; + rx_frame.ID = rx_frame_native.MsgID; + if (rx_frame_native.FIR.B.FF == CAN_frame_std) { + rx_frame.ext_ID = false; + } else { //CAN_frame_ext == 1 + rx_frame.ext_ID = true; + } + rx_frame.DLC = rx_frame_native.FIR.B.DLC; + for (uint8_t i = 0; i < rx_frame.DLC && i < 8; i++) { + rx_frame.data.u8[i] = rx_frame_native.data.u8[i]; + } + //message incoming, pass it on to the handler + receive_can(&rx_frame, CAN_NATIVE); + } +} + +#ifdef CAN_ADDON +void receive_can_addon() { // This section checks if we have a complete CAN message incoming on add-on CAN port + CAN_frame rx_frame; // Struct with our CAN format + CANMessage MCP2515Frame; // Struct with ACAN2515 library format, needed to use the MCP2515 library + + if (can.available()) { + can.receive(MCP2515Frame); + + rx_frame.ID = MCP2515Frame.id; + rx_frame.ext_ID = MCP2515Frame.ext ? CAN_frame_ext : CAN_frame_std; + rx_frame.DLC = MCP2515Frame.len; + for (uint8_t i = 0; i < MCP2515Frame.len && i < 8; i++) { + rx_frame.data.u8[i] = MCP2515Frame.data[i]; + } + + //message incoming, pass it on to the handler + receive_can(&rx_frame, CAN_ADDON_MCP2515); + } +} +#endif // CAN_ADDON + +#ifdef CANFD_ADDON +// Functions +void receive_canfd_addon() { // This section checks if we have a complete CAN-FD message incoming + CANFDMessage frame; + int count = 0; + while (canfd.available() && count++ < 16) { + canfd.receive(frame); + + CAN_frame rx_frame; + rx_frame.ID = frame.id; + rx_frame.ext_ID = frame.ext; + rx_frame.DLC = frame.len; + memcpy(rx_frame.data.u8, frame.data, MIN(rx_frame.DLC, 64)); + //message incoming, pass it on to the handler + receive_can(&rx_frame, CANFD_ADDON_MCP2518); + receive_can(&rx_frame, CANFD_NATIVE); + } +} +#endif // CANFD_ADDON + +// Support functions +void print_can_frame(CAN_frame frame, frameDirection msgDir) { +#ifdef DEBUG_CAN_DATA // If enabled in user settings, print out the CAN messages via USB + uint8_t i = 0; + Serial.print("("); + Serial.print(millis() / 1000.0); + (msgDir == MSG_RX) ? Serial.print(") RX0 ") : Serial.print(") TX1 "); + Serial.print(frame.ID, HEX); + Serial.print(" ["); + Serial.print(frame.DLC); + Serial.print("] "); + for (i = 0; i < frame.DLC; i++) { + Serial.print(frame.data.u8[i] < 16 ? "0" : ""); + Serial.print(frame.data.u8[i], HEX); + if (i < frame.DLC - 1) + Serial.print(" "); + } + Serial.println(""); +#endif // DEBUG_CAN_DATA + + if (datalayer.system.info.can_logging_active) { // If user clicked on CAN Logging page in webserver, start recording + char* message_string = datalayer.system.info.logged_can_messages; + int offset = datalayer.system.info.logged_can_messages_offset; // Keeps track of the current position in the buffer + size_t message_string_size = sizeof(datalayer.system.info.logged_can_messages); + + if (offset + 128 > sizeof(datalayer.system.info.logged_can_messages)) { + // Not enough space, reset and start from the beginning + offset = 0; + } + unsigned long currentTime = millis(); + // Add timestamp + offset += snprintf(message_string + offset, message_string_size - offset, "(%lu.%03lu) ", currentTime / 1000, + currentTime % 1000); + + // Add direction. The 0 and 1 after RX and TX ensures that SavvyCAN puts TX and RX in a different bus. + offset += + snprintf(message_string + offset, message_string_size - offset, "%s ", (msgDir == MSG_RX) ? "RX0" : "TX1"); + + // Add ID and DLC + offset += snprintf(message_string + offset, message_string_size - offset, "%X [%u] ", frame.ID, frame.DLC); + + // Add data bytes + for (uint8_t i = 0; i < frame.DLC; i++) { + if (i < frame.DLC - 1) { + offset += snprintf(message_string + offset, message_string_size - offset, "%02X ", frame.data.u8[i]); + } else { + offset += snprintf(message_string + offset, message_string_size - offset, "%02X", frame.data.u8[i]); + } + } + // Add linebreak + offset += snprintf(message_string + offset, message_string_size - offset, "\n"); + + datalayer.system.info.logged_can_messages_offset = offset; // Update offset in buffer + } +} diff --git a/Software/src/communication/can/comm_can.h b/Software/src/communication/can/comm_can.h new file mode 100644 index 00000000..48e3c9c3 --- /dev/null +++ b/Software/src/communication/can/comm_can.h @@ -0,0 +1,93 @@ +#ifndef _COMM_CAN_H_ +#define _COMM_CAN_H_ + +#include "../../include.h" + +#include "../../datalayer/datalayer.h" +#include "../../devboard/utils/events.h" +#include "../../devboard/utils/value_mapping.h" +#include "../../lib/me-no-dev-ESPAsyncWebServer/src/ESPAsyncWebServer.h" +#include "../../lib/miwagner-ESP32-Arduino-CAN/ESP32CAN.h" +#ifdef CAN_ADDON +#include "../../lib/pierremolinaro-acan2515/ACAN2515.h" +#endif //CAN_ADDON +#ifdef CANFD_ADDON +#include "../../lib/pierremolinaro-ACAN2517FD/ACAN2517FD.h" +#endif //CANFD_ADDON + +enum frameDirection { MSG_RX, MSG_TX }; //RX = 0, TX = 1 + +/** + * @brief Initialization function for CAN. + * + * @param[in] void + * + * @return void + */ +void init_CAN(); + +/** + * @brief Transmit one CAN frame + * + * @param[in] CAN_frame* tx_frame + * @param[in] int interface + * + * @return void + */ +void transmit_can(); + +/** + * @brief Send CAN messages to all components + * + * @param[in] void + * + * @return void + */ +void send_can(); + +/** + * @brief Receive CAN messages from all interfaces + * + * @param[in] void + * + * @return void + */ +void receive_can(); + +/** + * @brief Receive CAN messages from CAN tranceiver natively installed on Lilygo hardware + * + * @param[in] void + * + * @return void + */ +void receive_can_native(); + +/** + * @brief Receive CAN messages from CAN addon chip + * + * @param[in] void + * + * @return void + */ +void receive_can_addon(); + +/** + * @brief Receive CAN messages from CANFD addon chip + * + * @param[in] void + * + * @return void + */ +void receive_canfd_addon(); + +/** + * @brief print CAN frames via USB + * + * @param[in] void + * + * @return void + */ +void print_can_frame(CAN_frame frame, frameDirection msgDir); + +#endif diff --git a/Software/src/communication/contactorcontrol/comm_contactorcontrol.cpp b/Software/src/communication/contactorcontrol/comm_contactorcontrol.cpp new file mode 100644 index 00000000..c09e6c17 --- /dev/null +++ b/Software/src/communication/contactorcontrol/comm_contactorcontrol.cpp @@ -0,0 +1,204 @@ +#include "comm_contactorcontrol.h" +#include "../../include.h" + +// Parameters +#ifndef CONTACTOR_CONTROL +#ifdef PWM_CONTACTOR_CONTROL +#error CONTACTOR_CONTROL needs to be enabled for PWM_CONTACTOR_CONTROL +#endif +#endif + +#ifdef CONTACTOR_CONTROL +enum State { DISCONNECTED, START_PRECHARGE, PRECHARGE, POSITIVE, PRECHARGE_OFF, COMPLETED, SHUTDOWN_REQUESTED }; +State contactorStatus = DISCONNECTED; + +#define ON 1 +#define OFF 0 + +#ifdef NC_CONTACTORS //Normally closed contactors use inverted logic +#undef ON +#define ON 0 +#undef OFF +#define OFF 1 +#endif //NC_CONTACTORS + +#define MAX_ALLOWED_FAULT_TICKS 1000 +#define NEGATIVE_CONTACTOR_TIME_MS \ + 500 // Time after negative contactor is turned on, to start precharge (not actual precharge time!) +#define PRECHARGE_COMPLETED_TIME_MS \ + 1000 // After successful precharge, resistor is turned off after this delay (and contactors are economized if PWM enabled) +#define PWM_Freq 20000 // 20 kHz frequency, beyond audible range +#define PWM_Res 10 // 10 Bit resolution 0 to 1023, maps 'nicely' to 0% 100% +#define PWM_HOLD_DUTY 250 +#define PWM_OFF_DUTY 0 +#define PWM_ON_DUTY 1023 +#define PWM_Positive_Channel 0 +#define PWM_Negative_Channel 1 +unsigned long prechargeStartTime = 0; +unsigned long negativeStartTime = 0; +unsigned long prechargeCompletedTime = 0; +unsigned long timeSpentInFaultedMode = 0; +#endif + +void set(uint8_t pin, bool direction, uint32_t pwm_freq = 0xFFFF) { +#ifdef PWM_CONTACTOR_CONTROL + if (pwm_freq != 0xFFFF) { + ledcWrite(pin, pwm_freq); + return; + } +#endif + if (direction == 1) { + digitalWrite(pin, HIGH); + } else { // 0 + digitalWrite(pin, LOW); + } +} + +// Initialization functions + +void init_contactors() { + // Init contactor pins +#ifdef CONTACTOR_CONTROL +#ifdef PWM_CONTACTOR_CONTROL + // Setup PWM Channel Frequency and Resolution + ledcAttachChannel(POSITIVE_CONTACTOR_PIN, PWM_Freq, PWM_Res, PWM_Positive_Channel); + ledcAttachChannel(NEGATIVE_CONTACTOR_PIN, PWM_Freq, PWM_Res, PWM_Negative_Channel); + // Set all pins OFF (0% PWM) + ledcWrite(POSITIVE_CONTACTOR_PIN, PWM_OFF_DUTY); + ledcWrite(NEGATIVE_CONTACTOR_PIN, PWM_OFF_DUTY); +#else //Normal CONTACTOR_CONTROL + pinMode(POSITIVE_CONTACTOR_PIN, OUTPUT); + set(POSITIVE_CONTACTOR_PIN, OFF); + pinMode(NEGATIVE_CONTACTOR_PIN, OUTPUT); + set(NEGATIVE_CONTACTOR_PIN, OFF); +#endif // Precharge never has PWM regardless of setting + pinMode(PRECHARGE_PIN, OUTPUT); + set(PRECHARGE_PIN, OFF); +#endif // CONTACTOR_CONTROL +#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY + pinMode(SECOND_POSITIVE_CONTACTOR_PIN, OUTPUT); + set(SECOND_POSITIVE_CONTACTOR_PIN, OFF); + pinMode(SECOND_NEGATIVE_CONTACTOR_PIN, OUTPUT); + set(SECOND_NEGATIVE_CONTACTOR_PIN, OFF); +#endif // CONTACTOR_CONTROL_DOUBLE_BATTERY +// Init BMS contactor +#ifdef HW_STARK // TODO: Rewrite this so LilyGo can also handle this BMS contactor + pinMode(BMS_POWER, OUTPUT); + digitalWrite(BMS_POWER, HIGH); +#endif // HW_STARK +} + +// Main functions +void handle_contactors() { +#ifdef BYD_SMA + datalayer.system.status.inverter_allows_contactor_closing = digitalRead(INVERTER_CONTACTOR_ENABLE_PIN); +#endif // BYD_SMA + +#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY + handle_contactors_battery2(); +#endif // CONTACTOR_CONTROL_DOUBLE_BATTERY + +#ifdef CONTACTOR_CONTROL + // First check if we have any active errors, incase we do, turn off the battery + if (datalayer.battery.status.bms_status == FAULT) { + timeSpentInFaultedMode++; + } else { + timeSpentInFaultedMode = 0; + } + + //handle contactor control SHUTDOWN_REQUESTED + if (timeSpentInFaultedMode > MAX_ALLOWED_FAULT_TICKS) { + contactorStatus = SHUTDOWN_REQUESTED; + } + + if (contactorStatus == SHUTDOWN_REQUESTED) { + set(PRECHARGE_PIN, OFF); + set(NEGATIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); + set(POSITIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); + set_event(EVENT_ERROR_OPEN_CONTACTOR, 0); + datalayer.system.status.contactors_engaged = false; + return; // A fault scenario latches the contactor control. It is not possible to recover without a powercycle (and investigation why fault occured) + } + + // After that, check if we are OK to start turning on the battery + if (contactorStatus == DISCONNECTED) { + set(PRECHARGE_PIN, OFF); + set(NEGATIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); + set(POSITIVE_CONTACTOR_PIN, OFF, PWM_OFF_DUTY); + + if (datalayer.system.status.battery_allows_contactor_closing && + datalayer.system.status.inverter_allows_contactor_closing && !datalayer.system.settings.equipment_stop_active) { + contactorStatus = START_PRECHARGE; + } + } + + // In case the inverter requests contactors to open, set the state accordingly + if (contactorStatus == COMPLETED) { + //Incase inverter (or estop) requests contactors to open, make state machine jump to Disconnected state (recoverable) + if (!datalayer.system.status.inverter_allows_contactor_closing || datalayer.system.settings.equipment_stop_active) { + contactorStatus = DISCONNECTED; + } + // Skip running the state machine below if it has already completed + return; + } + + unsigned long currentTime = millis(); + + if (currentTime < INTERVAL_10_S) { + // Skip running the state machine before system has started up. + // Gives the system some time to detect any faults from battery before blindly just engaging the contactors + return; + } + + // Handle actual state machine. This first turns on Negative, then Precharge, then Positive, and finally turns OFF precharge + switch (contactorStatus) { + case START_PRECHARGE: + set(NEGATIVE_CONTACTOR_PIN, ON, PWM_ON_DUTY); + prechargeStartTime = currentTime; + contactorStatus = PRECHARGE; + break; + + case PRECHARGE: + if (currentTime - prechargeStartTime >= NEGATIVE_CONTACTOR_TIME_MS) { + set(PRECHARGE_PIN, ON); + negativeStartTime = currentTime; + contactorStatus = POSITIVE; + } + break; + + case POSITIVE: + if (currentTime - negativeStartTime >= PRECHARGE_TIME_MS) { + set(POSITIVE_CONTACTOR_PIN, ON, PWM_ON_DUTY); + prechargeCompletedTime = currentTime; + contactorStatus = PRECHARGE_OFF; + } + break; + + case PRECHARGE_OFF: + if (currentTime - prechargeCompletedTime >= PRECHARGE_COMPLETED_TIME_MS) { + set(PRECHARGE_PIN, OFF); + set(NEGATIVE_CONTACTOR_PIN, ON, PWM_HOLD_DUTY); + set(POSITIVE_CONTACTOR_PIN, ON, PWM_HOLD_DUTY); + contactorStatus = COMPLETED; + datalayer.system.status.contactors_engaged = true; + } + break; + default: + break; + } +#endif // CONTACTOR_CONTROL +} + +#ifdef CONTACTOR_CONTROL_DOUBLE_BATTERY +void handle_contactors_battery2() { + if ((contactorStatus == COMPLETED) && datalayer.system.status.battery2_allows_contactor_closing) { + set(SECOND_NEGATIVE_CONTACTOR_PIN, ON); + set(SECOND_POSITIVE_CONTACTOR_PIN, ON); + datalayer.system.status.contactors_battery2_engaged = true; + } else { // Closing contactors on secondary battery not allowed + set(SECOND_NEGATIVE_CONTACTOR_PIN, OFF); + set(SECOND_POSITIVE_CONTACTOR_PIN, OFF); + datalayer.system.status.contactors_battery2_engaged = false; + } +} +#endif // CONTACTOR_CONTROL_DOUBLE_BATTERY diff --git a/Software/src/communication/contactorcontrol/comm_contactorcontrol.h b/Software/src/communication/contactorcontrol/comm_contactorcontrol.h new file mode 100644 index 00000000..2f4e1e0c --- /dev/null +++ b/Software/src/communication/contactorcontrol/comm_contactorcontrol.h @@ -0,0 +1,36 @@ +#ifndef _COMM_CONTACTORCONTROL_H_ +#define _COMM_CONTACTORCONTROL_H_ + +#include "../../include.h" + +#include "../../datalayer/datalayer.h" +#include "../../devboard/utils/events.h" + +/** + * @brief Contactor initialization + * + * @param[in] void + * + * @return void + */ +void init_contactors(); + +/** + * @brief Handle contactors + * + * @param[in] void + * + * @return void + */ +void handle_contactors(); + +/** + * @brief Handle contactors of battery 2 + * + * @param[in] void + * + * @return void + */ +void handle_contactors_battery2(); + +#endif diff --git a/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.cpp b/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.cpp new file mode 100644 index 00000000..2a6a3d7d --- /dev/null +++ b/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.cpp @@ -0,0 +1,51 @@ +#include "comm_equipmentstopbutton.h" +#include "../../include.h" + +// Parameters +#ifdef EQUIPMENT_STOP_BUTTON +const unsigned long equipment_button_long_press_duration = + 15000; // 15 seconds for long press in case of MOMENTARY_SWITCH +const unsigned long equipment_button_debounce_duration = 200; // 200ms for debouncing the button +unsigned long timeSincePress = 0; // Variable to store the time since the last press +DebouncedButton equipment_stop_button; // Debounced button object +#endif // EQUIPMENT_STOP_BUTTON + +// Initialization functions +#ifdef EQUIPMENT_STOP_BUTTON +void init_equipment_stop_button() { + //using external pullup resistors NC + pinMode(EQUIPMENT_STOP_PIN, INPUT); + // Initialize the debounced button with NC switch type and equipment_button_debounce_duration debounce time + initDebouncedButton(equipment_stop_button, EQUIPMENT_STOP_PIN, NC, equipment_button_debounce_duration); +} +#endif // EQUIPMENT_STOP_BUTTON + +// Main functions + +#ifdef EQUIPMENT_STOP_BUTTON +void monitor_equipment_stop_button() { + + ButtonState changed_state = debounceButton(equipment_stop_button, timeSincePress); + + if (equipment_stop_behavior == LATCHING_SWITCH) { + if (changed_state == PRESSED) { + // Changed to ON – initiating equipment stop. + setBatteryPause(true, false, true); + } else if (changed_state == RELEASED) { + // Changed to OFF – ending equipment stop. + setBatteryPause(false, false, false); + } + } else if (equipment_stop_behavior == MOMENTARY_SWITCH) { + if (changed_state == RELEASED) { // button is released + + if (timeSincePress < equipment_button_long_press_duration) { + // Short press detected, trigger equipment stop + setBatteryPause(true, false, true); + } else { + // Long press detected, reset equipment stop state + setBatteryPause(false, false, false); + } + } + } +} +#endif // EQUIPMENT_STOP_BUTTON diff --git a/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.h b/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.h new file mode 100644 index 00000000..f4f5a989 --- /dev/null +++ b/Software/src/communication/equipmentstopbutton/comm_equipmentstopbutton.h @@ -0,0 +1,28 @@ +#ifndef _COMM_EQUIPMENTSTOPBUTTON_H_ +#define _COMM_EQUIPMENTSTOPBUTTON_H_ + +#include "../../include.h" + +#ifdef EQUIPMENT_STOP_BUTTON +#include "../../devboard/utils/debounce_button.h" +#endif + +/** + * @brief Initialization of equipment stop button + * + * @param[in] void + * + * @return void + */ +void init_equipment_stop_button(); + +/** + * @brief Monitor equipment stop button + * + * @param[in] void + * + * @return void + */ +void monitor_equipment_stop_button(); + +#endif diff --git a/Software/src/communication/nvm/comm_nvm.cpp b/Software/src/communication/nvm/comm_nvm.cpp new file mode 100644 index 00000000..03afbee0 --- /dev/null +++ b/Software/src/communication/nvm/comm_nvm.cpp @@ -0,0 +1,125 @@ +#include "comm_nvm.h" +#include "../../include.h" + +// Parameters +Preferences settings; // Store user settings + +// Initialization functions + +void init_stored_settings() { + static uint32_t temp = 0; + // ATTENTION ! The maximum length for settings keys is 15 characters + settings.begin("batterySettings", false); + + // Always get the equipment stop status + datalayer.system.settings.equipment_stop_active = settings.getBool("EQUIPMENT_STOP", false); + if (datalayer.system.settings.equipment_stop_active) { + set_event(EVENT_EQUIPMENT_STOP, 1); + } + +#ifndef LOAD_SAVED_SETTINGS_ON_BOOT + settings.clear(); // If this clear function is executed, no settings will be read from storage + + //always save the equipment stop status + settings.putBool("EQUIPMENT_STOP", datalayer.system.settings.equipment_stop_active); +#endif // LOAD_SAVED_SETTINGS_ON_BOOT + +#ifdef WIFI + char tempSSIDstring[63]; // Allocate buffer with sufficient size + size_t lengthSSID = settings.getString("SSID", tempSSIDstring, sizeof(tempSSIDstring)); + if (lengthSSID > 0) { // Successfully read the string from memory. Set it to SSID! + ssid = tempSSIDstring; + } else { // Reading from settings failed. Do nothing with SSID. Raise event? + } + char tempPasswordString[63]; // Allocate buffer with sufficient size + size_t lengthPassword = settings.getString("PASSWORD", tempPasswordString, sizeof(tempPasswordString)); + if (lengthPassword > 7) { // Successfully read the string from memory. Set it to password! + password = tempPasswordString; + } else { // Reading from settings failed. Do nothing with SSID. Raise event? + } +#endif // WIFI + + temp = settings.getUInt("BATTERY_WH_MAX", false); + if (temp != 0) { + datalayer.battery.info.total_capacity_Wh = temp; + } + temp = settings.getUInt("MAXPERCENTAGE", false); + if (temp != 0) { + datalayer.battery.settings.max_percentage = temp * 10; // Multiply by 10 for backwards compatibility + } + temp = settings.getUInt("MINPERCENTAGE", false); + if (temp != 0) { + datalayer.battery.settings.min_percentage = temp * 10; // Multiply by 10 for backwards compatibility + } + temp = settings.getUInt("MAXCHARGEAMP", false); + if (temp != 0) { + datalayer.battery.settings.max_user_set_charge_dA = temp; + } + temp = settings.getUInt("MAXDISCHARGEAMP", false); + if (temp != 0) { + datalayer.battery.settings.max_user_set_discharge_dA = temp; + } + datalayer.battery.settings.soc_scaling_active = settings.getBool("USE_SCALED_SOC", false); + temp = settings.getUInt("TARGETCHVOLT", false); + if (temp != 0) { + datalayer.battery.settings.max_user_set_charge_voltage_dV = temp; + } + temp = settings.getUInt("TARGETDISCHVOLT", false); + if (temp != 0) { + datalayer.battery.settings.max_user_set_discharge_voltage_dV = temp; + } + datalayer.battery.settings.user_set_voltage_limits_active = settings.getBool("USEVOLTLIMITS", false); + settings.end(); +} + +void store_settings_equipment_stop() { + settings.begin("batterySettings", false); + settings.putBool("EQUIPMENT_STOP", datalayer.system.settings.equipment_stop_active); + settings.end(); +} + +void store_settings() { + // ATTENTION ! The maximum length for settings keys is 15 characters + if (!settings.begin("batterySettings", false)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 0); + return; + } + +#ifdef WIFI + if (!settings.putString("SSID", String(ssid.c_str()))) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 1); + } + if (!settings.putString("PASSWORD", String(password.c_str()))) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 2); + } +#endif + + if (!settings.putUInt("BATTERY_WH_MAX", datalayer.battery.info.total_capacity_Wh)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 3); + } + if (!settings.putBool("USE_SCALED_SOC", datalayer.battery.settings.soc_scaling_active)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 4); + } + if (!settings.putUInt("MAXPERCENTAGE", datalayer.battery.settings.max_percentage / 10)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 5); + } + if (!settings.putUInt("MINPERCENTAGE", datalayer.battery.settings.min_percentage / 10)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 6); + } + if (!settings.putUInt("MAXCHARGEAMP", datalayer.battery.settings.max_user_set_charge_dA)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 7); + } + if (!settings.putUInt("MAXDISCHARGEAMP", datalayer.battery.settings.max_user_set_discharge_dA)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 8); + } + if (!settings.putBool("USEVOLTLIMITS", datalayer.battery.settings.user_set_voltage_limits_active)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 9); + } + if (!settings.putUInt("TARGETCHVOLT", datalayer.battery.settings.max_user_set_charge_voltage_dV)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 10); + } + if (!settings.putUInt("TARGETDISCHVOLT", datalayer.battery.settings.max_user_set_discharge_voltage_dV)) { + set_event(EVENT_PERSISTENT_SAVE_INFO, 11); + } + settings.end(); // Close preferences handle +} diff --git a/Software/src/communication/nvm/comm_nvm.h b/Software/src/communication/nvm/comm_nvm.h new file mode 100644 index 00000000..eb624e89 --- /dev/null +++ b/Software/src/communication/nvm/comm_nvm.h @@ -0,0 +1,37 @@ +#ifndef _COMM_NVM_H_ +#define _COMM_NVM_H_ + +#include "../../include.h" + +#include "../../datalayer/datalayer.h" +#include "../../devboard/utils/events.h" +#include "../../devboard/wifi/wifi.h" + +/** + * @brief Initialization of setting storage + * + * @param[in] void + * + * @return void + */ +void init_stored_settings(); + +/** + * @brief Store settings of equipment stop button + * + * @param[in] void + * + * @return void + */ +void store_settings_equipment_stop(); + +/** + * @brief Store settings + * + * @param[in] void + * + * @return void + */ +void store_settings(); + +#endif diff --git a/Software/src/communication/rs485/comm_rs485.cpp b/Software/src/communication/rs485/comm_rs485.cpp new file mode 100644 index 00000000..feec33aa --- /dev/null +++ b/Software/src/communication/rs485/comm_rs485.cpp @@ -0,0 +1,51 @@ +#include "comm_rs485.h" +#include "../../include.h" + +// Parameters + +#ifdef MODBUS_INVERTER_SELECTED +#define MB_RTU_NUM_VALUES 13100 +uint16_t mbPV[MB_RTU_NUM_VALUES]; // Process variable memory +// Create a ModbusRTU server instance listening on Serial2 with 2000ms timeout +ModbusServerRTU MBserver(Serial2, 2000); +#endif +#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) +#define SERIAL_LINK_BAUDRATE 112500 +#endif + +// Initialization functions + +void init_rs485() { +// Set up Modbus RTU Server +#ifdef RS485_EN_PIN + pinMode(RS485_EN_PIN, OUTPUT); + digitalWrite(RS485_EN_PIN, HIGH); +#endif // RS485_EN_PIN +#ifdef RS485_SE_PIN + pinMode(RS485_SE_PIN, OUTPUT); + digitalWrite(RS485_SE_PIN, HIGH); +#endif // RS485_SE_PIN +#ifdef PIN_5V_EN + pinMode(PIN_5V_EN, OUTPUT); + digitalWrite(PIN_5V_EN, HIGH); +#endif // PIN_5V_EN +#ifdef RS485_INVERTER_SELECTED + Serial2.begin(57600, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); +#endif // RS485_INVERTER_SELECTED +#ifdef MODBUS_INVERTER_SELECTED +#ifdef BYD_MODBUS + // Init Static data to the RTU Modbus + handle_static_data_modbus_byd(); +#endif // BYD_MODBUS + // Init Serial2 connected to the RTU Modbus + RTUutils::prepareHardwareSerial(Serial2); + Serial2.begin(9600, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); + // Register served function code worker for server + MBserver.registerWorker(MBTCP_ID, READ_HOLD_REGISTER, &FC03); + MBserver.registerWorker(MBTCP_ID, WRITE_HOLD_REGISTER, &FC06); + MBserver.registerWorker(MBTCP_ID, WRITE_MULT_REGISTERS, &FC16); + MBserver.registerWorker(MBTCP_ID, R_W_MULT_REGISTERS, &FC23); + // Start ModbusRTU background task + MBserver.begin(Serial2, MODBUS_CORE); +#endif // MODBUS_INVERTER_SELECTED +} diff --git a/Software/src/communication/rs485/comm_rs485.h b/Software/src/communication/rs485/comm_rs485.h new file mode 100644 index 00000000..b4077dd4 --- /dev/null +++ b/Software/src/communication/rs485/comm_rs485.h @@ -0,0 +1,19 @@ +#ifndef _COMM_RS485_H_ +#define _COMM_RS485_H_ + +#include "../../include.h" + +#include "../../lib/eModbus-eModbus/Logging.h" +#include "../../lib/eModbus-eModbus/ModbusServerRTU.h" +#include "../../lib/eModbus-eModbus/scripts/mbServerFCs.h" + +/** + * @brief Initialization of RS485 + * + * @param[in] void + * + * @return void + */ +void init_rs485(); + +#endif diff --git a/Software/src/communication/seriallink/comm_seriallink.cpp b/Software/src/communication/seriallink/comm_seriallink.cpp new file mode 100644 index 00000000..b355ae97 --- /dev/null +++ b/Software/src/communication/seriallink/comm_seriallink.cpp @@ -0,0 +1,35 @@ +#include "comm_seriallink.h" +#include "../../include.h" + +// Parameters + +#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) +#define SERIAL_LINK_BAUDRATE 112500 +#endif + +// Initialization functions + +void init_serialDataLink() { +#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) + Serial2.begin(SERIAL_LINK_BAUDRATE, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN); +#endif // SERIAL_LINK_RECEIVER || SERIAL_LINK_TRANSMITTER +} + +// Main functions + +#if defined(SERIAL_LINK_RECEIVER) || defined(SERIAL_LINK_TRANSMITTER) +void run_serialDataLink() { + static unsigned long updateTime = 0; + unsigned long currentMillis = millis(); + + if ((currentMillis - updateTime) > 1) { //Every 2ms + updateTime = currentMillis; +#ifdef SERIAL_LINK_RECEIVER + manageSerialLinkReceiver(); +#endif +#ifdef SERIAL_LINK_TRANSMITTER + manageSerialLinkTransmitter(); +#endif + } +} +#endif // SERIAL_LINK_RECEIVER || SERIAL_LINK_TRANSMITTER diff --git a/Software/src/communication/seriallink/comm_seriallink.h b/Software/src/communication/seriallink/comm_seriallink.h new file mode 100644 index 00000000..c88b3437 --- /dev/null +++ b/Software/src/communication/seriallink/comm_seriallink.h @@ -0,0 +1,17 @@ +#ifndef _COMM_SERIALLINK_H_ +#define _COMM_SERIALLINK_H_ + +#include "../../include.h" + +/** + * @brief Initialization of serial data link + * + * @param[in] void + * + * @return void + */ +void init_serialDataLink(); + +void run_serialDataLink(); + +#endif diff --git a/Software/src/devboard/hal/hw_3LB.h b/Software/src/devboard/hal/hw_3LB.h index a54de2a4..f4883be7 100644 --- a/Software/src/devboard/hal/hw_3LB.h +++ b/Software/src/devboard/hal/hw_3LB.h @@ -26,14 +26,14 @@ // CAN2 defines below -// DUAL_CAN defines +// CAN_ADDON defines #define MCP2515_SCK 12 // SCK input of MCP2515 #define MCP2515_MOSI 5 // SDI input of MCP2515 #define MCP2515_MISO 34 // SDO output of MCP2515 | Pin 34 is input only, without pullup/down resistors #define MCP2515_CS 18 // CS input of MCP2515 #define MCP2515_INT 35 // INT output of MCP2515 | | Pin 35 is input only, without pullup/down resistors -// CAN_FD defines +// CANFD_ADDON defines #define MCP2517_SCK 17 // SCK input of MCP2517 #define MCP2517_SDI 23 // SDI input of MCP2517 #define MCP2517_SDO 39 // SDO output of MCP2517 @@ -80,17 +80,17 @@ #endif #ifdef CHADEMO_BATTERY -#ifdef DUAL_CAN -#error CHADEMO and DUAL_CAN cannot coexist due to overlapping GPIO pin usage +#ifdef CAN_ADDON +#error CHADEMO and CAN_ADDON cannot coexist due to overlapping GPIO pin usage #endif #endif #ifdef EQUIPMENT_STOP_BUTTON -#ifdef DUAL_CAN -#error EQUIPMENT_STOP_BUTTON and DUAL_CAN cannot coexist due to overlapping GPIO pin usage +#ifdef CAN_ADDON +#error EQUIPMENT_STOP_BUTTON and CAN_ADDON cannot coexist due to overlapping GPIO pin usage #endif -#ifdef CAN_FD -#error EQUIPMENT_STOP_BUTTON and CAN_FD cannot coexist due to overlapping GPIO pin usage +#ifdef CANFD_ADDON +#error EQUIPMENT_STOP_BUTTON and CANFD_ADDON cannot coexist due to overlapping GPIO pin usage #endif #ifdef CHADEMO_BATTERY #error EQUIPMENT_STOP_BUTTON and CHADEMO_BATTERY cannot coexist due to overlapping GPIO pin usage diff --git a/Software/src/devboard/hal/hw_lilygo.h b/Software/src/devboard/hal/hw_lilygo.h index d4d7ce2c..2ea8d260 100644 --- a/Software/src/devboard/hal/hw_lilygo.h +++ b/Software/src/devboard/hal/hw_lilygo.h @@ -26,14 +26,14 @@ // CAN2 defines below -// DUAL_CAN defines +// CAN_ADDON defines #define MCP2515_SCK 12 // SCK input of MCP2515 #define MCP2515_MOSI 5 // SDI input of MCP2515 #define MCP2515_MISO 34 // SDO output of MCP2515 | Pin 34 is input only, without pullup/down resistors #define MCP2515_CS 18 // CS input of MCP2515 #define MCP2515_INT 35 // INT output of MCP2515 | | Pin 35 is input only, without pullup/down resistors -// CAN_FD defines +// CANFD_ADDON defines #define MCP2517_SCK 12 // SCK input of MCP2517 #define MCP2517_SDI 5 // SDI input of MCP2517 #define MCP2517_SDO 34 // SDO output of MCP2517 @@ -76,17 +76,17 @@ #endif #ifdef CHADEMO_BATTERY -#ifdef DUAL_CAN -#error CHADEMO and DUAL_CAN cannot coexist due to overlapping GPIO pin usage +#ifdef CAN_ADDON +#error CHADEMO and CAN_ADDON cannot coexist due to overlapping GPIO pin usage #endif #endif #ifdef EQUIPMENT_STOP_BUTTON -#ifdef DUAL_CAN -#error EQUIPMENT_STOP_BUTTON and DUAL_CAN cannot coexist due to overlapping GPIO pin usage +#ifdef CAN_ADDON +#error EQUIPMENT_STOP_BUTTON and CAN_ADDON cannot coexist due to overlapping GPIO pin usage #endif -#ifdef CAN_FD -#error EQUIPMENT_STOP_BUTTON and CAN_FD cannot coexist due to overlapping GPIO pin usage +#ifdef CANFD_ADDON +#error EQUIPMENT_STOP_BUTTON and CANFD_ADDON cannot coexist due to overlapping GPIO pin usage #endif #ifdef CHADEMO_BATTERY #error EQUIPMENT_STOP_BUTTON and CHADEMO_BATTERY cannot coexist due to overlapping GPIO pin usage diff --git a/Software/src/devboard/hal/hw_stark.h b/Software/src/devboard/hal/hw_stark.h index 5e26f559..f12ea84a 100644 --- a/Software/src/devboard/hal/hw_stark.h +++ b/Software/src/devboard/hal/hw_stark.h @@ -38,7 +38,7 @@ GPIOs on extra header #define CAN_RX_PIN GPIO_NUM_26 // #define CAN_SE_PIN 23 // (No function, GPIO 23 used instead as MCP_SCK) -// CAN_FD defines +// CANFD_ADDON defines #define MCP2517_SCK 17 // SCK input of MCP2517 #define MCP2517_SDI 5 // SDI input of MCP2517 #define MCP2517_SDO 34 // SDO output of MCP2517 diff --git a/Software/src/devboard/webserver/settings_html.cpp b/Software/src/devboard/webserver/settings_html.cpp index 5a26b745..e21ec722 100644 --- a/Software/src/devboard/webserver/settings_html.cpp +++ b/Software/src/devboard/webserver/settings_html.cpp @@ -271,7 +271,7 @@ const char* getCANInterfaceName(CAN_Interface interface) { #endif case CAN_ADDON_MCP2515: return "Add-on CAN via GPIO MCP2515"; - case CAN_ADDON_FD_MCP2518: + case CANFD_ADDON_MCP2518: #ifdef USE_CANFD_INTERFACE_AS_CLASSIC_CAN return "Add-on CAN-FD via GPIO MCP2518 (Classic CAN)"; #else diff --git a/Software/src/devboard/webserver/webserver.cpp b/Software/src/devboard/webserver/webserver.cpp index ff7770ee..5468c704 100644 --- a/Software/src/devboard/webserver/webserver.cpp +++ b/Software/src/devboard/webserver/webserver.cpp @@ -131,7 +131,7 @@ void init_webserver() { String value = request->getParam("value")->value(); if (value.length() <= 63) { // Check if SSID is within the allowable length ssid = value.c_str(); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "SSID must be 63 characters or less"); @@ -148,7 +148,7 @@ void init_webserver() { String value = request->getParam("value")->value(); if (value.length() > 8) { // Check if password is within the allowable length password = value.c_str(); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Password must be atleast 8 characters"); @@ -165,7 +165,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.info.total_capacity_Wh = value.toInt(); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -179,7 +179,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.soc_scaling_active = value.toInt(); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -193,7 +193,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.max_percentage = static_cast(value.toFloat() * 100); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -237,7 +237,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.min_percentage = static_cast(value.toFloat() * 100); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -251,7 +251,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.max_user_set_charge_dA = static_cast(value.toFloat() * 10); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -265,7 +265,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.max_user_set_discharge_dA = static_cast(value.toFloat() * 10); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -279,7 +279,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.user_set_voltage_limits_active = value.toInt(); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -293,7 +293,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.max_user_set_charge_voltage_dV = static_cast(value.toFloat() * 10); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); @@ -307,7 +307,7 @@ void init_webserver() { if (request->hasParam("value")) { String value = request->getParam("value")->value(); datalayer.battery.settings.max_user_set_discharge_voltage_dV = static_cast(value.toFloat() * 10); - storeSettings(); + store_settings(); request->send(200, "text/plain", "Updated successfully"); } else { request->send(400, "text/plain", "Bad Request"); diff --git a/Software/src/devboard/webserver/webserver.h b/Software/src/devboard/webserver/webserver.h index 21416850..a75ef8fb 100644 --- a/Software/src/devboard/webserver/webserver.h +++ b/Software/src/devboard/webserver/webserver.h @@ -104,7 +104,7 @@ void onOTAEnd(bool success); template String formatPowerValue(String label, T value, String unit, int precision, String color = "white"); -extern void storeSettings(); +extern void store_settings(); void ota_monitor(); diff --git a/Software/src/include.h b/Software/src/include.h index f00e2382..5d5520d8 100644 --- a/Software/src/include.h +++ b/Software/src/include.h @@ -22,15 +22,15 @@ #error You must select a HW to run on! #endif -#if defined(DUAL_CAN) && defined(CAN_FD) +#if defined(CAN_ADDON) && defined(CANFD_ADDON) // Check that user did not try to use dual can and fd-can on same hardware pins -#error CAN-FD AND DUAL-CAN CANNOT BE USED SIMULTANEOUSLY +#error CAN_ADDON AND CANFD_ADDON CANNOT BE USED SIMULTANEOUSLY #endif #ifdef USE_CANFD_INTERFACE_AS_CLASSIC_CAN -#if !defined(CAN_FD) +#if !defined(CANFD_ADDON) // Check that user did not try to use classic CAN over FD, without FD component -#error PLEASE ENABLE CAN_FD TO USE CLASSIC CAN OVER CANFD INTERFACE +#error PLEASE ENABLE CANFD_ADDON TO USE CLASSIC CAN OVER CANFD INTERFACE #endif #endif