forked from achamaikin/CCSModuleSW30Web
739 lines
21 KiB
C
Executable File
739 lines
21 KiB
C
Executable File
#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 <stdio.h>
|
||
#include <string.h>
|
||
|
||
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
|
||
|
||
uint32_t can_lastpacket;
|
||
|
||
extern CAN_HandleTypeDef hcan2;
|
||
|
||
static uint16_t psu_last_can_v;
|
||
static uint16_t psu_last_can_i_0p1A;
|
||
|
||
static void PSU_SendCmd(uint8_t source, uint8_t destination, uint8_t cmd, void *data);
|
||
|
||
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;
|
||
|
||
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_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;
|
||
}
|
||
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_MIN_VOLTAGE) voltage = PSU_MIN_VOLTAGE;
|
||
|
||
if((CONN.hv_limit != 0u) && (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;
|
||
}
|
||
|
||
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();
|
||
}
|
||
|
||
|