529 lines
16 KiB
C
529 lines
16 KiB
C
// #include "project.h"
|
||
#include "adc_tools.h"
|
||
// #include "xp_project.h"
|
||
#include "IQmathLib.h"
|
||
#include "math.h"
|
||
#include "my_filter.h"
|
||
#include "params.h"
|
||
// #include "params_protect.h"
|
||
#include "vector.h"
|
||
// #include "xp_adc.h"
|
||
// #include "TuneUpPlane.h" //Ìîðãàíèå ñâåòîäèîäîì
|
||
// #include "log_to_memory.h"
|
||
// #include "errors.h"
|
||
|
||
// #define COUNT_ARR_ADC_BUF 2
|
||
#define Shift_Filter 1 //2
|
||
|
||
#define BTR_ENABLED
|
||
|
||
#define R_ADC_DEFAULT { 1180 ,1180 , 256, 256, 256, 256, 256, 1180, 1180, 256, 256, 256, 256, 256, 256, 256, 256, 256 }
|
||
#define K_LEM_ADC_DEFAULT { 60000,60000,5000, 5000,5000,5000,5000,60000,60000,5000,5000,5000,5000,5000,5000,5000,5000,5000 }
|
||
//#define LOG_ACP_TO_BUF
|
||
|
||
int ADC_f[COUNT_ARR_ADC_BUF][16];
|
||
|
||
int ADC_sf[COUNT_ARR_ADC_BUF][16];
|
||
|
||
|
||
#define ZERO_ADC_DEFAULT {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048}
|
||
_iq19 iq19_zero_ADC[18] = ZERO_ADC_DEFAULT;
|
||
|
||
int zero_ADC[18] = ZERO_ADC_DEFAULT;
|
||
_iq iq_k_norm_ADC[18];
|
||
_iq19 iq19_k_norm_ADC[18];
|
||
|
||
_iq iq_norm_ADC[18];
|
||
|
||
unsigned int const R_ADC[18] = R_ADC_DEFAULT;
|
||
unsigned int const K_LEM_ADC[18] = K_LEM_ADC_DEFAULT;
|
||
|
||
//#pragma DATA_SECTION(analog,".fast_vars");
|
||
ANALOG_VALUE analog;
|
||
//#pragma DATA_SECTION(filter,".fast_vars");
|
||
ANALOG_VALUE filter;
|
||
|
||
ANALOG_RAW_DATA rawData = ANALOG_RAW_DATA_DEFAULT;
|
||
|
||
_iq koef_Ud_long_filter=0;
|
||
_iq koef_Ud_fast_filter=0;
|
||
_iq koef_Im_filter=0;
|
||
|
||
_iq koef_Iabc_filter=0;
|
||
|
||
_iq koef_Wlong=0;
|
||
|
||
|
||
void calc_norm_ADC(void);
|
||
void analog_values_calc(void);
|
||
_iq im_calc( _iq ia, _iq ib, _iq ic);
|
||
|
||
void read_adc(ANALOG_RAW_DATA *data)
|
||
{
|
||
ADC_f[0][0] = data->U1_1;
|
||
ADC_f[0][1] = data->U1_2;
|
||
ADC_f[0][2] = data->Izpt1_1;
|
||
ADC_f[0][3] = data->Izpt1_2;
|
||
ADC_f[0][4] = data->Ia1;
|
||
ADC_f[0][5] = data->Ib1;
|
||
ADC_f[0][6] = data->Ic1;
|
||
ADC_f[0][7] = 0;
|
||
ADC_f[0][8] = data->U2_1;
|
||
ADC_f[0][9] = data->U2_2;
|
||
ADC_f[0][10] = data->Izpt2_1;
|
||
ADC_f[0][11] = data->Izpt2_2;
|
||
ADC_f[0][12] = data->Ia2;
|
||
ADC_f[0][13] = data->Ib2;
|
||
ADC_f[0][14] = data->Ic2;
|
||
ADC_f[0][15] = 0;
|
||
|
||
|
||
ADC_sf[0][0] += (ADC_f[0][0] - ADC_sf[0][0]) >> Shift_Filter;
|
||
ADC_sf[0][1] += (ADC_f[0][1] - ADC_sf[0][1]) >> Shift_Filter;
|
||
ADC_sf[0][2] += (ADC_f[0][2] - ADC_sf[0][2]) >> Shift_Filter;
|
||
ADC_sf[0][3] += (ADC_f[0][3] - ADC_sf[0][3]) >> Shift_Filter;
|
||
ADC_sf[0][4] += (ADC_f[0][4] - ADC_sf[0][4]) >> Shift_Filter;
|
||
ADC_sf[0][5] += (ADC_f[0][5] - ADC_sf[0][5]) >> Shift_Filter;
|
||
ADC_sf[0][6] += (ADC_f[0][6] - ADC_sf[0][6]) >> Shift_Filter;
|
||
|
||
ADC_sf[0][7] += (ADC_f[0][7] - ADC_sf[0][7]) >> Shift_Filter;
|
||
ADC_sf[0][8] += (ADC_f[0][8] - ADC_sf[0][8]) >> Shift_Filter;
|
||
ADC_sf[0][9] += (ADC_f[0][9] - ADC_sf[0][9]) >> Shift_Filter;
|
||
ADC_sf[0][10] += (ADC_f[0][10] - ADC_sf[0][10]) >> Shift_Filter;
|
||
ADC_sf[0][11] += (ADC_f[0][11] - ADC_sf[0][11]) >> Shift_Filter;
|
||
ADC_sf[0][12] += (ADC_f[0][12] - ADC_sf[0][12]) >> Shift_Filter;
|
||
ADC_sf[0][13] += (ADC_f[0][13] - ADC_sf[0][13]) >> Shift_Filter;
|
||
ADC_sf[0][14] += (ADC_f[0][14] - ADC_sf[0][14]) >> Shift_Filter;
|
||
ADC_sf[0][15] += (ADC_f[0][15] - ADC_sf[0][15]) >> Shift_Filter;
|
||
|
||
|
||
ADC_f[1][0] = 0;
|
||
ADC_f[1][1] = 0;
|
||
ADC_f[1][2] = 0;
|
||
ADC_f[1][3] = 0;
|
||
|
||
ADC_sf[1][0] += (ADC_f[1][0] - ADC_sf[1][0]) >> Shift_Filter;
|
||
ADC_sf[1][1] += (ADC_f[1][1] - ADC_sf[1][1]) >> Shift_Filter;
|
||
ADC_sf[1][2] += (ADC_f[1][2] - ADC_sf[1][2]) >> Shift_Filter;
|
||
ADC_sf[1][3] += (ADC_f[1][3] - ADC_sf[1][3]) >> Shift_Filter;
|
||
}
|
||
|
||
void acp_Handler(void)
|
||
{
|
||
// read_adc();
|
||
calc_norm_ADC();
|
||
analog_values_calc();
|
||
// Read_Fast_Errors();
|
||
}
|
||
|
||
#define COUNT_DETECT_ZERO 500
|
||
|
||
// void detect_zero_analog()
|
||
// {
|
||
// int i, k;
|
||
// double tmp_buff[2][16] = {0};
|
||
// // _iq koef_zero_ADC_filter = 1;
|
||
// for(i = 0; i < 16; tmp_buff[0][i++] = 0);
|
||
// for(i = 0; i < 16; tmp_buff[1][i++] = 0);
|
||
// i = 0;
|
||
// for (i = 0; i<COUNT_DETECT_ZERO; i++)
|
||
// {
|
||
// read_adc();
|
||
|
||
// for(k = 2; k < 15; k++)
|
||
// {
|
||
// if((k == 7) || (k == 8))
|
||
// {
|
||
// continue;
|
||
// }
|
||
// tmp_buff[0][k] += ADC_f[0][k];
|
||
// }
|
||
// for(k = 0;k < 4; k++)
|
||
// {
|
||
// tmp_buff[1][k] += ADC_f[1][k];
|
||
// }
|
||
// pause_1000(1000);
|
||
// }
|
||
// k = 2;
|
||
// for(i = 2; i < 15; i++)
|
||
// {
|
||
// if(i == 7)
|
||
// {
|
||
// continue;
|
||
// }
|
||
|
||
// if((i == 8) || (i == 9))
|
||
// {
|
||
// k++;
|
||
// continue;
|
||
// }
|
||
// zero_ADC[k] = (int)(tmp_buff[0][i] / COUNT_DETECT_ZERO + 0.5);
|
||
// k++;
|
||
// }
|
||
// for(i = 0; i < 4; i++, k++)
|
||
// {
|
||
// zero_ADC[k] = (int)(tmp_buff[1][i] / COUNT_DETECT_ZERO + 0.5);
|
||
// }
|
||
// zero_ADC[0] = zero_ADC[2];
|
||
// zero_ADC[1] = zero_ADC[2];
|
||
// zero_ADC[7] = zero_ADC[9];
|
||
// zero_ADC[8] = zero_ADC[9];
|
||
|
||
|
||
// }
|
||
|
||
void init_Adc_Variables(void)
|
||
{
|
||
unsigned int k;
|
||
int *panalog, *pfilter;
|
||
|
||
for (k=0;k<16;k++)
|
||
{
|
||
|
||
ADC_f[0][k]=0;
|
||
ADC_sf[0][k]=0;
|
||
ADC_f[1][k]=0;
|
||
ADC_sf[1][k]=0;
|
||
}
|
||
|
||
// detect_zero_analog();
|
||
|
||
for (k=0;k<18;k++)
|
||
{
|
||
iq19_k_norm_ADC[k] = _IQ19(K_LEM_ADC[k]*250.0/R_ADC[k]/NORMA_ACP/4096.0);
|
||
|
||
iq19_zero_ADC[k]=_IQ19(zero_ADC[k]); //_IQ19(2030);//_IQ19(1770);
|
||
|
||
}
|
||
|
||
koef_Ud_long_filter = _IQ(0.05);
|
||
koef_Ud_fast_filter = _IQ(0.2); // _IQ(0.001/0.00931); //_IQ(0.001/0.00131);
|
||
|
||
koef_Im_filter = _IQ(0.015);
|
||
|
||
koef_Iabc_filter = _IQ(0.0005); //_IQ(0.002/0.006);
|
||
koef_Wlong = _IQ(0.001);
|
||
|
||
panalog = (int*)&analog;
|
||
pfilter = (int*)&filter;
|
||
for (k=0;k < sizeof(ANALOG_VALUE) / sizeof(int) ;k++)
|
||
{
|
||
|
||
*(panalog + k) = 0;
|
||
*(pfilter + k) = 0;
|
||
}
|
||
|
||
}
|
||
|
||
inline _iq norma_adc(int plane, int chan, int n_norm)
|
||
{
|
||
// return _IQ19toIQ(_IQ19mpy((iq19_zero_ADC[n_norm] - ((long)ADC_sf[plane][chan]<<19) ),iq19_k_norm_ADC[n_norm]));
|
||
// return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[plane][chan]<<19) - iq19_zero_ADC[n_norm]),iq19_k_norm_ADC[n_norm]));
|
||
return _IQ19toIQ(_IQ19mpy((((long)ADC_sf[plane][chan]<<19)),iq19_k_norm_ADC[n_norm]));
|
||
}
|
||
|
||
void calc_norm_ADC(void)
|
||
{
|
||
iq_norm_ADC[0] = norma_adc(0, 0, 0);
|
||
iq_norm_ADC[1] = norma_adc(0, 1, 1);
|
||
iq_norm_ADC[2] = norma_adc(0, 2, 2);
|
||
iq_norm_ADC[3] = norma_adc(0, 3, 3);
|
||
iq_norm_ADC[4] = norma_adc(0, 4, 4);
|
||
iq_norm_ADC[5] = norma_adc(0, 5, 5);
|
||
iq_norm_ADC[6] = norma_adc(0, 6, 6);
|
||
iq_norm_ADC[7] = norma_adc(0, 8, 7);
|
||
iq_norm_ADC[8] = norma_adc(0, 9, 8);
|
||
iq_norm_ADC[9] = norma_adc(0, 10, 9);
|
||
iq_norm_ADC[10] = norma_adc(0, 11, 10);
|
||
iq_norm_ADC[11] = norma_adc(0, 12, 11);
|
||
iq_norm_ADC[12] = norma_adc(0, 13, 12);
|
||
iq_norm_ADC[13] = norma_adc(0, 14, 13);
|
||
iq_norm_ADC[14] = norma_adc(1, 0, 14);
|
||
iq_norm_ADC[15] = norma_adc(1, 1, 15);
|
||
iq_norm_ADC[16] = norma_adc(1, 2, 16);
|
||
iq_norm_ADC[17] = norma_adc(1, 3, 17);
|
||
}
|
||
|
||
void analog_values_calc(void)
|
||
{
|
||
|
||
analog.iqU_1 = iq_norm_ADC[0];
|
||
analog.iqU_2 = iq_norm_ADC[1];
|
||
analog.iqU_3 = iq_norm_ADC[7];
|
||
analog.iqU_4 = iq_norm_ADC[8];
|
||
|
||
analog.iqIin_1 = iq_norm_ADC[2];
|
||
analog.iqIin_2 = -iq_norm_ADC[3];
|
||
analog.iqIin_3 = iq_norm_ADC[9];
|
||
analog.iqIin_4 = -iq_norm_ADC[10];
|
||
|
||
|
||
// #if PROJECT_ARKTIKA == 1
|
||
analog.iqIa1_1 = iq_norm_ADC[4];
|
||
analog.iqIb1_1 = iq_norm_ADC[5];
|
||
analog.iqIc1_1 = iq_norm_ADC[6];
|
||
|
||
analog.iqIa2_1 = iq_norm_ADC[11];
|
||
analog.iqIb2_1 = iq_norm_ADC[12];
|
||
analog.iqIc2_1 = iq_norm_ADC[13];
|
||
// #endif
|
||
#if PROJECT_SIBERIA == 1
|
||
analog.iqIa1_1 = iq_norm_ADC[6];
|
||
analog.iqIb1_1 = iq_norm_ADC[5];
|
||
analog.iqIc1_1 = iq_norm_ADC[4];
|
||
|
||
analog.iqIa2_1 = iq_norm_ADC[13];
|
||
analog.iqIb2_1 = iq_norm_ADC[12];
|
||
analog.iqIc2_1 = iq_norm_ADC[11];
|
||
#endif
|
||
|
||
analog.iqIbtr1_1 = labs(iq_norm_ADC[14]);
|
||
analog.iqIbtr1_2 = labs(iq_norm_ADC[16]);
|
||
analog.iqIbtr2_1 = labs(iq_norm_ADC[15]);
|
||
analog.iqIbtr2_2 = labs(iq_norm_ADC[17]);
|
||
|
||
analog.iqIa1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIa1_rms, _IQabs(analog.iqIa1_1));
|
||
analog.iqIb1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIb1_rms, _IQabs(analog.iqIb1_1));
|
||
analog.iqIc1_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIc1_rms, _IQabs(analog.iqIc1_1));
|
||
|
||
analog.iqIa2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIa2_rms, _IQabs(analog.iqIa2_1));
|
||
analog.iqIb2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIb2_rms, _IQabs(analog.iqIb2_1));
|
||
analog.iqIc2_rms = exp_regul_iq(koef_Iabc_filter, analog.iqIc2_rms, _IQabs(analog.iqIc2_1));
|
||
|
||
|
||
filter.iqU_1_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_1_long, analog.iqU_1);
|
||
filter.iqU_2_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_2_long, analog.iqU_2);
|
||
filter.iqU_3_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_3_long, analog.iqU_3);
|
||
filter.iqU_4_long = exp_regul_iq(koef_Ud_long_filter, filter.iqU_4_long, analog.iqU_4);
|
||
|
||
filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1);
|
||
filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2);
|
||
filter.iqIin_3 = exp_regul_iq(koef_Im_filter, filter.iqIin_3, analog.iqIin_3);
|
||
filter.iqIin_4 = exp_regul_iq(koef_Im_filter, filter.iqIin_4, analog.iqIin_4);
|
||
|
||
|
||
|
||
|
||
filter.iqU_1_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_1_fast, analog.iqU_1);
|
||
filter.iqU_2_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_2_fast, analog.iqU_2);
|
||
|
||
filter.iqU_3_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_3_fast, analog.iqU_3);
|
||
filter.iqU_4_fast = exp_regul_iq(koef_Ud_fast_filter, filter.iqU_4_fast, analog.iqU_4);
|
||
|
||
filter.iqIa1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIa1_1, analog.iqIa1_1);
|
||
filter.iqIb1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIb1_1, analog.iqIb1_1);
|
||
filter.iqIc1_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIc1_1, analog.iqIc1_1);
|
||
|
||
filter.iqIa2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIa2_1, analog.iqIa2_1);
|
||
filter.iqIb2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIb2_1, analog.iqIb2_1);
|
||
filter.iqIc2_1 = exp_regul_iq(koef_Ud_fast_filter, filter.iqIc2_1, analog.iqIc2_1);
|
||
|
||
|
||
|
||
analog.iqW1 = _IQmpy(filter.iqU_1_fast, analog.iqIin_1) + _IQmpy(filter.iqU_2_fast, analog.iqIin_2);
|
||
analog.iqW2 = _IQmpy(filter.iqU_3_fast, analog.iqIin_3) + _IQmpy(filter.iqU_4_fast, analog.iqIin_4);
|
||
analog.iqW = analog.iqW1 + analog.iqW2;
|
||
|
||
|
||
|
||
analog.iqIm_1 = im_calc(filter.iqIa1_1, filter.iqIb1_1, filter.iqIc1_1);
|
||
analog.iqIm_2 = im_calc(filter.iqIa2_1, filter.iqIb2_1, filter.iqIc2_1);
|
||
|
||
|
||
// i_led1_on_off(0);
|
||
// i_led1_on_off(1);
|
||
|
||
filter.iqIm_1 = exp_regul_iq(koef_Im_filter, filter.iqIm_1, analog.iqIm_1);
|
||
filter.iqIm_2 = exp_regul_iq(koef_Im_filter, filter.iqIm_2, analog.iqIm_2);
|
||
// filter.iqIm = exp_regul_iq(koef_Iabc_filter, filter.iqIm, filter.iqIm_2);
|
||
|
||
filter.iqW1 = exp_regul_iq(koef_Iabc_filter, filter.iqW1, analog.iqW1);
|
||
filter.iqW2 = exp_regul_iq(koef_Iabc_filter, filter.iqW2, analog.iqW2);
|
||
filter.iqW = exp_regul_iq(koef_Wlong, filter.iqW, filter.iqW1 + filter.iqW2);
|
||
|
||
// filter.iqIin_1 = exp_regul_iq(koef_Im_filter, filter.iqIin_1, analog.iqIin_1);
|
||
// filter.iqIin_2 = exp_regul_iq(koef_Im_filter, filter.iqIin_2, analog.iqIin_2);
|
||
|
||
}
|
||
|
||
#define CONST_IQ_11PI6 96629827 //11Pi/6
|
||
#define CONST_IQ_PI6 8784529 // Pi/6
|
||
|
||
|
||
// _iq err_level_adc_on_go = ERR_LEVEL_ADC_PLUS_6_ON_GO_IQ;
|
||
|
||
// _iq err_level_adc = ERR_LEVEL_ADC_PLUS_6_IQ;
|
||
|
||
|
||
// unsigned int detect_protect_ACP_plus()
|
||
// {
|
||
// unsigned int mask = 0, result = 0;
|
||
// static unsigned int prev_mask = 0xFCFC;
|
||
|
||
// if(f.Go)
|
||
// {
|
||
// mask |= filter.iqU_1_long > err_level_adc_on_go ? 1 : 0;
|
||
// mask |= filter.iqU_2_long > err_level_adc_on_go ? 1 << 1: 0;
|
||
// }
|
||
// else
|
||
// {
|
||
// mask |= filter.iqU_1_long > err_level_adc ? 1 : 0;
|
||
// mask |= filter.iqU_2_long > err_level_adc ? 1 << 1: 0;
|
||
// }
|
||
|
||
|
||
// #ifdef NEW_I_ZPT_REZISTORS
|
||
// mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 2: 0;
|
||
// mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 3: 0;
|
||
// #else
|
||
// mask |= ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS? 1 << 2: 0;
|
||
// mask |= ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS? 1 << 3: 0;
|
||
// #endif
|
||
|
||
// mask |= ADC_sf[0][4] < ERR_LEVEL_I_FAZA_PLUS? 1 << 4: 0;
|
||
// mask |= ADC_sf[0][5] < ERR_LEVEL_I_FAZA_PLUS? 1 << 5: 0;
|
||
// mask |= ADC_sf[0][6] < ERR_LEVEL_I_FAZA_PLUS? 1 << 6: 0;
|
||
|
||
// if(f.Go)
|
||
// {
|
||
// mask |= filter.iqU_3_long > err_level_adc_on_go ? 1 << 8: 0;
|
||
// mask |= filter.iqU_4_long > err_level_adc_on_go ? 1 << 9: 0;
|
||
// }
|
||
// else
|
||
// {
|
||
// mask |= (filter.iqU_3_long > err_level_adc) ? 1 << 8: 0;
|
||
// mask |= (filter.iqU_4_long > err_level_adc) ? 1 << 9: 0;
|
||
// }
|
||
|
||
|
||
// #ifdef NEW_I_ZPT_REZISTORS
|
||
// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 10: 0;
|
||
// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS_REZ_249? 1 << 11: 0;
|
||
// #else
|
||
// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0;
|
||
// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0;
|
||
// #endif
|
||
|
||
// mask |= ADC_sf[0][12] < ERR_LEVEL_I_FAZA_PLUS? 1 << 12: 0;
|
||
// mask |= ADC_sf[0][13] < ERR_LEVEL_I_FAZA_PLUS? 1 << 13: 0;
|
||
// mask |= ADC_sf[0][14] < ERR_LEVEL_I_FAZA_PLUS? 1 << 14: 0;
|
||
|
||
// //Calculate values for BTR
|
||
// #ifdef BTR_ENABLED
|
||
// mask |= ADC_sf[1][0] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0; //BTR1pos
|
||
// mask |= ADC_sf[1][2] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0; //BTR1neg
|
||
// #endif //BTR_ENABLED
|
||
// /* */
|
||
|
||
// result = mask & prev_mask;
|
||
// prev_mask |= mask; //
|
||
// //
|
||
// if(mask == 0) { prev_mask = 0xFFFF;}
|
||
// return result;
|
||
|
||
// }
|
||
|
||
|
||
// unsigned int detect_protect_ACP_minus()
|
||
// {
|
||
// unsigned int mask = 0, result = 0;
|
||
// static unsigned int prev_mask = 0xFCFC; //
|
||
|
||
// /* */
|
||
// if(f.Ready2)
|
||
// {
|
||
// // mask |= (ADC_sf[0] > ERR_LEVEL_ADC_MINUS_6) ? 1: 0;
|
||
// // mask |= (ADC_sf[1] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 1: 0;
|
||
// }
|
||
// #ifdef NEW_I_ZPT_REZISTORS
|
||
// mask |= (ADC_sf[0][2] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 2: 0;
|
||
// mask |= (ADC_sf[0][3] > ERR_LEVEL_I_ZPT_MINUS_REZ_249)? 1 << 3: 0;
|
||
// #else
|
||
// mask |= (ADC_sf[0][2] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 2: 0;
|
||
// mask |= (ADC_sf[0][3] < ERR_LEVEL_I_ZPT_PLUS)? 1 << 3: 0;
|
||
// #endif
|
||
// mask |= ADC_sf[0][4] > ERR_LEVEL_I_FAZA_MINUS? 1 << 4: 0;
|
||
// mask |= ADC_sf[0][5] > ERR_LEVEL_I_FAZA_MINUS? 1 << 5: 0;
|
||
// mask |= ADC_sf[0][6] > ERR_LEVEL_I_FAZA_MINUS? 1 << 6: 0;
|
||
// /* */
|
||
// /* if(f.Ready2)
|
||
// {
|
||
// mask |= (ADC_sf[0][7] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 8: 0;
|
||
// mask |= (ADC_sf[0][8] > ERR_LEVEL_ADC_MINUS_6) ? 1 << 9: 0;
|
||
// } */
|
||
// #ifdef NEW_I_ZPT_REZISTORS
|
||
// mask |= ADC_sf[0][10] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 10: 0;
|
||
// mask |= ADC_sf[0][11] > ERR_LEVEL_I_ZPT_MINUS_REZ_249? 1 << 11: 0;
|
||
// #else
|
||
// mask |= ADC_sf[0][10] < ERR_LEVEL_I_ZPT_PLUS? 1 << 10: 0;
|
||
// mask |= ADC_sf[0][11] < ERR_LEVEL_I_ZPT_PLUS? 1 << 11: 0;
|
||
// #endif
|
||
|
||
// mask |= ADC_sf[0][12] > ERR_LEVEL_I_FAZA_MINUS? 1 << 12: 0;
|
||
// mask |= ADC_sf[0][13] > ERR_LEVEL_I_FAZA_MINUS? 1 << 13: 0;
|
||
// mask |= ADC_sf[0][14] > ERR_LEVEL_I_FAZA_MINUS? 1 << 14: 0;
|
||
|
||
// //Calculate values for BTR
|
||
// #ifdef BTR_ENABLED
|
||
// mask |= ADC_sf[1][1] < ERR_LEVEL_BREAK_REZ? 1 << 7: 0; //BTR2pos
|
||
// mask |= ADC_sf[1][3] < ERR_LEVEL_BREAK_REZ? 1 << 15: 0; //BTR2neg
|
||
// #endif //BTR_ENABLED
|
||
// /* */
|
||
|
||
|
||
// result = mask & prev_mask;
|
||
// prev_mask |= mask; //
|
||
// //
|
||
// if(mask == 0) { prev_mask = 0xFFFF;}
|
||
// return result;
|
||
|
||
// }
|
||
|
||
// #pragma CODE_SECTION(fillADClogs,".fast_run");
|
||
// void fillADClogs(void)
|
||
// {
|
||
// logpar.log1 = (int16)_IQtoIQ15(analog.iqU_1);
|
||
// logpar.log2 = (int16)_IQtoIQ15(analog.iqU_2);
|
||
// logpar.log3 = (int16)_IQtoIQ15(analog.iqIin_1);
|
||
// logpar.log4 = (int16)_IQtoIQ15(analog.iqIin_2);
|
||
// logpar.log5 = (int16)_IQtoIQ15(analog.iqIa1_1);
|
||
// logpar.log6 = (int16)_IQtoIQ15(analog.iqIb1_1);
|
||
// logpar.log7 = (int16)_IQtoIQ15(analog.iqIc1_1);
|
||
// logpar.log8 = (int16)_IQtoIQ15(analog.iqU_3);
|
||
// logpar.log9 = (int16)_IQtoIQ15(analog.iqU_4);
|
||
// logpar.log10 = (int16)_IQtoIQ15(analog.iqIin_3);
|
||
// logpar.log11 = (int16)_IQtoIQ15(analog.iqIin_4);
|
||
// logpar.log12 = (int16)_IQtoIQ15(analog.iqIa2_1);
|
||
// logpar.log13 = (int16)_IQtoIQ15(analog.iqIb2_1);
|
||
// logpar.log14 = (int16)_IQtoIQ15(analog.iqIc2_1);
|
||
// // logpar.log15 = (int16)_IQtoIQ15(filter.iqU_1_fast);
|
||
// // logpar.log16 = (int16)_IQtoIQ15(filter.iqU_1_long);
|
||
// }
|
||
|
||
/********************************************************************/
|
||
/* Расчет модулy тока из показаний трех фаз */
|
||
/********************************************************************/
|
||
_iq SQRT_32 = _IQ(0.8660254037844);
|
||
_iq CONST_23 = _IQ(2.0/3.0);
|
||
_iq CONST_15 = _IQ(1.5);
|
||
|
||
_iq im_calc( _iq ia, _iq ib, _iq ic)
|
||
{
|
||
_iq isa,isb, t;
|
||
|
||
|
||
isa = _IQmpy(CONST_15,ia);
|
||
isb = _IQmpy(SQRT_32,ib-ic );
|
||
|
||
// ( _IQ19mpy(SQRT_32, _IQ15toIQ19(ib)) - _IQ15mpy(SQRT_32, _IQ15toIQ19(ic)) );
|
||
|
||
// t = _IQmag(isa,isb);
|
||
t = _IQsqrt(_IQmpy(isa, isa) + _IQmpy(isb, isb));
|
||
t = _IQmpy(CONST_23,t);
|
||
|
||
return (t);
|
||
|
||
}
|
||
|
||
|