Razvalyaev
9234b4508b
А также: - общий rms ток считается через сумму rms фаз, а не по векторам - в установку угла добавлен аргумент коррекции - пока только смщеение между фазными и линейными. Фильтр практически не смещает, но еще посмотрим - коэф пид приведены к диапзону 0-1 - ПИД надо еще донастраивать. не нравится пока как он работает И еще не были проверены тайминги. Мб это не будет успевать рассчитываться)))
217 lines
7.7 KiB
C
217 lines
7.7 KiB
C
/**
|
||
**************************************************************************
|
||
* @file app_init.h
|
||
* @brief Файл с функциями записи входов/выходов программы МК @ref app_init.
|
||
**************************************************************************/
|
||
#include "mcu_wrapper_conf.h"
|
||
#include "app_wrapper.h"
|
||
|
||
float dbg_err_limit = 0;
|
||
float dbg[16];
|
||
#define PIN_READ(_verbname_) (_verbname_##_GPIO_Port->ODR & (_verbname_##_Pin)) ? 1 : 0
|
||
|
||
void pwm_wtf(PWM_State_t state1, PWM_State_t state2, int* pwm_pin)
|
||
{
|
||
if ((*pwm_pin == 0) && (state1 == PWM_THYR_TIM_ACTIVE))
|
||
{
|
||
*pwm_pin = 1;
|
||
}
|
||
else if ((*pwm_pin == 1) && (state2 == PWM_THYR_TIM_ACTIVE))
|
||
{
|
||
*pwm_pin = 0;
|
||
}
|
||
}
|
||
|
||
int pwm1_pin = 0;
|
||
int pwm2_pin = 0;
|
||
int pwm3_pin = 0;
|
||
int pwm4_pin = 0;
|
||
int pwm5_pin = 0;
|
||
int pwm6_pin = 0;
|
||
void Write_UPP_Outputs(real_T* Buffer, int ind_port)
|
||
{
|
||
//int pwm1_pin = PIN_READ(PWM1);
|
||
//int pwm2_pin = PIN_READ(PWM2);
|
||
//int pwm3_pin = PIN_READ(PWM3);
|
||
//int pwm4_pin = PIN_READ(PWM4);
|
||
//int pwm5_pin = PIN_READ(PWM5);
|
||
//int pwm6_pin = PIN_READ(PWM6);
|
||
int pwm1_pin = (upp.hpwm.AllPhases[PHASE_A_POS].State == PWM_THYR_TIM_ACTIVE);
|
||
int pwm2_pin = (upp.hpwm.AllPhases[PHASE_A_NEG].State == PWM_THYR_TIM_ACTIVE);
|
||
int pwm3_pin = (upp.hpwm.AllPhases[PHASE_B_POS].State == PWM_THYR_TIM_ACTIVE);
|
||
int pwm4_pin = (upp.hpwm.AllPhases[PHASE_B_NEG].State == PWM_THYR_TIM_ACTIVE);
|
||
int pwm5_pin = (upp.hpwm.AllPhases[PHASE_C_POS].State == PWM_THYR_TIM_ACTIVE);
|
||
int pwm6_pin = (upp.hpwm.AllPhases[PHASE_C_NEG].State == PWM_THYR_TIM_ACTIVE);
|
||
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_A_POS].State, upp.hpwm.AllPhases[PHASE_A_NEG].State, &pwm1_pin);
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_A_NEG].State, upp.hpwm.AllPhases[PHASE_A_POS].State, &pwm2_pin);
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_B_POS].State, upp.hpwm.AllPhases[PHASE_B_NEG].State, &pwm3_pin);
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_B_NEG].State, upp.hpwm.AllPhases[PHASE_B_POS].State, &pwm4_pin);
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_C_POS].State, upp.hpwm.AllPhases[PHASE_C_NEG].State, &pwm5_pin);
|
||
//pwm_wtf(upp.hpwm.AllPhases[PHASE_C_NEG].State, upp.hpwm.AllPhases[PHASE_C_POS].State, &pwm6_pin);
|
||
int err = PIN_READ(RDO1);
|
||
int work = PIN_READ(RDO2);
|
||
int ready = upp.errors->common;
|
||
|
||
if (CEN_GPIO_Port->ODR & CEN_Pin)
|
||
{
|
||
WriteOutputArray(0, ind_port, 0);
|
||
WriteOutputArray(0, ind_port, 1);
|
||
WriteOutputArray(0, ind_port, 2);
|
||
WriteOutputArray(0, ind_port, 3);
|
||
WriteOutputArray(0, ind_port, 4);
|
||
WriteOutputArray(0, ind_port, 5);
|
||
|
||
|
||
WriteOutputArray(0, ind_port+1, 0);
|
||
WriteOutputArray(0, ind_port+1, 1);
|
||
WriteOutputArray(0, ind_port+1, 2);
|
||
}
|
||
else
|
||
{
|
||
WriteOutputArray(pwm1_pin, ind_port, 0);
|
||
WriteOutputArray(pwm2_pin, ind_port, 1);
|
||
WriteOutputArray(pwm3_pin, ind_port, 2);
|
||
WriteOutputArray(pwm4_pin, ind_port, 3);
|
||
WriteOutputArray(pwm5_pin, ind_port, 4);
|
||
WriteOutputArray(pwm6_pin, ind_port, 5);
|
||
|
||
WriteOutputArray(ready, ind_port+1, 0);
|
||
WriteOutputArray(work, ind_port+1, 1);
|
||
WriteOutputArray(err, ind_port+1, 2);
|
||
}
|
||
|
||
|
||
}
|
||
|
||
void Write_PowerMonitor(real_T* Buffer, int ind_port)
|
||
{
|
||
int nn = 0;
|
||
for (int i = 0; i < 3; i++)
|
||
{ //0-2
|
||
WriteOutputArray(upp.pm.measured.fast.U[i], ind_port, nn++);
|
||
}
|
||
for (int i = 0; i < 3; i++)
|
||
{ //3-5
|
||
WriteOutputArray(upp.pm.zc.Channel[i].HalfWave, ind_port, nn++);
|
||
}
|
||
for (int i = 0; i < 3; i++)
|
||
{ //6-8
|
||
WriteOutputArray(upp.pm.measured.final.F[i], ind_port, nn++);
|
||
}
|
||
for (int i = 0; i < 3; i++)
|
||
{ //9-11
|
||
WriteOutputArray(upp.pm.measured.fast.I[i], ind_port, nn++);
|
||
}
|
||
for (int i = 0; i < 2; i++)
|
||
{ //12-13
|
||
WriteOutputArray(upp.pm.measured.final.T[i], ind_port, nn++);
|
||
}
|
||
{ //14-19
|
||
WriteOutputArray(upp.pm.measured.final.Uamp, ind_port, nn++);
|
||
WriteOutputArray(upp.pm.measured.final.Iamp, ind_port, nn++);
|
||
WriteOutputArray(upp.pm.measured.final.I[0], ind_port, nn++);
|
||
WriteOutputArray(upp.pm.measured.final.I[1], ind_port, nn++);
|
||
WriteOutputArray(upp.pm.measured.final.I[2], ind_port, nn++);
|
||
WriteOutputArray(upp.pm.measured.final.Fmean, ind_port, nn++);
|
||
}
|
||
|
||
{ //20-21
|
||
WriteOutputArray(upp.pm.isr_cnt, ind_port, nn++);
|
||
WriteOutputArray(upp.pm.slow_cnt%PM_SLOW_PERIOD_CNT, ind_port, nn++);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
void Write_AngleControl(real_T* Buffer, int ind_port)
|
||
{
|
||
int nn = 0;
|
||
|
||
WriteOutputArray(upp.hangle.Iref, ind_port, nn++);
|
||
WriteOutputArray(upp.hangle.Imeas, ind_port, nn++);
|
||
WriteOutputArray(upp.hangle.alpha, ind_port, nn++);
|
||
|
||
|
||
WriteOutputArray((long long)(upp.hangle.htim->Instance->CCR1) - upp.hangle.htim->Instance->CNT, ind_port, nn++);
|
||
WriteOutputArray((long long)(upp.hangle.htim->Instance->CCR2) - upp.hangle.htim->Instance->CNT, ind_port, nn++);
|
||
WriteOutputArray((long long)(upp.hangle.htim->Instance->CCR3) - upp.hangle.htim->Instance->CNT, ind_port, nn++);
|
||
|
||
}
|
||
|
||
/**
|
||
* @brief Функция для записи входов в приложение МК
|
||
* @param u - массив входных значений
|
||
*/
|
||
void app_readInputs(const real_T* Buffer) {
|
||
// USER APP INPUT START
|
||
ADC_Set_Channel_Value(ADC3, 4, ReadInputArray(0,0));
|
||
ADC_Set_Channel_Value(ADC3, 5, ReadInputArray(0,1));
|
||
ADC_Set_Channel_Value(ADC3, 6, ReadInputArray(0,2));
|
||
ADC_Set_Channel_Value(ADC3, 7, ReadInputArray(0,3));
|
||
ADC_Set_Channel_Value(ADC3, 8, ReadInputArray(0,4));
|
||
ADC_Set_Channel_Value(ADC3, 10, ReadInputArray(0,5));
|
||
|
||
upp.call->go = ReadInputArray(1, 0);
|
||
|
||
if (upp.workmode != UPP_Work)
|
||
{
|
||
MB_DATA.HoldRegs.pui_params.Iref = ReadInputArray(1, 1);
|
||
MB_DATA.HoldRegs.pui_params.Tnt = ReadInputArray(1, 2);
|
||
MB_DATA.HoldRegs.pui_params.Umin = ReadInputArray(1, 3);
|
||
MB_DATA.HoldRegs.pui_params.Umax = ReadInputArray(1, 4);
|
||
MB_DATA.HoldRegs.pui_params.Imax = ReadInputArray(1, 5);
|
||
MB_DATA.HoldRegs.pui_params.Imin = ReadInputArray(1, 6);
|
||
MB_DATA.HoldRegs.pui_params.TiMax = ReadInputArray(1, 7);
|
||
MB_DATA.HoldRegs.pui_params.Tdelay = ReadInputArray(1, 8);
|
||
MB_DATA.HoldRegs.pui_params.Interlace = ReadInputArray(1, 9);
|
||
|
||
|
||
MB_INTERNAL.param.adc.ADC_Max[ADC_CHANNEL_UAC] = ReadInputArray(2, 0) * 10;
|
||
MB_INTERNAL.param.adc.ADC_Max[ADC_CHANNEL_UBA] = ReadInputArray(2, 0) * 10;
|
||
MB_INTERNAL.param.adc.ADC_Max[ADC_CHANNEL_IA] = ReadInputArray(2, 1) * 10;
|
||
MB_INTERNAL.param.adc.ADC_Max[ADC_CHANNEL_IC] = ReadInputArray(2, 1) * 10;
|
||
|
||
MB_INTERNAL.param.nominal.U = ReadInputArray(2, 2) * 10;
|
||
MB_INTERNAL.param.nominal.I = ReadInputArray(2, 3) * 10;
|
||
|
||
MB_INTERNAL.param.angle.PID_Kp = ReadInputArray(2, 4) * 65535;
|
||
MB_INTERNAL.param.angle.PID_Ki = ReadInputArray(2, 5) * 65535;
|
||
/*MB_INTERNAL.param.angle.PID_Kd*/dbg_err_limit = ReadInputArray(2, 6);
|
||
|
||
MB_INTERNAL.param.angle.Angle_Max = ReadInputArray(2, 7)/180 * 65535;
|
||
MB_INTERNAL.param.angle.Angle_Min = ReadInputArray(2, 8)/180 * 65535;
|
||
MB_INTERNAL.param.pwm.PulseLength = ReadInputArray(2, 9)/180 * 65535;
|
||
|
||
}
|
||
// USER APP INPUT END
|
||
}
|
||
|
||
/**
|
||
* @brief Функция для записи выходов приложения МК
|
||
* @param xD - массив буффера выходов(дискретных выходов)
|
||
* @details Используте WriteOutputArray(val, arr_ind, val_ind) для записи
|
||
*/
|
||
void app_writeOutputBuffer(real_T* Buffer) {
|
||
// USER APP OUTPUT START
|
||
Write_UPP_Outputs(Buffer, 0);
|
||
|
||
Write_PowerMonitor(Buffer, 2);
|
||
|
||
Write_AngleControl(Buffer, 3);
|
||
|
||
int nn = 0;
|
||
//WriteOutputArray(upp.hangle.htim->Instance->CNT, 2, nn++);
|
||
|
||
//WriteOutputArray(dbg[0], 2, nn++);
|
||
//WriteOutputArray(dbg[1], 2, nn++);
|
||
//WriteOutputArray(dbg[2], 2, nn++);
|
||
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_A_POS].State, 2, nn++);
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_A_NEG].State, 2, nn++);
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_B_POS].State, 2, nn++);
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_B_NEG].State, 2, nn++);
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_C_POS].State, 2, nn++);
|
||
//WriteOutputArray(upp.hpwm.AllPhases[PHASE_C_NEG].State, 2, nn++);
|
||
// USER APP OUTPUT END
|
||
} |