Dmitry Kovalev
/
LG
n
Fork of LG by
Diff: SIP.c
- Revision:
- 1:f2adcae3d304
- Parent:
- 0:8ad47e2b6f00
- Child:
- 21:bc8c1cec3da6
--- a/SIP.c Sat Jan 30 13:00:39 2016 +0000 +++ b/SIP.c Sat Jan 30 13:53:19 2016 +0000 @@ -1,10 +1,6 @@ -#include "mathDSP.h" -#include "SIP.h" -#include "CyclesSync.h" -#include "CntrlGLD.h" -#include "InputOutput.h" -#include "ThermoCalc.h" -#include "el_lin.h" + +#include "Global.h" + uint32_t Old_Cnt_Vib = 0; @@ -12,8 +8,8 @@ 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 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; @@ -23,11 +19,11 @@ 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 last_Cnt_Plus; int32_t dif_sum_32; int32_t Cnt_Pls; int32_t Cnt_Mns; @@ -44,12 +40,12 @@ int hi_part; hi_part = *x32 >> truncate_bits; - *x32 -= hi_part << truncate_bits; //r. îñòàâëÿåì ìëàäøèå 16 áèò + *x32 -= hi_part << truncate_bits; //r. ��������� ������� 16 ��� } /****************************************************************************** ** Function name: interpolation ** -** Descriptions: êîëè÷åñòâî èòåðïîëÿöèé äî ïîÿâëåíèÿ âíåøíåé çàùåëêè +** Descriptions: ���������� ����������� �� ��������� ������� ������� ** ** Returned value: Number in moment of external latch appearing ** parameters: y_curr - current number, y_prev - number at one cycle before time @@ -92,12 +88,12 @@ 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). + Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //�������� �� ������������ (Dif_Curr_Vib). // Uin UpSat DwnSat //#define Cnt_Overload(Uin, UpSat, DwnSat) @@ -112,36 +108,36 @@ } */ - Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Òî÷íîñòü ôèëüòðàöèè 1/(2^18) + Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // �������� ���������� 1/(2^18) - switch (RgConB) //r. äîïîëíèòåëüíûé ðåãèñòð óïðàâëåíèÿ + switch (RgConB) //r. �������������� ������� ���������� { - case RATE_VIBRO_1: //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 ); //èíòåðïîëÿöèÿ + //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. äîáàâëÿåì ê íàêîïëåííîé ñóììå èíòåðïîëèðîâàííûé îòñ÷åò âíåøíåé çàùåëêè + //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. èç íàêîïëåííîãî ÷èñëà âû÷èòàåì íàêîïëåííóþ òåðìîêîìïåíñàöèîííóþ ñîñòàâëÿþùóþ + //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. îáíóëÿåì íàêîïëåííóþ òåðìîêîìïåíñàöèþ äëÿ íà÷àëà íîâîãî öèêëà íàêîïëåíèÿ + 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_32 = 0; //r. ���������������� ����� ���� ��������� �� ������� PSdif_sum_Vib_64 = 0; } else @@ -150,17 +146,17 @@ 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; //ñîõðàíèòü îñòàëüíûå ñ÷åò÷èêè ðàçíèöû äëÿ ñëåäóþùåãî òàêòà öèêëà + PSdif_sum_Vib_64 += dif_Curr_32_Ext; //��������� ��������� �������� ������� ��� ���������� ����� ����� } - else //r. çàùåëêè íà íàñòîÿùèé ìîìåíò íå áûëî + else //r. ������� �� ��������� ������ �� ���� { - //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. çàïîìèíàåì ïðåäûäóùåå ÷èñëî + dif_Curr_32_previous = Dif_Curr_32; //r. ���������� ���������� ����� break; case RATE_REPER_OR_REFMEANDR: @@ -172,14 +168,14 @@ // LPC_GPIO2->FIOCLR = 0x10; Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2); - if (LPC_QEI->STAT) //e. "+" direction //r. ñòàëè âðàùàòüñÿ â "+" ñòîðîíó + if (LPC_QEI->STAT) //e. "+" direction //r. ����� ��������� � "+" ������� { //sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec Cnt_Mns = RefMeand_Cnt_Dif; } else { - //r. ñòàëè âðàùàòüñÿ â "-" ñòîðîíó + //r. ����� ��������� � "-" ������� //sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec Cnt_Pls = -RefMeand_Cnt_Dif; } @@ -207,9 +203,9 @@ 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. òåêóùèé ïîñëåäíèé îòñ÷åò ñòàë ïðåäûäóùèì + 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; @@ -219,9 +215,9 @@ } break; // RATE_REPER_OR_REFMEANDR } - //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - ïåðåìåííûå äëÿ êîíòðîëÿ â ðåæèìå rate3 + //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 + Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 ������� ������� ��������� ��� ������� ����������� ��� ������� ������� rate3 } // clc_Pulses @@ -237,19 +233,19 @@ ******************************************************************************/ void SOI_Init(void) { - LPC_SC->PCONP |= (1<<18);//0x00040000; âêëþ÷åíèå êâàäðàòóðíîãî ýíêîäåðà. - LPC_SC->PCLKSEL1 |= 0x00000001; // âûáîð ÷àñòîòû äëÿ êâ.ý. CLK=100MHz + 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_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->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. çàïðåòèòü ïðåðûâàíèå ïðè èçìåíåíèè íàïðàâëåíèÿ + LPC_QEI->CLR = 0x1fff; //r. �������� ��� ���������� + LPC_QEI->IEC = 0x1fff; //r. ��������� ���������� ��� ��������� ����������� NVIC_SetPriority(QEI_IRQn, 0); NVIC_EnableIRQ(QEI_IRQn);