Dmitry Kovalev
/
LG
n
Fork of LG by
Diff: SIP.c
- Revision:
- 21:bc8c1cec3da6
- Parent:
- 1:f2adcae3d304
--- a/SIP.c Tue Feb 02 17:14:25 2016 +0000 +++ b/SIP.c Wed Feb 03 07:19:30 2016 +0000 @@ -3,14 +3,15 @@ - uint32_t Old_Cnt_Vib = 0; - uint32_t Old_Cnt = 0; - int32_t RefMeand_Cnt_Dif; - int32_t PSdif_sum_Vib_32 = 0; - __int64 PSdif_sum_Vib_64 = 0; - int32_t dif_Curr_32_Ext; //r. �������� (�����) ��� ������ ������� ������� - int32_t dif_Curr_32_previous; //e. Previous (in comparison with Dif_Curr_32) number //r. ���������� (�� ��������� � Dif_Curr_32) ����� - int32_t temp22=0; + +uint32_t Old_Cnt_Vib = 0; +uint32_t Old_Cnt = 0; +int32_t RefMeand_Cnt_Dif; +int32_t PSdif_sum_Vib_32 = 0; +__int64 PSdif_sum_Vib_64 = 0; +int32_t dif_Curr_32_Ext; //r. разность (число) для режима внешней защелки +int32_t dif_Curr_32_previous; //e. Previous (in comparison with Dif_Curr_32) number //r. предыдущее (по сравнению с Dif_Curr_32) число +int32_t temp22=0; //+++++++++++++++++++++++++++++++INPUT DATA++++++++++++++++++++++++++++++++++++++++++++++++ uint32_t Curr_Cnt_Vib; uint32_t Cnt_curr; @@ -19,238 +20,226 @@ int32_t Dif_Curr_Vib; //e. current difference output for dithering control in LightUp mode and Dither regulator int32_t Dif_Curr_32; //e. current difference without dithering for dithering control -#if (defined GLOBALRATE)//�� ������ +#if (defined GLOBALRATE)//не входит //variables for rate mode - int32_t cntPls_sum_32; - ��� int32_t last_Cnt_Plus; - int32_t dif_sum_32; - int32_t Cnt_Pls; - int32_t Cnt_Mns; - int32_t preLast_Cnt_Plus; - // uint32_t sumCnt_Mns = 0; - //uint32_t sumCnt_Pls = 0; - extern int32_t dif_cur_test[30]; - extern unsigned ii; - uint32_t halfPeriod = 0; +int32_t cntPls_sum_32; +ссс int32_t last_Cnt_Plus; +int32_t dif_sum_32; +int32_t Cnt_Pls; +int32_t Cnt_Mns; +int32_t preLast_Cnt_Plus; +// uint32_t sumCnt_Mns = 0; +//uint32_t sumCnt_Pls = 0; +extern int32_t dif_cur_test[30]; +extern unsigned ii; +uint32_t halfPeriod = 0; #endif void ResetBitsOfWord(int * x32, int truncate_bits) { - int hi_part; + int hi_part; - hi_part = *x32 >> truncate_bits; - *x32 -= hi_part << truncate_bits; //r. ��������� ������� 16 ��� -} + hi_part = *x32 >> truncate_bits; + *x32 -= hi_part << truncate_bits; //r. оставляем младшие 16 бит +} /****************************************************************************** ** Function name: interpolation ** -** Descriptions: ���������� ����������� �� ��������� ������� ������� +** Descriptions: количество итерполяций до появления внешней защелки ** -** Returned value: Number in moment of external latch appearing +** Returned value: Number in moment of external latch appearing ** parameters: y_curr - current number, y_prev - number at one cycle before time ** x_interp - moment of external latch appearing, - + ** Precision of interpolation is 1/8 of impulse (3 digits after point in 14.18 format) ******************************************************************************/ int interpolation(int y_curr, int x_interp) { - __int64 temp,temp3; + __int64 temp,temp3; - temp = (__int64)y_curr *(__int64)x_interp; - temp /= PrevPeriod; // void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c) + temp = (__int64)y_curr *(__int64)x_interp; + temp /= PrevPeriod; // void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c) - temp3 = (__int64)y_curr *(__int64)(x_interp+1); - temp3 /= PrevPeriod; - temp22 = (int)temp3; + temp3 = (__int64)y_curr *(__int64)(x_interp+1); + temp3 /= PrevPeriod; + temp22 = (int)temp3; - return ((int)temp); + return ((int)temp); } // interpolation /****************************************************************************** ** Function name: clc_Pulses ** -** Descriptions: Processing of information from SPOI +** Descriptions: Processing of information from SPOI ** -** parameters: None -** Returned value: None -** +** parameters: None +** Returned value: None +** ******************************************************************************/ void clc_Pulses() { #if (!defined GLOBALRATE) - static int32_t cntPls_sum_32; - static int32_t last_Cnt_Plus; - static int32_t dif_sum_32; - static int32_t Cnt_Pls; - static int32_t Cnt_Mns; - static int32_t preLast_Cnt_Plus; + static int32_t cntPls_sum_32; + static int32_t last_Cnt_Plus; + static int32_t dif_sum_32; + static int32_t Cnt_Pls; + static int32_t Cnt_Mns; + static int32_t preLast_Cnt_Plus; #endif - Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //���������� ���������� �������� �������� ���������. + Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //Вычисление приращения текущего счетчика импульсов. - Old_Cnt_Vib = Curr_Cnt_Vib; //���������� �������� �������� ��������� ��� ���������� ����� ��������� + Old_Cnt_Vib = Curr_Cnt_Vib; //сохранение текущего счетчика импульсов для следующего цикла измерений + - - Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //�������� �� ������������ (Dif_Curr_Vib). - // Uin UpSat DwnSat - - //#define Cnt_Overload(Uin, UpSat, DwnSat) -/* if (Uin > UpSat) - - { - Uin -= INT32_MAX; - } - if (Uin < DwnSat) - { - Uin += INT32_MAX; - } */ - - - Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // �������� ���������� 1/(2^18) - - - - - switch (RgConB) //r. �������������� ������� ���������� - { - case RATE_VIBRO_1: //r. �������� �������������� ����� ������� ����������� �������� - - if (Latch_Rdy) //e. latch has arrived - { - //dif_Curr_32_Ext �������� (�����) ��� ������ ������� ������� - dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase ); //������������ - Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); - Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); - //r. ��������� � ����������� ����� ����������������� ������ ������� ������� - PSdif_sum_Vib_32 += dif_Curr_32_Ext; - PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data - //count--; - - //r. ����������� ����� ��� ������ - Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum; //r. �� ������������ ����� �������� ����������� �������������������� ������������ - Output.Str.PS_dif = Output.Str.BINS_dif >> 16; - LatchPhase = INT32_MAX; //INT32_MAX=2147483647 //in Latch_Event it's indicator of latch appearing - Output.Str.SF_dif = PSdif_sum_Vib_64; - TermoCompens_Sum = 0; //r. �������� ����������� ���������������� ��� ������ ������ ����� ���������� - - if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable)) - { - PSdif_sum_Vib_32 = 0; //r. ���������������� ����� ���� ��������� �� ������� - PSdif_sum_Vib_64 = 0; - } - else - ResetBitsOfWord(&PSdif_sum_Vib_32, 16); - - dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext; + Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //проверка на переполнение (Dif_Curr_Vib). + // Uin UpSat DwnSat + + //#define Cnt_Overload(Uin, UpSat, DwnSat) + /* if (Uin > UpSat) - PSdif_sum_Vib_32 += dif_Curr_32_Ext; // preserve rest of counters difference for next measure cycle: PSdif_sum_Vib_32 += Dif_Curr_32 - dif_Curr_32_Ext; - PSdif_sum_Vib_64 += dif_Curr_32_Ext; //��������� ��������� �������� ������� ��� ���������� ����� ����� - - } - else //r. ������� �� ��������� ������ �� ���� - { - //r. ���������� ����������� ����� �� ���������� �������� - PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ; - PSdif_sum_Vib_64 += Dif_Curr_32; //e. sum for scale factor measurement mode - } - - dif_Curr_32_previous = Dif_Curr_32; //r. ���������� ���������� ����� - break; - - case RATE_REPER_OR_REFMEANDR: - - if (data_Rdy & HALF_PERIOD) //e. calculate Cnt_Mns or Cnt_Pls - { - RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt; - Old_Cnt = Cnt_curr; - // LPC_GPIO2->FIOCLR = 0x10; - Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2); - - if (LPC_QEI->STAT) //e. "+" direction //r. ����� ��������� � "+" ������� - { - //sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec - Cnt_Mns = RefMeand_Cnt_Dif; - } - else - { - //r. ����� ��������� � "-" ������� - //sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec - Cnt_Pls = -RefMeand_Cnt_Dif; - } + { + Uin -= INT32_MAX; + } + if (Uin < DwnSat) + { + Uin += INT32_MAX; + } */ - // UART1_SendByte((dif_sum_32>>8) & 0xff); - // UART1_SendByte((dif_sum_32) & 0xff); - - if (data_Rdy & WHOLE_PERIOD) //e. period of vibro elapsed - { - // LPC_GPIO2->FIOSET = 0x10; - last_Cnt_Plus = Cnt_Pls; - dif_sum_32 += Cnt_Pls - Cnt_Mns; - } - data_Rdy &= ~RESET_PERIOD; - } - if (Latch_Rdy) //e it's time for output - { - LatchPhase = INT32_MAX; - - Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); - Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); - cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus; - - Output.Str.Cnt_Dif = dif_sum_32; - Output.Str.Cnt_Dif += cntPls_sum_32 >> 1; - - dif_sum_32 = 0; //r. � �������� �������� ���������� - ResetBitsOfWord(&cntPls_sum_32, 1); //r. ��������� ������� ������, ����� �� �������� �������� ��� ������������ - preLast_Cnt_Plus = last_Cnt_Plus; //r. ������� ��������� ������ ���� ���������� - - Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns; //e. rewrite accumulated data to output - Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls; + Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Точность фильтрации 1/(2^18) - //sumCnt_Mns = 0; //e. prepare for new accumulation - //sumCnt_Pls = 0; - } - break; // RATE_REPER_OR_REFMEANDR - } - //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - ���������� ��� �������� � ������ rate3 - Output.Str.WP_scope1 = Dif_Curr_Vib; - Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 ������� ������� ��������� ��� ������� ����������� ��� ������� ������� rate3 -} - // clc_Pulses + - /****************************************************************************** + switch (RgConB) { //r. дополнительный регистр управления + case RATE_VIBRO_1: //r. разность вибросчетчиков после фильтра скользящнго среднего + + if (Latch_Rdy) { //e. latch has arrived + //dif_Curr_32_Ext разность (число) для режима внешней защелки + dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase ); //интерполяция + Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); + Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); + //r. добавляем к накопленной сумме интерполированный отсчет внешней защелки + PSdif_sum_Vib_32 += dif_Curr_32_Ext; + PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data + //count--; + + //r. подготовить число для выдачи + Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum; //r. из накопленного числа вычитаем накопленную термокомпенсационную составляющую + Output.Str.PS_dif = Output.Str.BINS_dif >> 16; + LatchPhase = INT32_MAX; //INT32_MAX=2147483647 //in Latch_Event it's indicator of latch appearing + Output.Str.SF_dif = PSdif_sum_Vib_64; + TermoCompens_Sum = 0; //r. обнуляем накопленную термокомпенсацию для начала нового цикла накопления + + if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable)) { + PSdif_sum_Vib_32 = 0; //r. инициализировать новый цикл измерения по защелке + PSdif_sum_Vib_64 = 0; + } else + ResetBitsOfWord(&PSdif_sum_Vib_32, 16); + + dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext; + + PSdif_sum_Vib_32 += dif_Curr_32_Ext; // preserve rest of counters difference for next measure cycle: PSdif_sum_Vib_32 += Dif_Curr_32 - dif_Curr_32_Ext; + PSdif_sum_Vib_64 += dif_Curr_32_Ext; //сохранить остальные счетчики разницы для следующего такта цикла + + } else { //r. защелки на настоящий момент не было + //r. продолжаем накапливать сумму из внутренних отсчетов + PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ; + PSdif_sum_Vib_64 += Dif_Curr_32; //e. sum for scale factor measurement mode + } + + dif_Curr_32_previous = Dif_Curr_32; //r. запоминаем предыдущее число + break; + + case RATE_REPER_OR_REFMEANDR: + + if (data_Rdy & HALF_PERIOD) { //e. calculate Cnt_Mns or Cnt_Pls + RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt; + Old_Cnt = Cnt_curr; + // LPC_GPIO2->FIOCLR = 0x10; + Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2); + + if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" сторону + //sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec + Cnt_Mns = RefMeand_Cnt_Dif; + } else { + //r. стали вращаться в "-" сторону + //sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec + Cnt_Pls = -RefMeand_Cnt_Dif; + } + + // UART1_SendByte((dif_sum_32>>8) & 0xff); + // UART1_SendByte((dif_sum_32) & 0xff); + + if (data_Rdy & WHOLE_PERIOD) { //e. period of vibro elapsed + // LPC_GPIO2->FIOSET = 0x10; + last_Cnt_Plus = Cnt_Pls; + dif_sum_32 += Cnt_Pls - Cnt_Mns; + } + data_Rdy &= ~RESET_PERIOD; + } + if (Latch_Rdy) { //e it's time for output + LatchPhase = INT32_MAX; + + Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff); + Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff); + + cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus; + + Output.Str.Cnt_Dif = dif_sum_32; + Output.Str.Cnt_Dif += cntPls_sum_32 >> 1; + + dif_sum_32 = 0; //r. и сбросить регистры накопления + ResetBitsOfWord(&cntPls_sum_32, 1); //r. оставляем младший разряд, чтобы не терялась точность при суммировании + preLast_Cnt_Plus = last_Cnt_Plus; //r. текущий последний отсчет стал предыдущим + + Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns; //e. rewrite accumulated data to output + Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls; + + //sumCnt_Mns = 0; //e. prepare for new accumulation + //sumCnt_Pls = 0; + } + break; // RATE_REPER_OR_REFMEANDR + } + //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - переменные для контроля в режиме rate3 + Output.Str.WP_scope1 = Dif_Curr_Vib; + Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 дробных разряда оставляем для большей наглядности при анализе сигнала rate3 +} +// clc_Pulses + + +/****************************************************************************** ** Function name: SOI_Init ** ** Descriptions: Quadrature encoder initialization. ** ** parameters: None ** Returned value: None -** +** ******************************************************************************/ void SOI_Init(void) { - LPC_SC->PCONP |= (1<<18);//0x00040000; ��������� ������������� ��������. - LPC_SC->PCLKSEL1 |= 0x00000001; // ����� ������� ��� ��.�. CLK=100MHz - LPC_PINCON->PINSEL3 &= ~0x4100; - LPC_PINCON->PINSEL3 |= 0x4100; //P1.20, p1.23 ���������� ��� ����� ��.� - LPC_PINCON->PINMODE3 |= 0x3C300; //P1.20, p1.23, p1.24 ??are pulled-down??(��������� � �������)???? + LPC_SC->PCONP |= (1<<18);//0x00040000; включение квадратурного энкодера. + LPC_SC->PCLKSEL1 |= 0x00000001; // выбор частоты для кв.э. CLK=100MHz + LPC_PINCON->PINSEL3 &= ~0x4100; + LPC_PINCON->PINSEL3 |= 0x4100; //P1.20, p1.23 установить как входы кв.э + LPC_PINCON->PINMODE3 |= 0x3C300; //P1.20, p1.23, p1.24 ??are pulled-down??(притянуть к питанию)???? - LPC_QEI->MAXPOS = MAX_QEI_CNT; //LPC_QEI->"�������� ������������� ���������" = 2147483646 - LPC_QEI->FILTER = 2; // ������( ?? ) - LPC_QEI->CON = 0xF; //�������� � ���� ��� �������� ��.�. (��������, �����������, ������� � ��) - LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted + LPC_QEI->MAXPOS = MAX_QEI_CNT; //LPC_QEI->"регистор максимального положения" = 2147483646 + LPC_QEI->FILTER = 2; // фильтр( ?? ) + LPC_QEI->CON = 0xF; //сбросить в ноль все счетчики кв.э. (скорость, направление, позицию и тд) + LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted - LPC_QEI->CLR = 0x1fff; //r. �������� ��� ���������� - LPC_QEI->IEC = 0x1fff; //r. ��������� ���������� ��� ��������� ����������� - - NVIC_SetPriority(QEI_IRQn, 0); - NVIC_EnableIRQ(QEI_IRQn); - return; - } + LPC_QEI->CLR = 0x1fff; //r. сбросить все прерывания + LPC_QEI->IEC = 0x1fff; //r. запретить прерывание при изменении направления + + NVIC_SetPriority(QEI_IRQn, 0); + NVIC_EnableIRQ(QEI_IRQn); + return; +} /******************************************************************************