Dmitry Kovalev
n
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;
+}
/******************************************************************************