SIP.c

00001 
00002 #include "Global.h"
00003 
00004 
00005 
00006 
00007 uint32_t     Old_Cnt_Vib = 0;
00008 uint32_t     Old_Cnt = 0;
00009 int32_t  RefMeand_Cnt_Dif;
00010 int32_t  PSdif_sum_Vib_32 = 0;
00011 __int64  PSdif_sum_Vib_64 = 0;
00012 int32_t  dif_Curr_32_Ext;       //r. разность (число) для режима внешней защелки
00013 int32_t  dif_Curr_32_previous;  //e. Previous (in comparison with Dif_Curr_32) number //r. предыдущее (по сравнению с Dif_Curr_32) число
00014 int32_t temp22=0;
00015 //+++++++++++++++++++++++++++++++INPUT DATA++++++++++++++++++++++++++++++++++++++++++++++++
00016 uint32_t    Curr_Cnt_Vib;
00017 uint32_t    Cnt_curr;
00018 
00019 //+++++++++++++++++++++++++++++++variables for output++++++++++++++++++++++++++++++++++++++
00020 int32_t     Dif_Curr_Vib;       //e. current difference output for dithering control in LightUp mode and Dither regulator
00021 int32_t     Dif_Curr_32;        //e. current difference without dithering for dithering control
00022 
00023 #if (defined GLOBALRATE)//не входит
00024 
00025 //variables for rate mode
00026 int32_t  cntPls_sum_32;
00027 ссс int32_t   last_Cnt_Plus;
00028 int32_t  dif_sum_32;
00029 int32_t  Cnt_Pls;
00030 int32_t  Cnt_Mns;
00031 int32_t  preLast_Cnt_Plus;
00032 // uint32_t   sumCnt_Mns = 0;
00033 //uint32_t   sumCnt_Pls = 0;
00034 extern int32_t dif_cur_test[30];
00035 extern   unsigned ii;
00036 uint32_t halfPeriod = 0;
00037 #endif
00038 
00039 void ResetBitsOfWord(int * x32, int truncate_bits)
00040 {
00041     int hi_part;
00042 
00043     hi_part = *x32 >> truncate_bits;
00044     *x32 -= hi_part << truncate_bits;  //r. оставляем младшие 16 бит
00045 }
00046 /******************************************************************************
00047 ** Function name:       interpolation
00048 **
00049 ** Descriptions: количество итерполяций до появления внешней защелки
00050 **
00051 ** Returned value:  Number in moment of external latch  appearing
00052 ** parameters:      y_curr - current number, y_prev - number at one cycle before time
00053 **                  x_interp - moment of external latch appearing,
00054 
00055 **  Precision of interpolation is 1/8 of impulse (3 digits after point in 14.18 format)
00056 ******************************************************************************/
00057 int interpolation(int y_curr, int x_interp)
00058 {
00059     __int64 temp,temp3;
00060 
00061     temp = (__int64)y_curr *(__int64)x_interp;
00062     temp /= PrevPeriod;  //  void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c)
00063 
00064     temp3 = (__int64)y_curr *(__int64)(x_interp+1);
00065     temp3 /= PrevPeriod;
00066     temp22 = (int)temp3;
00067 
00068     return ((int)temp);
00069 } // interpolation
00070 /******************************************************************************
00071 ** Function name:       clc_Pulses
00072 **
00073 ** Descriptions:    Processing of information from SPOI
00074 **
00075 ** parameters:  None
00076 ** Returned value:  None
00077 **
00078 ******************************************************************************/
00079 
00080 void clc_Pulses()
00081 {
00082 
00083 #if (!defined GLOBALRATE)
00084     static   int32_t     cntPls_sum_32;
00085     static   int32_t     last_Cnt_Plus;
00086     static   int32_t     dif_sum_32;
00087     static   int32_t     Cnt_Pls;
00088     static   int32_t     Cnt_Mns;
00089     static   int32_t     preLast_Cnt_Plus;
00090 #endif
00091 
00092     Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //Вычисление приращения текущего счетчика импульсов.
00093 
00094     Old_Cnt_Vib = Curr_Cnt_Vib;                      //сохранение текущего счетчика импульсов для следующего цикла измерений
00095 
00096 
00097     Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2);  //проверка на переполнение (Dif_Curr_Vib).
00098     //                Uin           UpSat          DwnSat
00099 
00100     //#define   Cnt_Overload(Uin, UpSat, DwnSat)
00101     /*  if (Uin > UpSat)
00102 
00103            {
00104                      Uin -= INT32_MAX;
00105                  }
00106           if (Uin < DwnSat)
00107                 {
00108                  Uin += INT32_MAX;
00109               } */
00110 
00111 
00112     Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Точность фильтрации 1/(2^18)
00113 
00114 
00115 
00116 
00117     switch (RgConB) { //r. дополнительный регистр управления
00118         case RATE_VIBRO_1:                                              //r. разность вибросчетчиков после фильтра скользящнго среднего
00119 
00120             if (Latch_Rdy) { //e. latch has arrived
00121                 //dif_Curr_32_Ext разность (число) для режима внешней защелки
00122                 dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase );  //интерполяция
00123                 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
00124                 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
00125                 //r. добавляем к накопленной сумме интерполированный отсчет внешней защелки
00126                 PSdif_sum_Vib_32 += dif_Curr_32_Ext;
00127                 PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data
00128                 //count--;
00129 
00130                 //r. подготовить число для выдачи
00131                 Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum;    //r. из накопленного числа вычитаем накопленную термокомпенсационную составляющую
00132                 Output.Str.PS_dif = Output.Str.BINS_dif >> 16;
00133                 LatchPhase = INT32_MAX; //INT32_MAX=2147483647                //in Latch_Event it's indicator of latch appearing
00134                 Output.Str.SF_dif = PSdif_sum_Vib_64;
00135                 TermoCompens_Sum = 0;                                         //r. обнуляем накопленную термокомпенсацию для начала нового цикла накопления
00136 
00137                 if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable)) {
00138                     PSdif_sum_Vib_32 = 0;                                   //r. инициализировать новый цикл измерения по защелке
00139                     PSdif_sum_Vib_64 = 0;
00140                 } else
00141                     ResetBitsOfWord(&PSdif_sum_Vib_32, 16);
00142 
00143                 dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext;
00144 
00145                 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;
00146                 PSdif_sum_Vib_64 +=  dif_Curr_32_Ext; //сохранить остальные счетчики разницы для следующего такта цикла
00147 
00148             } else { //r. защелки на настоящий момент не было
00149                 //r. продолжаем накапливать сумму из внутренних отсчетов
00150                 PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ;
00151                 PSdif_sum_Vib_64 += Dif_Curr_32;     //e. sum for scale factor measurement mode
00152             }
00153 
00154             dif_Curr_32_previous = Dif_Curr_32; //r. запоминаем предыдущее число
00155             break;
00156 
00157         case RATE_REPER_OR_REFMEANDR:
00158 
00159             if (data_Rdy & HALF_PERIOD) {          //e. calculate Cnt_Mns or Cnt_Pls
00160                 RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt;
00161                 Old_Cnt = Cnt_curr;
00162                 //  LPC_GPIO2->FIOCLR = 0x10;
00163                 Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2);
00164 
00165                 if (LPC_QEI->STAT) {                    //e. "+" direction //r. стали вращаться в "+" сторону
00166                     //sumCnt_Mns += -RefMeand_Cnt_Dif;    //e. accumulation during 1 sec
00167                     Cnt_Mns = RefMeand_Cnt_Dif;
00168                 } else {
00169                     //r. стали вращаться в "-" сторону
00170                     //sumCnt_Pls += RefMeand_Cnt_Dif;       //e. accumulation during 1 sec
00171                     Cnt_Pls = -RefMeand_Cnt_Dif;
00172                 }
00173 
00174                 //  UART1_SendByte((dif_sum_32>>8) & 0xff);
00175                 //  UART1_SendByte((dif_sum_32) & 0xff);
00176 
00177                 if (data_Rdy & WHOLE_PERIOD) {     //e. period of vibro elapsed
00178                     // LPC_GPIO2->FIOSET = 0x10;
00179                     last_Cnt_Plus = Cnt_Pls;
00180                     dif_sum_32 += Cnt_Pls - Cnt_Mns;
00181                 }
00182                 data_Rdy &= ~RESET_PERIOD;
00183             }
00184             if (Latch_Rdy) {           //e it's time for output
00185                 LatchPhase = INT32_MAX;
00186 
00187                 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
00188                 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
00189 
00190                 cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus;
00191 
00192                 Output.Str.Cnt_Dif = dif_sum_32;
00193                 Output.Str.Cnt_Dif += cntPls_sum_32 >> 1;
00194 
00195                 dif_sum_32 = 0;                                 //r. и сбросить регистры накопления
00196                 ResetBitsOfWord(&cntPls_sum_32, 1);             //r. оставляем младший разряд, чтобы не терялась точность при суммировании
00197                 preLast_Cnt_Plus = last_Cnt_Plus;                   //r. текущий последний отсчет стал предыдущим
00198 
00199                 Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns;     //e. rewrite accumulated data to output
00200                 Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls;
00201 
00202                 //sumCnt_Mns = 0;      //e. prepare for new accumulation
00203                 //sumCnt_Pls = 0;
00204             }
00205             break; // RATE_REPER_OR_REFMEANDR
00206     }
00207     //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - переменные для контроля в режиме rate3
00208     Output.Str.WP_scope1 = Dif_Curr_Vib;
00209     Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 дробных разряда оставляем для большей наглядности при анализе сигнала rate3
00210 }
00211 // clc_Pulses
00212 
00213 
00214 /******************************************************************************
00215 ** Function name:       SOI_Init
00216 **
00217 ** Descriptions:        Quadrature encoder initialization.
00218 **
00219 ** parameters:          None
00220 ** Returned value:      None
00221 **
00222 ******************************************************************************/
00223 void SOI_Init(void)
00224 {
00225     LPC_SC->PCONP |= (1<<18);//0x00040000;      включение квадратурного энкодера.
00226     LPC_SC->PCLKSEL1 |= 0x00000001;    // выбор частоты для кв.э. CLK=100MHz
00227     LPC_PINCON->PINSEL3 &= ~0x4100;
00228     LPC_PINCON->PINSEL3 |= 0x4100;    //P1.20, p1.23 установить как входы кв.э
00229     LPC_PINCON->PINMODE3 |= 0x3C300;      //P1.20, p1.23, p1.24 ??are pulled-down??(притянуть к питанию)????
00230 
00231     LPC_QEI->MAXPOS = MAX_QEI_CNT;  //LPC_QEI->"регистор максимального положения" = 2147483646
00232     LPC_QEI->FILTER = 2;           // фильтр( ?? )
00233     LPC_QEI->CON = 0xF;             //сбросить в ноль все счетчики кв.э. (скорость, направление, позицию и тд)
00234     LPC_QEI->CONF = (0<<2) |(0<<1); //Quadrature inputs, no inverting,only A pulses are counted
00235 
00236     LPC_QEI->CLR = 0x1fff;           //r. сбросить все прерывания
00237     LPC_QEI->IEC = 0x1fff;           //r. запретить прерывание при изменении направления
00238 
00239     NVIC_SetPriority(QEI_IRQn , 0);
00240     NVIC_EnableIRQ(QEI_IRQn );
00241     return;
00242 }
00243 
00244 
00245 /******************************************************************************
00246 **                            End Of File
00247 ******************************************************************************/
00248