Dmitry Kovalev
forkd
Fork of
LG2
by 
Dmitry Kovalev
Diff: SIP.c
- Revision:
- 0:8ad47e2b6f00
- Child:
- 1:f2adcae3d304
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SIP.c Sat Jan 30 13:00:39 2016 +0000
@@ -0,0 +1,263 @@
+#include "mathDSP.h"
+#include "SIP.h"
+#include "CyclesSync.h"
+#include "CntrlGLD.h"
+#include "InputOutput.h"
+#include "ThermoCalc.h"
+#include "el_lin.h"
+
+
+ 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;
+
+//+++++++++++++++++++++++++++++++variables for output++++++++++++++++++++++++++++++++++++++
+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)//íå âõîäèò
+
+//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;
+#endif
+
+void ResetBitsOfWord(int * x32, int truncate_bits)
+{
+ int hi_part;
+
+ hi_part = *x32 >> truncate_bits;
+ *x32 -= hi_part << truncate_bits; //r. îñòàâëÿåì ìëàäøèå 16 áèò
+}
+/******************************************************************************
+** Function name: interpolation
+**
+** Descriptions: êîëè÷åñòâî èòåðïîëÿöèé äî ïîÿâëåíèÿ âíåøíåé çàùåëêè
+**
+** 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;
+
+ 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;
+
+ return ((int)temp);
+} // interpolation
+/******************************************************************************
+** Function name: clc_Pulses
+**
+** Descriptions: Processing of information from SPOI
+**
+** 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;
+#endif
+
+ Dif_Curr_Vib = Curr_Cnt_Vib - Old_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;
+
+ 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_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;
+ }
+
+
+/******************************************************************************
+** End Of File
+******************************************************************************/
+