/* * power_ctrl.c * * Created on: 2025Äê10ÔÂ17ÈÕ * Author: LHYe200 */ #include "power_ctrl.h" #include "zf_driver_pit.h" #include "power_out.h" #include "pid.h" #include "vofa_client.h" #include "status_led.h" #include "math.h" #include "ina226.h" // PID DC_OUT1_PID; // PID DC_AMP1_PID; uint32 timer_counter_1ms = 0; // low_pass_filter_t vot_set_filter; // float vot_set_filtered = 0.0f; // moving_average_filter_t mov_avg_filter; // Kalman_filter_t kalman_filter; // Kalman_filter_t unscented_kalman_filter; // Kalman_filter_t adaptive_kalman_filter; // float kalman_q = 0.0001f; // float kalman_r = 0.10f; void Power_Ctrl_Init(void) { Power_Out_Init(); VOFA_Client_Init(); INA226_Init(); // PID_Init(&DC_OUT1_PID, 3.25, 0.00909, 0.0, 500.0, 36000.0, 24000.0); // PID_Init(&DC_OUT1_PID, 3.25, 0.00909, 0.0, 500.0, 50000.0, -50000.0); // PID_Init(&DC_AMP1_PID, 0.0, 0.0, 0.0, 10.0, 50000.0, -50000.0); // Init_lowPass_alpha(&vot_set_filter, 0.2f / 1000.0f, 10.0f); // Moving_Average_filter_init(&mov_avg_filter, 10); // Kalman_filter_init(&kalman_filter, kalman_q, kalman_r); // Unscented_Kalman_filter_init(&unscented_kalman_filter, kalman_q, kalman_r); // Adaptive_Kalman_filter_init(&adaptive_kalman_filter, kalman_q, kalman_r); pit_us_init(CCU60_CH0, 1000); } void Power_Ctrl_Enable_Output(uint8 out_index, uint8 enable) { if(out_index < POWER_OUT_NUM) { Power_Out_Enable(&power_outs[out_index], enable); } } void Power_Ctrl_Loop_1ms(void) { timer_counter_1ms++; // power_outs[4].set_voltage_V = Low_pass_filter(&vot_set_filter, vot_set_filtered); if(timer_counter_1ms % POWER_OUT_READ_TIME_MS == 0) { Power_Out_Read(); // Moving_Average_filter(&mov_avg_filter, power_outs[4].config.v_adc); // Kalman_filter(&kalman_filter, mov_avg_filter.avg); // Unscented_Kalman_filter(&unscented_kalman_filter, power_outs[4].config.v_adc); // Adaptive_Kalman_filter(&adaptive_kalman_filter, power_outs[4].config.v_adc); } // if(power_outs[4].enabled) // { // // float err = power_outs[4].set_voltage_V - power_outs[4].status.voltage_V; // // float set_vot = (power_outs[4].set_voltage_V - power_outs[4].config.votK[1]) / power_outs[4].config.votK[0]; // // float err = set_vot - power_outs[4].config.vot_adc_filter.lastYn; // // if(err < 0.0f) // // { // // err *= 5.0f; // // } // // PID_Loc_Ctrl(&DC_OUT1_PID, err); // float err = (power_outs[4].set_voltage_V - power_outs[4].config.votK[1]) / power_outs[4].config.votK[0] - kalman_filter.prevData; // if(fabsf(err) < 10.0f) // { // err = 0.0f; // } // else if(fabsf(err) < 282.4f) // { // err = err/fabsf(err)*(expf(fabsf(err)*0.02f)-1.0f); // } // PID_Loc_Ctrl(&DC_OUT1_PID, err); // // PID_Inc_Ctrl(&DC_OUT1_PID,power_outs[4].set_voltage_V - power_outs[4].status.voltage_V); // // if(power_outs[4].status.voltage_V > power_outs[4].set_voltage_V * 1.1f) // // { // // Power_Out_Set_PWM(&power_outs[4], 10000); // // } // // else if(power_outs[4].status.voltage_V < power_outs[4].set_voltage_V * 0.9f) // // { // // Power_Out_Set_PWM(&power_outs[4], 50000); // // } // // else // // { // // Power_Out_Set_PWM(&power_outs[4], 50000 + DC_OUT1_PID.output); // // } // // Power_Out_Set_PWM(&power_outs[4], 50000 + DC_OUT1_PID.output); // // // Power_Out_Set_PWM(&power_outs[4], 10000); // Flash_LED(STATUS_LED_7); // } if(timer_counter_1ms % INA226_READ_TIME_MS == 0) { INA226_Read(); } // if(timer_counter_1ms % 3 == 0) // { // PID_Loc_Ctrl(&DC_OUT1_PID,power_outs[4].set_voltage_V - power_outs[4].status.voltage_V); // } timer_counter_1ms %= (1 * 500); } typedef union { uint8 send[4]; float data; }tmp_float_data; float Power_Vot_Protect_Send_Parser(void) { tmp_float_data protect_status; for(uint8 i = 0; i < POWER_OUT_NUM; i++) { protect_status.send[0] |= (power_outs[i].status.voltage_protect_triggered >= power_outs[i].config.protect_trigger_cycles) << i; } for(uint8 i = 0; i < POWER_OUT_NUM; i++) { protect_status.send[1] |= (power_outs[i].status.current_protect_triggered >= power_outs[i].config.protect_trigger_cycles) << i; } for(uint8 i = 0; i < POWER_OUT_NUM; i++) { protect_status.send[2] |= (power_outs[i].status.power_protect_triggered >= power_outs[i].config.protect_trigger_cycles) << i; } for(uint8 i = 0; i < INA226_NUM; i++) { protect_status.send[3] |= (ina226[i].result.alert_flag) << i; } return protect_status.data; } void Power_Status_Upload(void) { VOFA_Set_JustFloat_Data(0, power_outs[0].status.voltage_V); VOFA_Set_JustFloat_Data(1, power_outs[0].status.current_A); VOFA_Set_JustFloat_Data(2, power_outs[1].status.voltage_V); VOFA_Set_JustFloat_Data(3, power_outs[1].status.current_A); VOFA_Set_JustFloat_Data(4, power_outs[2].status.voltage_V); VOFA_Set_JustFloat_Data(5, power_outs[2].status.current_A); VOFA_Set_JustFloat_Data(6, power_outs[3].status.voltage_V); VOFA_Set_JustFloat_Data(7, power_outs[3].status.current_A); VOFA_Set_JustFloat_Data(8, power_outs[4].status.voltage_V); VOFA_Set_JustFloat_Data(9, power_outs[4].status.current_A); VOFA_Set_JustFloat_Data(10, power_outs[5].status.voltage_V); VOFA_Set_JustFloat_Data(11, power_outs[5].status.current_A); VOFA_Set_JustFloat_Data(12, power_outs[6].status.voltage_V); VOFA_Set_JustFloat_Data(13, power_outs[6].status.current_A); VOFA_Set_JustFloat_Data(14, power_outs[7].status.voltage_V); VOFA_Set_JustFloat_Data(15, power_outs[7].status.current_A); VOFA_Set_JustFloat_Data(16,ina226[0].result.voltage_V); VOFA_Set_JustFloat_Data(17,ina226[0].result.current_A); VOFA_Set_JustFloat_Data(18,ina226[1].result.voltage_V); VOFA_Set_JustFloat_Data(19,ina226[1].result.current_A); VOFA_Set_JustFloat_Data(20,power_outs[0].status.power_W); VOFA_Set_JustFloat_Data(21,power_outs[0].status.energy_J); VOFA_Set_JustFloat_Data(22,power_outs[0].status.quantity_C); VOFA_Set_JustFloat_Data(23,power_outs[1].status.power_W); VOFA_Set_JustFloat_Data(24,power_outs[1].status.energy_J); VOFA_Set_JustFloat_Data(25,power_outs[1].status.quantity_C); VOFA_Set_JustFloat_Data(26,power_outs[2].status.power_W); VOFA_Set_JustFloat_Data(27,power_outs[2].status.energy_J); VOFA_Set_JustFloat_Data(28,power_outs[2].status.quantity_C); VOFA_Set_JustFloat_Data(29,power_outs[3].status.power_W); VOFA_Set_JustFloat_Data(30,power_outs[3].status.energy_J); VOFA_Set_JustFloat_Data(31,power_outs[3].status.quantity_C); VOFA_Set_JustFloat_Data(32,power_outs[4].status.power_W); VOFA_Set_JustFloat_Data(33,power_outs[4].status.energy_J); VOFA_Set_JustFloat_Data(34,power_outs[4].status.quantity_C); VOFA_Set_JustFloat_Data(35,power_outs[5].status.power_W); VOFA_Set_JustFloat_Data(36,power_outs[5].status.energy_J); VOFA_Set_JustFloat_Data(37,power_outs[5].status.quantity_C); VOFA_Set_JustFloat_Data(38,power_outs[6].status.power_W); VOFA_Set_JustFloat_Data(39,power_outs[6].status.energy_J); VOFA_Set_JustFloat_Data(40,power_outs[6].status.quantity_C); VOFA_Set_JustFloat_Data(41,power_outs[7].status.power_W); VOFA_Set_JustFloat_Data(42,power_outs[7].status.energy_J); VOFA_Set_JustFloat_Data(43,power_outs[7].status.quantity_C); VOFA_Set_JustFloat_Data(44,ina226[0].result.power_W); VOFA_Set_JustFloat_Data(45,ina226[0].result.energy_J); VOFA_Set_JustFloat_Data(46,ina226[0].result.quantity_C); VOFA_Set_JustFloat_Data(47,ina226[1].result.power_W); VOFA_Set_JustFloat_Data(48,ina226[1].result.energy_J); VOFA_Set_JustFloat_Data(49,ina226[1].result.quantity_C); float protect_status = Power_Vot_Protect_Send_Parser(); VOFA_Set_JustFloat_Data(50,protect_status); VOFA_Set_JustFloat_Data(51,vofa_last_data); VOFA_Send_Datas(52); } // void Power_Status_Upload(void) // { // VOFA_Set_JustFloat_Data(0, power_outs[0].status.voltage_V); // VOFA_Set_JustFloat_Data(1, power_outs[0].status.current_A); // VOFA_Set_JustFloat_Data(2, power_outs[1].status.voltage_V); // VOFA_Set_JustFloat_Data(3, power_outs[1].status.current_A); // VOFA_Set_JustFloat_Data(4, power_outs[2].status.voltage_V); // VOFA_Set_JustFloat_Data(5, power_outs[2].status.current_A); // VOFA_Set_JustFloat_Data(6, power_outs[3].status.voltage_V); // VOFA_Set_JustFloat_Data(7, power_outs[3].status.current_A); // VOFA_Set_JustFloat_Data(8, power_outs[4].status.voltage_V); // VOFA_Set_JustFloat_Data(9, power_outs[4].status.current_A); // VOFA_Set_JustFloat_Data(10, power_outs[5].status.voltage_V); // VOFA_Set_JustFloat_Data(11, power_outs[5].status.current_A); // VOFA_Set_JustFloat_Data(12, power_outs[6].status.voltage_V); // VOFA_Set_JustFloat_Data(13, power_outs[6].status.current_A); // VOFA_Set_JustFloat_Data(14, power_outs[7].status.voltage_V); // VOFA_Set_JustFloat_Data(15, power_outs[7].status.current_A); // VOFA_Set_JustFloat_Data(16,vofa_last_data); // VOFA_Set_JustFloat_Data(17,DC_OUT1_PID.p_output); // VOFA_Set_JustFloat_Data(18,DC_OUT1_PID.i_output); // VOFA_Set_JustFloat_Data(19,DC_OUT1_PID.d_output); // VOFA_Set_JustFloat_Data(20,power_outs[4].config.vot_adc_filter.lastYn); // VOFA_Set_JustFloat_Data(21,DC_OUT1_PID.output); // VOFA_Set_JustFloat_Data(22,mov_avg_filter.avg); // VOFA_Set_JustFloat_Data(23,kalman_filter.prevData); // VOFA_Set_JustFloat_Data(24,unscented_kalman_filter.prevData); // VOFA_Set_JustFloat_Data(25,adaptive_kalman_filter.prevData); // VOFA_Set_JustFloat_Data(26,power_outs[4].config.v_adc); // VOFA_Send_Datas(27); // } void Power_Vofa_Set(void) { // if(vofa_receive_new_data[1]) // { // vofa_receive_new_data[1] = 0; // DC_OUT1_PID.kp = vofa_data[1]; // } // if(vofa_receive_new_data[2]) // { // vofa_receive_new_data[2] = 0; // DC_OUT1_PID.ki = vofa_data[2]; // } // if(vofa_receive_new_data[3]) // { // vofa_receive_new_data[3] = 0; // DC_OUT1_PID.kd = vofa_data[3]; // } // if(vofa_receive_new_data[4]) // { // vofa_receive_new_data[4] = 0; // power_outs[4].set_voltage_V = vofa_data[4]; // // vot_set_filtered = vofa_data[4]; // } // if(vofa_receive_new_data[5]) // { // vofa_receive_new_data[5] = 0; // Power_Out_Set_PWM(&power_outs[4], vofa_data[5]); // } }