#include "psu_control.h" #include "board.h" #include "can.h" #include "charger_config.h" #include "charger_control.h" #include "debug.h" #include "isr_opt.h" #include "power_setpoint_executor.h" #include #include PSU_02_t PSU_02; PSU_04_t PSU_04; PSU_06_t PSU_06; PSU_08_t PSU_08; PSU_09_t PSU_09; PSU_1A_t PSU_1A; PSU_1B_t PSU_1B; PSU_1C_t PSU_1C; PSU_t PSU0; #define CAN_DELAY 20 #define PSU_VOLTAGE_THRESHOLD 20 // Порог напряжения для определения состояния (В) #define PSU_ONLINE_TIMEOUT 500 // Таймаут для определения состояния (мс) #define PSU_STARTUP_DELAY 4000 // Задержка 2 секунды перед включением #define PSU_DIAG_LOG_MS 2000u #define PSU_SUDDEN_OFF_DEBOUNCE_MS 200u uint32_t can_lastpacket; extern CAN_HandleTypeDef hcan2; static uint16_t psu_last_can_v; static uint16_t psu_last_can_i_0p1A; static uint8_t psu_last_good_s0; static uint8_t psu_last_good_s1; static uint8_t psu_last_good_s2; static void PSU_SendCmd(uint8_t source, uint8_t destination, uint8_t cmd, void *data); static void PSU_LogActiveStatusFlags(LogLevel_t level); static void PSU_MonitorStatusAndUnexpectedOff(void); static const char *PSU_StateName(PSU_State_t state) { switch (state) { case PSU_UNREADY: return "UNREADY"; case PSU_INITIALIZING: return "INIT"; case PSU_READY: return "READY"; case PSU_WAIT_ACK_ON: return "WAIT_ON"; case PSU_CONT_WAIT_ACK_ON: return "CONT_ON"; case PSU_CONNECTED: return "CONNECTED"; case PSU_FAST_DISCHARGE_OFF: return "FD_OFF"; case PSU_FAST_DISCHARGE_WAIT: return "FD_WAIT"; case PSU_FAST_DISCHARGE_SET: return "FD_SET"; case PSU_FAST_DISCHARGE_ON: return "FD_ON"; case PSU_FAST_DISCHARGE_ON_WAIT: return "FD_ON_WAIT"; case PSU_CURRENT_DROP: return "CUR_DROP"; case PSU_CONT_WAIT_ACK_OFF: return "CONT_OFF"; case PSU_WAIT_ACK_OFF: return "WAIT_OFF"; case PSU_OFF_PAUSE: return "OFF_PAUSE"; default: return "?"; } } static void PSU_LogPeriodicDiag(void) { static uint32_t last_log_ms; uint32_t now = HAL_GetTick(); uint8_t block_setpoint; /* Periodic PSU telemetry only while session is in Charging. */ if (CONN.connState != Charging) { return; } if ((now - last_log_ms) < PSU_DIAG_LOG_MS) { return; } last_log_ms = now; #if PSD_ENABLE block_setpoint = PowerSetpoint_HasPending() || PowerSetpoint_IsBusy(); #else block_setpoint = 0u; #endif log_printf(LOG_INFO, "PSU diag st=%s req=%uV/%u.%uA want=%uV/%u.%uA " "can=%uV/%u.%uA meas=%uV/%d.%uA " "enOut=%u out=%u rdy=%u on=%u cont=%u ac=%u hv=%u psd_blk=%u " "err=%u s0=0x%02x s1=0x%02x s2=0x%02x can_age=%lums\n", PSU_StateName(PSU0.state), (unsigned)CONN.RequestedVoltage, (unsigned)(CONN.RequestedCurrent / 10u), (unsigned)(CONN.RequestedCurrent % 10u), (unsigned)CONN.RequestedVoltage, (unsigned)(CONN.WantedCurrent / 10u), (unsigned)(CONN.WantedCurrent % 10u), (unsigned)psu_last_can_v, (unsigned)(psu_last_can_i_0p1A / 10u), (unsigned)(psu_last_can_i_0p1A % 10u), (unsigned)CONN.MeasuredVoltage, (int)(CONN.MeasuredCurrent / 10), (unsigned)(((CONN.MeasuredCurrent < 0) ? -CONN.MeasuredCurrent : CONN.MeasuredCurrent) % 10), (unsigned)CONN.EnableOutput, (unsigned)CONN.outputEnabled, (unsigned)PSU0.ready, (unsigned)PSU0.PSU_enabled, (unsigned)PSU0.CONT_enabled, (unsigned)PSU0.enableAC, (unsigned)CONN.hv_limit, (unsigned)block_setpoint, (unsigned)CONN.chargingError, (unsigned)PSU0.status0.raw, (unsigned)PSU0.status1.raw, (unsigned)PSU0.status2.raw, (unsigned long)(can_lastpacket ? (now - can_lastpacket) : 9999u)); } static void PSU_LogActiveStatusFlags(LogLevel_t level) { PSU_Status0_t s0 = PSU0.status0.bits; PSU_Status1_t s1 = PSU0.status1.bits; PSU_Status2_t s2 = PSU0.status2.bits; log_printf(level, "PSU flags%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", s0.shortCircuitFault ? " sc" : "", s0.unevenFlowAlarm ? " uneven" : "", s0.internalCommunicationFault ? " int_comm" : "", s0.inputBusLineFault ? " in_bus" : "", s0.lockProtection ? " lock" : "", s0.dischargeFault ? " disch" : "", s0.eepromFault ? " eeprom" : "", s1.dcSideOffStatus ? " dc_off" : "", s1.moduleFaultAlarm ? " fault" : "", s1.moduleProtectionAlarm ? " prot" : "", s1.fanFaultAlarm ? " fan" : "", s1.overTempAlarm ? " ot" : "", s1.outputOverVoltageAlarm ? " oov" : "", s1.outputOverCurrentAlarm ? " ooc" : "", s1.canCommunicationInterruptAlarm ? " can_int" : "", s2.powerLimitStatus ? " plim" : "", s2.moduleAddressDuplicate ? " addr_dup" : "", s2.severeUnevenFlowFault ? " uneven_hi" : "", s2.threePhaseInputPhaseLossAlarm ? " ph_loss" : "", s2.threePhaseInputUnbalanceAlarm ? " unbal" : "", s2.inputUnderVoltageAlarm ? " uv" : "", s2.inputOverVoltageAlarm ? " ov" : "", s2.pfcSideOffStatus ? " pfc_off" : ""); } /* * Observability only: does not change PSU/contactor state machine. * Armed only in CONN Charging phase: * - status bit edges while output is requested * - PSU_enabled 1->0 while CONNECTED + EnableOutput (sudden disable) * * Note: PSU_enabled is derived from V >= PSU_VOLTAGE_THRESHOLD, not from * the module enable command bit. */ static void PSU_MonitorStatusAndUnexpectedOff(void) { static uint8_t prev_s0; static uint8_t prev_s1; static uint8_t prev_s2; static uint8_t prev_enabled; static uint8_t inited; static uint8_t sudden_latched; static uint32_t sudden_cand_ms; uint32_t now = HAL_GetTick(); uint8_t in_charging; uint8_t watch_status; uint8_t expect_on; in_charging = (CONN.connState == Charging); watch_status = in_charging && (CONN.EnableOutput != 0) && (PSU0.online != 0) && ((PSU0.state == PSU_CONNECTED) || (PSU0.state == PSU_WAIT_ACK_ON) || (PSU0.state == PSU_CONT_WAIT_ACK_ON)); expect_on = in_charging && (CONN.EnableOutput != 0) && (PSU0.state == PSU_CONNECTED); if (!inited) { prev_s0 = PSU0.status0.raw; prev_s1 = PSU0.status1.raw; prev_s2 = PSU0.status2.raw; prev_enabled = PSU0.PSU_enabled; inited = 1u; return; } if (watch_status) { if ((PSU0.status0.raw != prev_s0) || (PSU0.status1.raw != prev_s1) || (PSU0.status2.raw != prev_s2)) { log_printf(LOG_WARN, "PSU status change s0=0x%02x->0x%02x s1=0x%02x->0x%02x s2=0x%02x->0x%02x\n", (unsigned)prev_s0, (unsigned)PSU0.status0.raw, (unsigned)prev_s1, (unsigned)PSU0.status1.raw, (unsigned)prev_s2, (unsigned)PSU0.status2.raw); PSU_LogActiveStatusFlags(LOG_WARN); } } prev_s0 = PSU0.status0.raw; prev_s1 = PSU0.status1.raw; prev_s2 = PSU0.status2.raw; if (!expect_on) { sudden_cand_ms = 0u; if ((CONN.EnableOutput == 0) || (CONN.connState != Charging) || (PSU0.state == PSU_READY) || (PSU0.state == PSU_UNREADY) || (PSU0.state == PSU_OFF_PAUSE) || (PSU0.state == PSU_WAIT_ACK_OFF) || (PSU0.state == PSU_CONT_WAIT_ACK_OFF) || (PSU0.state == PSU_CURRENT_DROP)) { sudden_latched = 0u; } prev_enabled = PSU0.PSU_enabled; return; } if (PSU0.PSU_enabled) { sudden_cand_ms = 0u; sudden_latched = 0u; prev_enabled = 1u; return; } if (sudden_latched) { prev_enabled = 0u; return; } if (!prev_enabled) { sudden_cand_ms = 0u; return; } /* Falling edge of PSU_enabled while still CONNECTED + EnableOutput + Charging */ if (!PSU0.online) { log_printf(LOG_ERR, "PSU suddenly disabled while charging reason=can_lost " "V=%u I=%d cont=%u s0=0x%02x s1=0x%02x s2=0x%02x can_age=%lums\n", (unsigned)PSU0.outputVoltage, (int)PSU0.outputCurrent, (unsigned)PSU0.CONT_enabled, (unsigned)psu_last_good_s0, (unsigned)psu_last_good_s1, (unsigned)psu_last_good_s2, (unsigned long)(can_lastpacket ? (now - can_lastpacket) : 9999u)); PSU_LogActiveStatusFlags(LOG_ERR); sudden_latched = 1u; sudden_cand_ms = 0u; prev_enabled = 0u; return; } if (sudden_cand_ms == 0u) { sudden_cand_ms = now; return; } if ((now - sudden_cand_ms) < PSU_SUDDEN_OFF_DEBOUNCE_MS) { return; } log_printf(LOG_ERR, "PSU suddenly disabled while charging reason=output_lost " "V=%u I=%d cont=%u s0=0x%02x s1=0x%02x s2=0x%02x can_age=%lums\n", (unsigned)PSU0.outputVoltage, (int)PSU0.outputCurrent, (unsigned)PSU0.CONT_enabled, (unsigned)PSU0.status0.raw, (unsigned)PSU0.status1.raw, (unsigned)PSU0.status2.raw, (unsigned long)(can_lastpacket ? (now - can_lastpacket) : 9999u)); PSU_LogActiveStatusFlags(LOG_ERR); sudden_latched = 1u; sudden_cand_ms = 0u; prev_enabled = 0u; } static void PSU_HvControl(void) { if (CONN.EnableOutput == 0u) { if ((CONN.hv_limit != 0u) || (CONN.hv_limit_count != 0u)) { log_printf(LOG_INFO, "HV limit reset output off\n"); } CONN.hv_limit = 0u; CONN.hv_tick = 0u; CONN.hv_limit_count = 0u; return; } if (!PSU0.online) { if ((CONN.hv_limit != 0u) || (CONN.hv_limit_count != 0u)) { log_printf(LOG_INFO, "HV limit reset no PSU\n"); } CONN.hv_limit = 0u; CONN.hv_tick = 0u; CONN.hv_limit_count = 0u; return; } if (CONN.hv_limit == 0u) { if ((CONN.hv_limit_count < PSU_HV_LIMIT_MAX_COUNT) && (CONN.MeasuredVoltage < PSU_HV_LIMIT_ON_THRESHOLD) && (CONN.MeasuredCurrent > PSU_HV_LOAD_CURRENT)) { if (CONN.hv_tick == 0u) { CONN.hv_tick = HAL_GetTick(); } else if ((HAL_GetTick() - CONN.hv_tick) >= PSU_HV_SWITCH_DELAY_MS) { CONN.hv_limit = 1u; CONN.hv_limit_count++; CONN.hv_tick = 0u; log_printf(LOG_WARN, "HV limit ON V=%d I=%d cnt=%d\n", (int)CONN.MeasuredVoltage, (int)CONN.MeasuredCurrent, (int)CONN.hv_limit_count); if (CONN.hv_limit_count >= PSU_HV_LIMIT_MAX_COUNT) { log_printf(LOG_WARN, "HV limit locked cnt=%d\n", (int)CONN.hv_limit_count); } } } else { CONN.hv_tick = 0u; } } else { if ((CONN.MeasuredVoltage > PSU_HV_LIMIT_OFF_THRESHOLD) || (CONN.MeasuredCurrent <= PSU_HV_LOAD_CURRENT)) { if (CONN.hv_tick == 0u) { CONN.hv_tick = HAL_GetTick(); } else if ((HAL_GetTick() - CONN.hv_tick) >= PSU_HV_SWITCH_DELAY_MS) { CONN.hv_limit = 0u; CONN.hv_tick = 0u; log_printf(LOG_INFO, "HV limit OFF V=%d I=%d cnt=%d\n", (int)CONN.MeasuredVoltage, (int)CONN.MeasuredCurrent, (int)CONN.hv_limit_count); } } else { CONN.hv_tick = 0u; } } } static void PSU_SwitchState(PSU_State_t state){ if (PSU0.state != state) { log_printf(LOG_INFO, "PSU st %s->%s enOut=%u rdy=%u on=%u cont=%u err=%u\n", PSU_StateName(PSU0.state), PSU_StateName(state), (unsigned)CONN.EnableOutput, (unsigned)PSU0.ready, (unsigned)PSU0.PSU_enabled, (unsigned)PSU0.CONT_enabled, (unsigned)CONN.chargingError); } PSU0.state = state; PSU0.statetick = HAL_GetTick(); } static uint32_t PSU_StateTime(void){ return HAL_GetTick() - PSU0.statetick; } static void PSU_FastDischarge_PushTarget(void) { PowerSetpoint_t fd_sp; PowerSetpoint_GetFastDischargeTarget(&fd_sp); PSU_SetVoltageCurrent(0, fd_sp.voltage_V, fd_sp.current_0p1A); PSU_Enable(0, 1); } static void PSU_FastDischarge_AbortToConnected(void) { PowerSetpoint_OnFastDischargeAbort(); if (!PSU0.PSU_enabled) { PSU_FastDischarge_PushTarget(); } PSU_SwitchState(PSU_CONNECTED); } ISR_FAST void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan){ static CAN_RxHeaderTypeDef RxHeader; static uint8_t RxData[8] = {0,}; CanId_t CanId; if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &RxHeader, RxData) == HAL_OK) { memcpy(&CanId, &RxHeader.ExtId, sizeof(CanId_t)); /* Для DC30 поддерживается только один силовой модуль (source == 0) */ if(CanId.source != 0) return; can_lastpacket = HAL_GetTick(); if(CanId.command==0x02){ memcpy(&PSU_02, RxData, 8); } if(CanId.command==0x04){ memcpy(&PSU_04, RxData, 8); PSU0.tempAmbient = PSU_04.moduleTemperature; PSU0.status0.raw = PSU_04.modularForm0; PSU0.status1.raw = PSU_04.modularForm1; PSU0.status2.raw = PSU_04.modularForm2; psu_last_good_s0 = PSU0.status0.raw; psu_last_good_s1 = PSU0.status1.raw; psu_last_good_s2 = PSU0.status2.raw; } if(CanId.command==0x06){ memcpy(&PSU_06, RxData, 8); PSU_06.VAB = PSU_06.VABLo+(PSU_06.VABHi<<8); PSU_06.VBC = PSU_06.VBCLo+(PSU_06.VBCHi<<8); PSU_06.VCA = PSU_06.VCALo+(PSU_06.VCAHi<<8); } if(CanId.command==0x08){ memcpy(&PSU_08, RxData, 8); } if(CanId.command==0x09){ memcpy(&PSU_09, RxData, 8); PSU_09.moduleNCurrent = PSU_09.moduleNCurrent_[3]; PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[2]<<8; PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[1]<<16; PSU_09.moduleNCurrent |= PSU_09.moduleNCurrent_[0]<<24; PSU_09.moduleNVoltage = PSU_09.moduleNVoltage_[3]; PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[2]<<8; PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[1]<<16; PSU_09.moduleNVoltage |= PSU_09.moduleNVoltage_[0]<<24; // PSU_09 -> PSU -> CONN (один модуль) { uint16_t v = PSU_09.moduleNVoltage / 1000; int16_t i = PSU_09.moduleNCurrent / 100; // Обновляем модель PSU0 по телеметрии PSU0.outputVoltage = v; PSU0.outputCurrent = i; PSU0.PSU_enabled = (v >= PSU_VOLTAGE_THRESHOLD); PSU0.online = 1; PSU0.temperature = PSU_04.moduleTemperature; // Экспортируем значения из PSU0 в CONN только, // когда модуль хотя бы в состоянии READY и выше if(PSU0.state >= PSU_READY){ CONN.MeasuredVoltage = PSU0.outputVoltage; CONN.MeasuredCurrent = PSU0.outputCurrent; CONN.Power = CONN.MeasuredCurrent * CONN.MeasuredVoltage / 10; CONN.outputEnabled = PSU0.PSU_enabled; } } } } } void PSU_CAN_FilterInit(){ CAN_FilterTypeDef sFilterConfig; sFilterConfig.FilterBank = 14; sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; sFilterConfig.FilterIdHigh = 0x0000; sFilterConfig.FilterIdLow = 0x0000; sFilterConfig.FilterMaskIdHigh = 0x0000; sFilterConfig.FilterMaskIdLow = 0x0000; sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; sFilterConfig.FilterActivation = ENABLE; sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1; sFilterConfig.SlaveStartFilterBank = 14; if(HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig) != HAL_OK) { Error_Handler(); } } void PSU_Init(){ HAL_CAN_Stop(&hcan2); MX_CAN2_Init(); PSU_CAN_FilterInit(); HAL_CAN_Start(&hcan2); HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING /* | CAN_IT_ERROR | CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE | CAN_IT_TX_MAILBOX_EMPTY*/); memset(&PSU0, 0, sizeof(PSU0)); PSU0.state = PSU_UNREADY; PSU0.statetick = HAL_GetTick(); PSU0.power_limit = PSU_MAX_POWER; // kW PSU_Enable(0, 0); PowerSetpoint_Init(); } void PSU_Enable(uint8_t addr, uint8_t enable){ PSU_1A_t data; memset(&data, 0, sizeof(data)); /* Для DC30 поддерживается только один модуль с адресом 0 */ if(addr != 0) return; if(PSU0.online == 0) return; data.enable = !enable; PSU_SendCmd(0xF0, addr, 0x1A, &data); ED_Delay(CAN_DELAY); } void PSU_SetHVMode(uint8_t addr, uint8_t enable){ PSU_1D_t data; memset(&data, 0, sizeof(data)); data.enable = !enable; if(addr != 0) return; PSU_SendCmd(0xF0, addr, 0x1D, &data); } void PSU_SetVoltageCurrent(uint8_t addr, uint16_t voltage, uint16_t current){ PSU_1C_t data; memset(&data, 0, sizeof(data)); if(addr != 0) return; if(voltage PSU_LV_CLAMP_V)){ voltage = PSU_LV_CLAMP_V; } uint32_t current_ma = current * 100; uint32_t voltage_mv = voltage * 1000; data.moduleCurrentTotal[0] = (current_ma >> 24) & 0xFF; data.moduleCurrentTotal[1] = (current_ma >> 16) & 0xFF; data.moduleCurrentTotal[2] = (current_ma >> 8) & 0xFF; data.moduleCurrentTotal[3] = (current_ma >> 0) & 0xFF; data.moduleVoltage[0] = (voltage_mv >> 24) & 0xFF; data.moduleVoltage[1] = (voltage_mv >> 16) & 0xFF; data.moduleVoltage[2] = (voltage_mv >> 8) & 0xFF; data.moduleVoltage[3] = (voltage_mv >> 0) & 0xFF; psu_last_can_v = voltage; psu_last_can_i_0p1A = current; PSU_SendCmd(0xF0, addr, 0x1C, &data); } void PSU_SendCmd(uint8_t source, uint8_t destination, uint8_t cmd, void *data){ CanId_t CanId; CanId.source = source; CanId.destination = destination; CanId.command = cmd; CanId.device = 0x0A; int8_t retry_counter = 10; CAN_TxHeaderTypeDef tx_header; uint32_t tx_mailbox; HAL_StatusTypeDef CAN_result; memcpy(&tx_header.ExtId, &CanId, sizeof(CanId_t)); tx_header.RTR = CAN_RTR_DATA; tx_header.IDE = CAN_ID_EXT; tx_header.DLC = 8; while(retry_counter>0){ //если буфер полон, ждем пока он освободится if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan2) > 0){ /* отправка сообщения */ CAN_result = HAL_CAN_AddTxMessage(&hcan2, &tx_header, (uint8_t*)data, &tx_mailbox); /* если отправка удалась, выход */ if(CAN_result == HAL_OK) { return; } } ED_Delay(1); retry_counter--; } } void PSU_ReadWrite(){ uint8_t zero_data[8] = {0,0,0,0,0,0,0,0}; PSU_SendCmd(0xF0, 0, 0x04, zero_data);ED_Delay(CAN_DELAY); PSU_SendCmd(0xF0, 0, 0x06, zero_data);ED_Delay(CAN_DELAY); // PSU_SendCmd(0xF0, 0, 0x08, zero_data);ED_Delay(CAN_DELAY); PSU_SendCmd(0xF0, 0, 0x09, zero_data);ED_Delay(CAN_DELAY); // Power Limit if ((CONN.WantedCurrent/10) * CONN.MeasuredVoltage > PSU0.power_limit){ CONN.RequestedCurrent = PSU0.power_limit * 10 / CONN.MeasuredVoltage; }else{ CONN.RequestedCurrent = CONN.WantedCurrent; } if(CONN.RequestedCurrent > (PSU_MAX_CURRENT*10)){ CONN.RequestedCurrent = PSU_MAX_CURRENT*10; } CONN.RequestedPower = CONN.RequestedCurrent * CONN.RequestedVoltage / 10; #if PSD_ENABLE { uint8_t block_setpoint = PowerSetpoint_HasPending() || PowerSetpoint_IsBusy(); if (PSU0.ready && !block_setpoint) { PSU_SetVoltageCurrent(0, CONN.RequestedVoltage, CONN.RequestedCurrent); ED_Delay(CAN_DELAY); } } #else if (PSU0.ready) { PSU_SetVoltageCurrent(0, CONN.RequestedVoltage, CONN.RequestedCurrent); // Normal mode ED_Delay(CAN_DELAY); } #endif if (PSU0.ready) { PSU_HvControl(); } } void PSU_Task(void){ static uint32_t psu_on_tick = 0; static uint32_t cont_ok_tick = 0; static uint32_t fd_on_last_cmd_ms = 0; PowerSetpointTryResult_t psd_result; #if PSD_ENABLE PowerSetpoint_UpdateDeliveryLatch(); if (PowerSetpoint_HasPending()) { psd_result = PowerSetpoint_TryApply(); if (psd_result == PSD_TRY_START_FAST_DISCHARGE) { PSU_SwitchState(PSU_FAST_DISCHARGE_OFF); } } #endif // Обновляем ONLINE/READY по таймауту if((HAL_GetTick() - can_lastpacket) > PSU_ONLINE_TIMEOUT){ if (PSU0.online) { log_printf(LOG_WARN, "PSU CAN timeout age=%lums -> offline\n", (unsigned long)(HAL_GetTick() - can_lastpacket)); } PSU0.online = 0; PSU0.PSU_enabled = 0; PSU_04.moduleTemperature = 0; PSU_04.modularForm0 = 0; PSU_04.modularForm1 = 0; PSU_04.modularForm2 = 0; PSU_06.VAB = 0; PSU_06.VBC = 0; PSU_06.VCA = 0; PSU_09.moduleNCurrent = 0; PSU_09.moduleNVoltage = 0; } if(!PSU0.online || !PSU0.enableAC){ CONN.MeasuredVoltage = 0; CONN.MeasuredCurrent = 0; CONN.outputEnabled = 0; } // Управление AC-контактором с задержкой отключения 1 минута if(CONN.EvConnected){ RELAY_Write(RELAY_AC, 1); psu_on_tick = HAL_GetTick(); PSU0.enableAC = 1; }else{ if((HAL_GetTick() - psu_on_tick) > 1 * 60000){ RELAY_Write(RELAY_AC, 0); PSU0.enableAC = 0; } } // Текущее состояние DC-контактора по обратной связи PSU0.CONT_enabled = IN_ReadInput(IN_CONT_FB_DC); // Обновляем ready с учётом ошибок if(PSU0.online && !PSU0.cont_fault && PSU0.enableAC){ // PSU0.ready = 1; }else{ PSU0.ready = 0; } /* Before state transitions so CONNECTED+can_lost is still visible */ PSU_MonitorStatusAndUnexpectedOff(); switch(PSU0.state){ case PSU_UNREADY: PSU0.enableOutput = 0; RELAY_Write(RELAY_DC, 0); if(PSU0.online && PSU0.enableAC && !PSU0.cont_fault){ PSU_SwitchState(PSU_INITIALIZING); } break; case PSU_INITIALIZING: if(PSU_StateTime() > 4000){ // Wait 4s for PSU to initialize PSU0.ready = 1; PSU_SwitchState(PSU_READY); } break; case PSU_READY: // модуль готов, но выключен CONN.hv_limit = 0u; CONN.hv_tick = 0u; CONN.hv_limit_count = 0u; RELAY_Write(RELAY_DC, 0); if(!PSU0.ready){ PSU_SwitchState(PSU_UNREADY); break; } if(CONN.EnableOutput){ PSU_Enable(0, 1); PSU_SwitchState(PSU_WAIT_ACK_ON); } break; case PSU_WAIT_ACK_ON: if(PSU0.PSU_enabled && PSU0.ready){ PSU_SwitchState(PSU_CONT_WAIT_ACK_ON); }else if(PSU_StateTime() > 10000){ PSU0.psu_fault = 1; CONN.chargingError = CONN_ERR_PSU_FAULT; PSU_SwitchState(PSU_UNREADY); log_printf(LOG_ERR, "PSU on timeout\n"); } break; case PSU_CONT_WAIT_ACK_ON: // замыкаем DC-контактор и ждём подтверждение RELAY_Write(RELAY_DC, 1); if(PSU0.CONT_enabled){ PSU_SwitchState(PSU_CONNECTED); }else if(PSU_StateTime() > 1000){ PSU0.cont_fault = 1; CONN.chargingError = CONN_ERR_CONTACTOR; PSU_SwitchState(PSU_CURRENT_DROP); log_printf(LOG_ERR, "Contactor error, stopping...\n"); } break; case PSU_CONNECTED: // Основное рабочее состояние if(!CONN.EnableOutput || !PSU0.ready){ PSU_SwitchState(PSU_CURRENT_DROP); break; } // контроль контактора: 1 c таймаут if (IN_ReadInput(IN_CONT_FB_DC) != RELAY_Read(RELAY_DC)){ if((HAL_GetTick() - cont_ok_tick) > 1000){ CONN.chargingError = CONN_ERR_CONTACTOR; PSU0.cont_fault = 1; PSU_SwitchState(PSU_CURRENT_DROP); log_printf(LOG_ERR, "Contactor error, stopping...\n"); } }else{ cont_ok_tick = HAL_GetTick(); } break; case PSU_FAST_DISCHARGE_OFF: if (!CONN.EnableOutput || !PSU0.ready || PSU0.cont_fault || PSU0.psu_fault) { log_printf(LOG_WARN, "Fast discharge abort -> stop\n"); PowerSetpoint_OnFastDischargeAbort(); PSU_SwitchState(PSU_CURRENT_DROP); break; } PSU_Enable(0, 0); PSU_SwitchState(PSU_FAST_DISCHARGE_WAIT); break; case PSU_FAST_DISCHARGE_WAIT: { PowerSetpoint_t fd_sp; if (!CONN.EnableOutput || !PSU0.ready || PSU0.cont_fault || PSU0.psu_fault) { log_printf(LOG_WARN, "Fast discharge abort -> stop\n"); PowerSetpoint_OnFastDischargeAbort(); PSU_SwitchState(PSU_CURRENT_DROP); break; } /* Как PSU_WAIT_ACK_OFF: 0–20 V на телеметрии = модуль выключен */ if (!PSU0.PSU_enabled) { PowerSetpoint_GetFastDischargeTarget(&fd_sp); log_printf(LOG_INFO, "Fast discharge off ack, V=%u -> set %u\n", (unsigned)CONN.MeasuredVoltage, (unsigned)fd_sp.voltage_V); PSU_SwitchState(PSU_FAST_DISCHARGE_SET); } else if (PSU_StateTime() > PSD_OFF_TIMEOUT_MS) { log_printf(LOG_WARN, "Fast discharge off timeout, V=%u\n", (unsigned)CONN.MeasuredVoltage); PSU_FastDischarge_AbortToConnected(); } break; } case PSU_FAST_DISCHARGE_SET: { PowerSetpoint_t fd_sp; if (!CONN.EnableOutput || !PSU0.ready || PSU0.cont_fault || PSU0.psu_fault) { log_printf(LOG_WARN, "Fast discharge abort -> stop\n"); PowerSetpoint_OnFastDischargeAbort(); PSU_SwitchState(PSU_CURRENT_DROP); break; } PowerSetpoint_GetFastDischargeTarget(&fd_sp); PSU_SetVoltageCurrent(0, fd_sp.voltage_V, fd_sp.current_0p1A); PSU_SwitchState(PSU_FAST_DISCHARGE_ON); break; } case PSU_FAST_DISCHARGE_ON: if (!CONN.EnableOutput || !PSU0.ready || PSU0.cont_fault || PSU0.psu_fault) { log_printf(LOG_WARN, "Fast discharge abort -> stop\n"); PowerSetpoint_OnFastDischargeAbort(); PSU_SwitchState(PSU_CURRENT_DROP); break; } PSU_Enable(0, 1); fd_on_last_cmd_ms = HAL_GetTick(); PSU_SwitchState(PSU_FAST_DISCHARGE_ON_WAIT); break; case PSU_FAST_DISCHARGE_ON_WAIT: { PowerSetpoint_t fd_sp; uint16_t v_low; if (!CONN.EnableOutput || !PSU0.ready || PSU0.cont_fault || PSU0.psu_fault) { log_printf(LOG_WARN, "Fast discharge abort -> stop\n"); PowerSetpoint_OnFastDischargeAbort(); PSU_SwitchState(PSU_CURRENT_DROP); break; } PowerSetpoint_GetFastDischargeTarget(&fd_sp); v_low = (fd_sp.voltage_V > PSD_PRECHARGE_TOLERANCE_V) ? (uint16_t)(fd_sp.voltage_V - PSD_PRECHARGE_TOLERANCE_V) : 0u; if (PSU0.PSU_enabled && CONN.MeasuredVoltage >= v_low) { PowerSetpoint_OnFastDischargeComplete(); PSU_SwitchState(PSU_CONNECTED); } else { if ((HAL_GetTick() - fd_on_last_cmd_ms) >= PSD_ON_RETRY_MS) { PSU_FastDischarge_PushTarget(); fd_on_last_cmd_ms = HAL_GetTick(); } if (PSU_StateTime() > PSD_ON_TIMEOUT_MS) { log_printf(LOG_WARN, "Fast discharge on timeout, V=%u (target %u)\n", (unsigned)CONN.MeasuredVoltage, (unsigned)fd_sp.voltage_V); PSU_FastDischarge_AbortToConnected(); } } break; } case PSU_CURRENT_DROP: // снижаем ток до нуля перед отключением DC CONN.RequestedCurrent = 0; // если ток действительно упал или вышло время, отключаем DC if((CONN.MeasuredCurrent < 30) || (PSU_StateTime() > 5000)){ PSU_SwitchState(PSU_CONT_WAIT_ACK_OFF); } break; case PSU_CONT_WAIT_ACK_OFF: RELAY_Write(RELAY_DC, 0); if(!PSU0.CONT_enabled){ PSU_Enable(0, 0); PSU_SwitchState(PSU_WAIT_ACK_OFF); }else if(PSU_StateTime() > 1000){ PSU0.cont_fault = 1; CONN.chargingError = CONN_ERR_CONTACTOR; PSU_Enable(0, 0); PSU_SwitchState(PSU_WAIT_ACK_OFF); log_printf(LOG_ERR, "Contactor error, stopping...\n"); } break; case PSU_WAIT_ACK_OFF: if(!PSU0.PSU_enabled){ PSU_SwitchState(PSU_OFF_PAUSE); }else if(PSU_StateTime() > 10000){ PSU0.psu_fault = 1; CONN.chargingError = CONN_ERR_PSU_FAULT; PSU_SwitchState(PSU_UNREADY); log_printf(LOG_ERR, "PSU off timeout\n"); } break; case PSU_OFF_PAUSE: if(PSU_StateTime() > 4000){ PSU_SwitchState(PSU_READY); } break; default: PSU_SwitchState(PSU_UNREADY); break; } PSU_LogPeriodicDiag(); }