forked from achamaikin/CCSModuleSW30Web
451 lines
12 KiB
C
Executable File
451 lines
12 KiB
C
Executable File
|
||
#include <psu_control.h>
|
||
|
||
#include "can.h"
|
||
#include "string.h"
|
||
#include "stdio.h"
|
||
#include "charger_config.h"
|
||
#include "charger_control.h"
|
||
#include "board.h"
|
||
#include "debug.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 секунды перед включением
|
||
|
||
uint32_t can_lastpacket;
|
||
|
||
extern CAN_HandleTypeDef hcan2;
|
||
|
||
static void PSU_SwitchState(PSU_State_t state){
|
||
PSU0.state = state;
|
||
PSU0.statetick = HAL_GetTick();
|
||
}
|
||
|
||
static uint32_t PSU_StateTime(void){
|
||
return HAL_GetTick() - PSU0.statetick;
|
||
}
|
||
|
||
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
|
||
PSU0.hv_mode = 0;
|
||
|
||
PSU_Enable(0, 0);
|
||
}
|
||
|
||
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((PSU0.hv_mode==0) && voltage>499) voltage = 499;
|
||
|
||
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_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;
|
||
retry_counter = 0;
|
||
}
|
||
}
|
||
ED_Delay(1);
|
||
|
||
retry_counter--;
|
||
}
|
||
|
||
}
|
||
|
||
uint32_t max(uint32_t a, uint32_t b){
|
||
if(a>b) return a;
|
||
else return b;
|
||
}
|
||
|
||
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(PSU0.ready){
|
||
if (CONN.RequestedVoltage == 500) { // fake
|
||
PSU_SetVoltageCurrent(0, 300, 10); // Normal mode
|
||
}else{
|
||
PSU_SetVoltageCurrent(0, CONN.RequestedVoltage, CONN.RequestedCurrent); // Normal mode
|
||
}
|
||
ED_Delay(CAN_DELAY);
|
||
if(CONN.MeasuredVoltage>490) PSU0.hv_mode = 1;
|
||
}
|
||
|
||
// PSU_SetHVMode(0, PSU0.hv_mode); // auto set, no need
|
||
// ED_Delay(CAN_DELAY);
|
||
|
||
}
|
||
|
||
void PSU_Task(void){
|
||
static uint32_t psu_on_tick = 0;
|
||
static uint32_t dc_on_tick = 0;
|
||
static uint32_t cont_ok_tick = 0;
|
||
|
||
// Обновляем ONLINE/READY по таймауту
|
||
if((HAL_GetTick() - can_lastpacket) > PSU_ONLINE_TIMEOUT){
|
||
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:
|
||
// модуль готов, но выключен
|
||
PSU0.hv_mode = 0;
|
||
|
||
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){
|
||
dc_on_tick = HAL_GetTick();
|
||
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_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;
|
||
}
|
||
}
|
||
|
||
|