SIP.c

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