Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

SIP.c@1:f2adcae3d304, 2016-01-30 (annotated)

Committer:
igor_v
Date:
Sat Jan 30 13:53:19 2016 +0000
Revision:
1:f2adcae3d304
Parent:
0:8ad47e2b6f00
Child:
21:bc8c1cec3da6
123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 1:f2adcae3d304 1
igor_v 1:f2adcae3d304 2 #include "Global.h"
igor_v 1:f2adcae3d304 3
igor_v 0:8ad47e2b6f00 4
igor_v 0:8ad47e2b6f00 5
igor_v 0:8ad47e2b6f00 6 uint32_t Old_Cnt_Vib = 0;
igor_v 0:8ad47e2b6f00 7 uint32_t Old_Cnt = 0;
igor_v 0:8ad47e2b6f00 8 int32_t RefMeand_Cnt_Dif;
igor_v 0:8ad47e2b6f00 9 int32_t PSdif_sum_Vib_32 = 0;
igor_v 0:8ad47e2b6f00 10 __int64 PSdif_sum_Vib_64 = 0;
igor_v 1:f2adcae3d304 11 int32_t dif_Curr_32_Ext; //r. �������� (�����) ��� ������ ������� �������
igor_v 1:f2adcae3d304 12 int32_t dif_Curr_32_previous; //e. Previous (in comparison with Dif_Curr_32) number //r. ���������� (�� ��������� � Dif_Curr_32) �����
igor_v 0:8ad47e2b6f00 13 int32_t temp22=0;
igor_v 0:8ad47e2b6f00 14 //+++++++++++++++++++++++++++++++INPUT DATA++++++++++++++++++++++++++++++++++++++++++++++++
igor_v 0:8ad47e2b6f00 15 uint32_t Curr_Cnt_Vib;
igor_v 0:8ad47e2b6f00 16 uint32_t Cnt_curr;
igor_v 0:8ad47e2b6f00 17
igor_v 0:8ad47e2b6f00 18 //+++++++++++++++++++++++++++++++variables for output++++++++++++++++++++++++++++++++++++++
igor_v 0:8ad47e2b6f00 19 int32_t Dif_Curr_Vib; //e. current difference output for dithering control in LightUp mode and Dither regulator
igor_v 0:8ad47e2b6f00 20 int32_t Dif_Curr_32; //e. current difference without dithering for dithering control
igor_v 0:8ad47e2b6f00 21
igor_v 1:f2adcae3d304 22 #if (defined GLOBALRATE)//�� ������
igor_v 0:8ad47e2b6f00 23
igor_v 0:8ad47e2b6f00 24 //variables for rate mode
igor_v 0:8ad47e2b6f00 25 int32_t cntPls_sum_32;
igor_v 1:f2adcae3d304 26 ��� int32_t last_Cnt_Plus;
igor_v 0:8ad47e2b6f00 27 int32_t dif_sum_32;
igor_v 0:8ad47e2b6f00 28 int32_t Cnt_Pls;
igor_v 0:8ad47e2b6f00 29 int32_t Cnt_Mns;
igor_v 0:8ad47e2b6f00 30 int32_t preLast_Cnt_Plus;
igor_v 0:8ad47e2b6f00 31 // uint32_t sumCnt_Mns = 0;
igor_v 0:8ad47e2b6f00 32 //uint32_t sumCnt_Pls = 0;
igor_v 0:8ad47e2b6f00 33 extern int32_t dif_cur_test[30];
igor_v 0:8ad47e2b6f00 34 extern unsigned ii;
igor_v 0:8ad47e2b6f00 35 uint32_t halfPeriod = 0;
igor_v 0:8ad47e2b6f00 36 #endif
igor_v 0:8ad47e2b6f00 37
igor_v 0:8ad47e2b6f00 38 void ResetBitsOfWord(int * x32, int truncate_bits)
igor_v 0:8ad47e2b6f00 39 {
igor_v 0:8ad47e2b6f00 40 int hi_part;
igor_v 0:8ad47e2b6f00 41
igor_v 0:8ad47e2b6f00 42 hi_part = *x32 >> truncate_bits;
igor_v 1:f2adcae3d304 43 *x32 -= hi_part << truncate_bits; //r. ��������� ������� 16 ���
igor_v 0:8ad47e2b6f00 44 }
igor_v 0:8ad47e2b6f00 45 /******************************************************************************
igor_v 0:8ad47e2b6f00 46 ** Function name: interpolation
igor_v 0:8ad47e2b6f00 47 **
igor_v 1:f2adcae3d304 48 ** Descriptions: ���������� ����������� �� ��������� ������� �������
igor_v 0:8ad47e2b6f00 49 **
igor_v 0:8ad47e2b6f00 50 ** Returned value: Number in moment of external latch appearing
igor_v 0:8ad47e2b6f00 51 ** parameters: y_curr - current number, y_prev - number at one cycle before time
igor_v 0:8ad47e2b6f00 52 ** x_interp - moment of external latch appearing,
igor_v 0:8ad47e2b6f00 53
igor_v 0:8ad47e2b6f00 54 ** Precision of interpolation is 1/8 of impulse (3 digits after point in 14.18 format)
igor_v 0:8ad47e2b6f00 55 ******************************************************************************/
igor_v 0:8ad47e2b6f00 56 int interpolation(int y_curr, int x_interp)
igor_v 0:8ad47e2b6f00 57 {
igor_v 0:8ad47e2b6f00 58 __int64 temp,temp3;
igor_v 0:8ad47e2b6f00 59
igor_v 0:8ad47e2b6f00 60 temp = (__int64)y_curr *(__int64)x_interp;
igor_v 0:8ad47e2b6f00 61 temp /= PrevPeriod; // void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c)
igor_v 0:8ad47e2b6f00 62
igor_v 0:8ad47e2b6f00 63 temp3 = (__int64)y_curr *(__int64)(x_interp+1);
igor_v 0:8ad47e2b6f00 64 temp3 /= PrevPeriod;
igor_v 0:8ad47e2b6f00 65 temp22 = (int)temp3;
igor_v 0:8ad47e2b6f00 66
igor_v 0:8ad47e2b6f00 67 return ((int)temp);
igor_v 0:8ad47e2b6f00 68 } // interpolation
igor_v 0:8ad47e2b6f00 69 /******************************************************************************
igor_v 0:8ad47e2b6f00 70 ** Function name: clc_Pulses
igor_v 0:8ad47e2b6f00 71 **
igor_v 0:8ad47e2b6f00 72 ** Descriptions: Processing of information from SPOI
igor_v 0:8ad47e2b6f00 73 **
igor_v 0:8ad47e2b6f00 74 ** parameters: None
igor_v 0:8ad47e2b6f00 75 ** Returned value: None
igor_v 0:8ad47e2b6f00 76 **
igor_v 0:8ad47e2b6f00 77 ******************************************************************************/
igor_v 0:8ad47e2b6f00 78
igor_v 0:8ad47e2b6f00 79 void clc_Pulses()
igor_v 0:8ad47e2b6f00 80 {
igor_v 0:8ad47e2b6f00 81
igor_v 0:8ad47e2b6f00 82 #if (!defined GLOBALRATE)
igor_v 0:8ad47e2b6f00 83 static int32_t cntPls_sum_32;
igor_v 0:8ad47e2b6f00 84 static int32_t last_Cnt_Plus;
igor_v 0:8ad47e2b6f00 85 static int32_t dif_sum_32;
igor_v 0:8ad47e2b6f00 86 static int32_t Cnt_Pls;
igor_v 0:8ad47e2b6f00 87 static int32_t Cnt_Mns;
igor_v 0:8ad47e2b6f00 88 static int32_t preLast_Cnt_Plus;
igor_v 0:8ad47e2b6f00 89 #endif
igor_v 0:8ad47e2b6f00 90
igor_v 1:f2adcae3d304 91 Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //���������� ���������� �������� �������� ���������.
igor_v 0:8ad47e2b6f00 92
igor_v 1:f2adcae3d304 93 Old_Cnt_Vib = Curr_Cnt_Vib; //���������� �������� �������� ��������� ��� ���������� ����� ���������
igor_v 0:8ad47e2b6f00 94
igor_v 0:8ad47e2b6f00 95
igor_v 1:f2adcae3d304 96 Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //�������� �� ������������ (Dif_Curr_Vib).
igor_v 0:8ad47e2b6f00 97 // Uin UpSat DwnSat
igor_v 0:8ad47e2b6f00 98
igor_v 0:8ad47e2b6f00 99 //#define Cnt_Overload(Uin, UpSat, DwnSat)
igor_v 0:8ad47e2b6f00 100 /* if (Uin > UpSat)
igor_v 0:8ad47e2b6f00 101
igor_v 0:8ad47e2b6f00 102 {
igor_v 0:8ad47e2b6f00 103 Uin -= INT32_MAX;
igor_v 0:8ad47e2b6f00 104 }
igor_v 0:8ad47e2b6f00 105 if (Uin < DwnSat)
igor_v 0:8ad47e2b6f00 106 {
igor_v 0:8ad47e2b6f00 107 Uin += INT32_MAX;
igor_v 0:8ad47e2b6f00 108 } */
igor_v 0:8ad47e2b6f00 109
igor_v 0:8ad47e2b6f00 110
igor_v 1:f2adcae3d304 111 Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // �������� ���������� 1/(2^18)
igor_v 0:8ad47e2b6f00 112
igor_v 0:8ad47e2b6f00 113
igor_v 0:8ad47e2b6f00 114
igor_v 0:8ad47e2b6f00 115
igor_v 1:f2adcae3d304 116 switch (RgConB) //r. �������������� ������� ����������
igor_v 0:8ad47e2b6f00 117 {
igor_v 1:f2adcae3d304 118 case RATE_VIBRO_1: //r. �������� �������������� ����� ������� ����������� ��������
igor_v 0:8ad47e2b6f00 119
igor_v 0:8ad47e2b6f00 120 if (Latch_Rdy) //e. latch has arrived
igor_v 0:8ad47e2b6f00 121 {
igor_v 1:f2adcae3d304 122 //dif_Curr_32_Ext �������� (�����) ��� ������ ������� �������
igor_v 1:f2adcae3d304 123 dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase ); //������������
igor_v 0:8ad47e2b6f00 124 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
igor_v 0:8ad47e2b6f00 125 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
igor_v 1:f2adcae3d304 126 //r. ��������� � ����������� ����� ����������������� ������ ������� �������
igor_v 0:8ad47e2b6f00 127 PSdif_sum_Vib_32 += dif_Curr_32_Ext;
igor_v 0:8ad47e2b6f00 128 PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data
igor_v 0:8ad47e2b6f00 129 //count--;
igor_v 0:8ad47e2b6f00 130
igor_v 1:f2adcae3d304 131 //r. ����������� ����� ��� ������
igor_v 1:f2adcae3d304 132 Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum; //r. �� ������������ ����� �������� ����������� �������������������� ������������
igor_v 0:8ad47e2b6f00 133 Output.Str.PS_dif = Output.Str.BINS_dif >> 16;
igor_v 0:8ad47e2b6f00 134 LatchPhase = INT32_MAX; //INT32_MAX=2147483647 //in Latch_Event it's indicator of latch appearing
igor_v 0:8ad47e2b6f00 135 Output.Str.SF_dif = PSdif_sum_Vib_64;
igor_v 1:f2adcae3d304 136 TermoCompens_Sum = 0; //r. �������� ����������� ���������������� ��� ������ ������ ����� ����������
igor_v 0:8ad47e2b6f00 137
igor_v 0:8ad47e2b6f00 138 if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable))
igor_v 0:8ad47e2b6f00 139 {
igor_v 1:f2adcae3d304 140 PSdif_sum_Vib_32 = 0; //r. ���������������� ����� ���� ��������� �� �������
igor_v 0:8ad47e2b6f00 141 PSdif_sum_Vib_64 = 0;
igor_v 0:8ad47e2b6f00 142 }
igor_v 0:8ad47e2b6f00 143 else
igor_v 0:8ad47e2b6f00 144 ResetBitsOfWord(&PSdif_sum_Vib_32, 16);
igor_v 0:8ad47e2b6f00 145
igor_v 0:8ad47e2b6f00 146 dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext;
igor_v 0:8ad47e2b6f00 147
igor_v 0:8ad47e2b6f00 148 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;
igor_v 1:f2adcae3d304 149 PSdif_sum_Vib_64 += dif_Curr_32_Ext; //��������� ��������� �������� ������� ��� ���������� ����� �����
igor_v 0:8ad47e2b6f00 150
igor_v 0:8ad47e2b6f00 151 }
igor_v 1:f2adcae3d304 152 else //r. ������� �� ��������� ������ �� ����
igor_v 0:8ad47e2b6f00 153 {
igor_v 1:f2adcae3d304 154 //r. ���������� ����������� ����� �� ���������� ��������
igor_v 0:8ad47e2b6f00 155 PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ;
igor_v 0:8ad47e2b6f00 156 PSdif_sum_Vib_64 += Dif_Curr_32; //e. sum for scale factor measurement mode
igor_v 0:8ad47e2b6f00 157 }
igor_v 0:8ad47e2b6f00 158
igor_v 1:f2adcae3d304 159 dif_Curr_32_previous = Dif_Curr_32; //r. ���������� ���������� �����
igor_v 0:8ad47e2b6f00 160 break;
igor_v 0:8ad47e2b6f00 161
igor_v 0:8ad47e2b6f00 162 case RATE_REPER_OR_REFMEANDR:
igor_v 0:8ad47e2b6f00 163
igor_v 0:8ad47e2b6f00 164 if (data_Rdy & HALF_PERIOD) //e. calculate Cnt_Mns or Cnt_Pls
igor_v 0:8ad47e2b6f00 165 {
igor_v 0:8ad47e2b6f00 166 RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt;
igor_v 0:8ad47e2b6f00 167 Old_Cnt = Cnt_curr;
igor_v 0:8ad47e2b6f00 168 // LPC_GPIO2->FIOCLR = 0x10;
igor_v 0:8ad47e2b6f00 169 Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2);
igor_v 0:8ad47e2b6f00 170
igor_v 1:f2adcae3d304 171 if (LPC_QEI->STAT) //e. "+" direction //r. ����� ��������� � "+" �������
igor_v 0:8ad47e2b6f00 172 {
igor_v 0:8ad47e2b6f00 173 //sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec
igor_v 0:8ad47e2b6f00 174 Cnt_Mns = RefMeand_Cnt_Dif;
igor_v 0:8ad47e2b6f00 175 }
igor_v 0:8ad47e2b6f00 176 else
igor_v 0:8ad47e2b6f00 177 {
igor_v 1:f2adcae3d304 178 //r. ����� ��������� � "-" �������
igor_v 0:8ad47e2b6f00 179 //sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec
igor_v 0:8ad47e2b6f00 180 Cnt_Pls = -RefMeand_Cnt_Dif;
igor_v 0:8ad47e2b6f00 181 }
igor_v 0:8ad47e2b6f00 182
igor_v 0:8ad47e2b6f00 183 // UART1_SendByte((dif_sum_32>>8) & 0xff);
igor_v 0:8ad47e2b6f00 184 // UART1_SendByte((dif_sum_32) & 0xff);
igor_v 0:8ad47e2b6f00 185
igor_v 0:8ad47e2b6f00 186 if (data_Rdy & WHOLE_PERIOD) //e. period of vibro elapsed
igor_v 0:8ad47e2b6f00 187 {
igor_v 0:8ad47e2b6f00 188 // LPC_GPIO2->FIOSET = 0x10;
igor_v 0:8ad47e2b6f00 189 last_Cnt_Plus = Cnt_Pls;
igor_v 0:8ad47e2b6f00 190 dif_sum_32 += Cnt_Pls - Cnt_Mns;
igor_v 0:8ad47e2b6f00 191 }
igor_v 0:8ad47e2b6f00 192 data_Rdy &= ~RESET_PERIOD;
igor_v 0:8ad47e2b6f00 193 }
igor_v 0:8ad47e2b6f00 194 if (Latch_Rdy) //e it's time for output
igor_v 0:8ad47e2b6f00 195 {
igor_v 0:8ad47e2b6f00 196 LatchPhase = INT32_MAX;
igor_v 0:8ad47e2b6f00 197
igor_v 0:8ad47e2b6f00 198 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
igor_v 0:8ad47e2b6f00 199 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
igor_v 0:8ad47e2b6f00 200
igor_v 0:8ad47e2b6f00 201 cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus;
igor_v 0:8ad47e2b6f00 202
igor_v 0:8ad47e2b6f00 203 Output.Str.Cnt_Dif = dif_sum_32;
igor_v 0:8ad47e2b6f00 204 Output.Str.Cnt_Dif += cntPls_sum_32 >> 1;
igor_v 0:8ad47e2b6f00 205
igor_v 1:f2adcae3d304 206 dif_sum_32 = 0; //r. � �������� �������� ����������
igor_v 1:f2adcae3d304 207 ResetBitsOfWord(&cntPls_sum_32, 1); //r. ��������� ������� ������, ����� �� �������� �������� ��� ������������
igor_v 1:f2adcae3d304 208 preLast_Cnt_Plus = last_Cnt_Plus; //r. ������� ��������� ������ ���� ����������
igor_v 0:8ad47e2b6f00 209
igor_v 0:8ad47e2b6f00 210 Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns; //e. rewrite accumulated data to output
igor_v 0:8ad47e2b6f00 211 Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls;
igor_v 0:8ad47e2b6f00 212
igor_v 0:8ad47e2b6f00 213 //sumCnt_Mns = 0; //e. prepare for new accumulation
igor_v 0:8ad47e2b6f00 214 //sumCnt_Pls = 0;
igor_v 0:8ad47e2b6f00 215 }
igor_v 0:8ad47e2b6f00 216 break; // RATE_REPER_OR_REFMEANDR
igor_v 0:8ad47e2b6f00 217 }
igor_v 1:f2adcae3d304 218 //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - ���������� ��� �������� � ������ rate3
igor_v 0:8ad47e2b6f00 219 Output.Str.WP_scope1 = Dif_Curr_Vib;
igor_v 1:f2adcae3d304 220 Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 ������� ������� ��������� ��� ������� ����������� ��� ������� ������� rate3
igor_v 0:8ad47e2b6f00 221 }
igor_v 0:8ad47e2b6f00 222 // clc_Pulses
igor_v 0:8ad47e2b6f00 223
igor_v 0:8ad47e2b6f00 224
igor_v 0:8ad47e2b6f00 225 /******************************************************************************
igor_v 0:8ad47e2b6f00 226 ** Function name: SOI_Init
igor_v 0:8ad47e2b6f00 227 **
igor_v 0:8ad47e2b6f00 228 ** Descriptions: Quadrature encoder initialization.
igor_v 0:8ad47e2b6f00 229 **
igor_v 0:8ad47e2b6f00 230 ** parameters: None
igor_v 0:8ad47e2b6f00 231 ** Returned value: None
igor_v 0:8ad47e2b6f00 232 **
igor_v 0:8ad47e2b6f00 233 ******************************************************************************/
igor_v 0:8ad47e2b6f00 234 void SOI_Init(void)
igor_v 0:8ad47e2b6f00 235 {
igor_v 1:f2adcae3d304 236 LPC_SC->PCONP |= (1<<18);//0x00040000; ��������� ������������� ��������.
igor_v 1:f2adcae3d304 237 LPC_SC->PCLKSEL1 |= 0x00000001; // ����� ������� ��� ��.�. CLK=100MHz
igor_v 0:8ad47e2b6f00 238 LPC_PINCON->PINSEL3 &= ~0x4100;
igor_v 1:f2adcae3d304 239 LPC_PINCON->PINSEL3 |= 0x4100; //P1.20, p1.23 ���������� ��� ����� ��.�
igor_v 1:f2adcae3d304 240 LPC_PINCON->PINMODE3 |= 0x3C300; //P1.20, p1.23, p1.24 ??are pulled-down??(��������� � �������)????
igor_v 0:8ad47e2b6f00 241
igor_v 1:f2adcae3d304 242 LPC_QEI->MAXPOS = MAX_QEI_CNT; //LPC_QEI->"�������� ������������� ���������" = 2147483646
igor_v 1:f2adcae3d304 243 LPC_QEI->FILTER = 2; // ������( ?? )
igor_v 1:f2adcae3d304 244 LPC_QEI->CON = 0xF; //�������� � ���� ��� �������� ��.�. (��������, �����������, ������� � ��)
igor_v 0:8ad47e2b6f00 245 LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted
igor_v 0:8ad47e2b6f00 246
igor_v 1:f2adcae3d304 247 LPC_QEI->CLR = 0x1fff; //r. �������� ��� ����������
igor_v 1:f2adcae3d304 248 LPC_QEI->IEC = 0x1fff; //r. ��������� ���������� ��� ��������� �����������
igor_v 0:8ad47e2b6f00 249
igor_v 0:8ad47e2b6f00 250 NVIC_SetPriority(QEI_IRQn, 0);
igor_v 0:8ad47e2b6f00 251 NVIC_EnableIRQ(QEI_IRQn);
igor_v 0:8ad47e2b6f00 252 return;
igor_v 0:8ad47e2b6f00 253 }
igor_v 0:8ad47e2b6f00 254
igor_v 0:8ad47e2b6f00 255
igor_v 0:8ad47e2b6f00 256 /******************************************************************************
igor_v 0:8ad47e2b6f00 257 ** End Of File
igor_v 0:8ad47e2b6f00 258 ******************************************************************************/
igor_v 0:8ad47e2b6f00 259