Dmitry Kovalev / Mbed 2 deprecated LG2

fork

Dependencies:   mbed

Fork of LG by igor Apu

SIP.c@219:2d3475d0dd1b, 2017-09-27 (annotated)

Committer:
Kovalev_D
Date:
Wed Sep 27 13:09:24 2017 +0000
Revision:
219:2d3475d0dd1b
Parent:
112:4a96133a1311 
hnjtfgyy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 1:f2adcae3d304 1 #include "Global.h"
igor_v 1:f2adcae3d304 2
igor_v 0:8ad47e2b6f00 3
igor_v 0:8ad47e2b6f00 4
igor_v 21:bc8c1cec3da6 5
igor_v 21:bc8c1cec3da6 6 uint32_t Old_Cnt_Vib = 0;
igor_v 21:bc8c1cec3da6 7 uint32_t Old_Cnt = 0;
igor_v 21:bc8c1cec3da6 8 int32_t RefMeand_Cnt_Dif;
igor_v 21:bc8c1cec3da6 9 int32_t PSdif_sum_Vib_32 = 0;
igor_v 21:bc8c1cec3da6 10 __int64 PSdif_sum_Vib_64 = 0;
igor_v 21:bc8c1cec3da6 11 int32_t dif_Curr_32_Ext; //r. разность (число) для режима внешней защелки
igor_v 21:bc8c1cec3da6 12 int32_t dif_Curr_32_previous; //e. Previous (in comparison with Dif_Curr_32) number //r. предыдущее (по сравнению с Dif_Curr_32) число
igor_v 21:bc8c1cec3da6 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 21:bc8c1cec3da6 22 #if (defined GLOBALRATE)//не входит
igor_v 0:8ad47e2b6f00 23
igor_v 0:8ad47e2b6f00 24 //variables for rate mode
igor_v 21:bc8c1cec3da6 25 int32_t cntPls_sum_32;
igor_v 21:bc8c1cec3da6 26 ссс int32_t last_Cnt_Plus;
igor_v 21:bc8c1cec3da6 27 int32_t dif_sum_32;
igor_v 21:bc8c1cec3da6 28 int32_t Cnt_Pls;
igor_v 21:bc8c1cec3da6 29 int32_t Cnt_Mns;
igor_v 21:bc8c1cec3da6 30 int32_t preLast_Cnt_Plus;
igor_v 21:bc8c1cec3da6 31 // uint32_t sumCnt_Mns = 0;
igor_v 21:bc8c1cec3da6 32 //uint32_t sumCnt_Pls = 0;
igor_v 21:bc8c1cec3da6 33 extern int32_t dif_cur_test[30];
igor_v 21:bc8c1cec3da6 34 extern unsigned ii;
igor_v 21:bc8c1cec3da6 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 21:bc8c1cec3da6 40 int hi_part;
igor_v 0:8ad47e2b6f00 41
igor_v 21:bc8c1cec3da6 42 hi_part = *x32 >> truncate_bits;
igor_v 21:bc8c1cec3da6 43 *x32 -= hi_part << truncate_bits; //r. оставляем младшие 16 бит
igor_v 21:bc8c1cec3da6 44 }
igor_v 0:8ad47e2b6f00 45 /******************************************************************************
igor_v 0:8ad47e2b6f00 46 ** Function name: interpolation
igor_v 0:8ad47e2b6f00 47 **
igor_v 21:bc8c1cec3da6 48 ** Descriptions: количество итерполяций до появления внешней защелки
igor_v 0:8ad47e2b6f00 49 **
igor_v 21:bc8c1cec3da6 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 21:bc8c1cec3da6 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 21:bc8c1cec3da6 58 __int64 temp,temp3;
igor_v 0:8ad47e2b6f00 59
igor_v 21:bc8c1cec3da6 60 temp = (__int64)y_curr *(__int64)x_interp;
igor_v 21:bc8c1cec3da6 61 temp /= PrevPeriod; // void ServiceTime(void) ||| PrevPeriod = LPC_PWM1->MR0; ||| (CyclesSync.c)
igor_v 0:8ad47e2b6f00 62
igor_v 21:bc8c1cec3da6 63 temp3 = (__int64)y_curr *(__int64)(x_interp+1);
igor_v 21:bc8c1cec3da6 64 temp3 /= PrevPeriod;
igor_v 21:bc8c1cec3da6 65 temp22 = (int)temp3;
igor_v 0:8ad47e2b6f00 66
igor_v 21:bc8c1cec3da6 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 21:bc8c1cec3da6 72 ** Descriptions: Processing of information from SPOI
igor_v 0:8ad47e2b6f00 73 **
igor_v 21:bc8c1cec3da6 74 ** parameters: None
igor_v 21:bc8c1cec3da6 75 ** Returned value: None
igor_v 21:bc8c1cec3da6 76 **
igor_v 0:8ad47e2b6f00 77 ******************************************************************************/
Kovalev_D 112:4a96133a1311 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 21:bc8c1cec3da6 83 static int32_t cntPls_sum_32;
igor_v 21:bc8c1cec3da6 84 static int32_t last_Cnt_Plus;
igor_v 21:bc8c1cec3da6 85 static int32_t dif_sum_32;
igor_v 21:bc8c1cec3da6 86 static int32_t Cnt_Pls;
igor_v 21:bc8c1cec3da6 87 static int32_t Cnt_Mns;
igor_v 21:bc8c1cec3da6 88 static int32_t preLast_Cnt_Plus;
igor_v 0:8ad47e2b6f00 89 #endif
igor_v 0:8ad47e2b6f00 90
igor_v 21:bc8c1cec3da6 91 Dif_Curr_Vib = Curr_Cnt_Vib - Old_Cnt_Vib; //Вычисление приращения текущего счетчика импульсов.
igor_v 0:8ad47e2b6f00 92
igor_v 21:bc8c1cec3da6 93 Old_Cnt_Vib = Curr_Cnt_Vib; //сохранение текущего счетчика импульсов для следующего цикла измерений
igor_v 21:bc8c1cec3da6 94
igor_v 0:8ad47e2b6f00 95
igor_v 21:bc8c1cec3da6 96 Cnt_Overload(Dif_Curr_Vib, INT32MAX_DIV2, INT32MIN_DIV2); //проверка на переполнение (Dif_Curr_Vib).
igor_v 21:bc8c1cec3da6 97 // Uin UpSat DwnSat
igor_v 21:bc8c1cec3da6 98
igor_v 21:bc8c1cec3da6 99 //#define Cnt_Overload(Uin, UpSat, DwnSat)
igor_v 21:bc8c1cec3da6 100 /* if (Uin > UpSat)
igor_v 0:8ad47e2b6f00 101
igor_v 21:bc8c1cec3da6 102 {
igor_v 21:bc8c1cec3da6 103 Uin -= INT32_MAX;
igor_v 21:bc8c1cec3da6 104 }
igor_v 21:bc8c1cec3da6 105 if (Uin < DwnSat)
igor_v 21:bc8c1cec3da6 106 {
igor_v 21:bc8c1cec3da6 107 Uin += INT32_MAX;
Kovalev_D 112:4a96133a1311 108 }
igor_v 0:8ad47e2b6f00 109
igor_v 0:8ad47e2b6f00 110
igor_v 21:bc8c1cec3da6 111 Dif_Curr_32 = VibroReduce(Dif_Curr_Vib << SHIFT_TO_FRACT); // Точность фильтрации 1/(2^18)
igor_v 0:8ad47e2b6f00 112
igor_v 21:bc8c1cec3da6 113
igor_v 0:8ad47e2b6f00 114
igor_v 0:8ad47e2b6f00 115
igor_v 21:bc8c1cec3da6 116 switch (RgConB) { //r. дополнительный регистр управления
igor_v 21:bc8c1cec3da6 117 case RATE_VIBRO_1: //r. разность вибросчетчиков после фильтра скользящнго среднего
igor_v 21:bc8c1cec3da6 118
igor_v 21:bc8c1cec3da6 119 if (Latch_Rdy) { //e. latch has arrived
igor_v 21:bc8c1cec3da6 120 //dif_Curr_32_Ext разность (число) для режима внешней защелки
igor_v 21:bc8c1cec3da6 121 dif_Curr_32_Ext = interpolation(Dif_Curr_32, LatchPhase ); //интерполяция
igor_v 21:bc8c1cec3da6 122 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
igor_v 21:bc8c1cec3da6 123 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
igor_v 21:bc8c1cec3da6 124 //r. добавляем к накопленной сумме интерполированный отсчет внешней защелки
igor_v 21:bc8c1cec3da6 125 PSdif_sum_Vib_32 += dif_Curr_32_Ext;
igor_v 21:bc8c1cec3da6 126 PSdif_sum_Vib_64 += dif_Curr_32_Ext; //e. receive last data
igor_v 21:bc8c1cec3da6 127 //count--;
igor_v 21:bc8c1cec3da6 128
igor_v 21:bc8c1cec3da6 129 //r. подготовить число для выдачи
igor_v 21:bc8c1cec3da6 130 Output.Str.BINS_dif = PSdif_sum_Vib_32 - TermoCompens_Sum; //r. из накопленного числа вычитаем накопленную термокомпенсационную составляющую
igor_v 21:bc8c1cec3da6 131 Output.Str.PS_dif = Output.Str.BINS_dif >> 16;
igor_v 21:bc8c1cec3da6 132 LatchPhase = INT32_MAX; //INT32_MAX=2147483647 //in Latch_Event it's indicator of latch appearing
igor_v 21:bc8c1cec3da6 133 Output.Str.SF_dif = PSdif_sum_Vib_64;
igor_v 21:bc8c1cec3da6 134 TermoCompens_Sum = 0; //r. обнуляем накопленную термокомпенсацию для начала нового цикла накопления
igor_v 21:bc8c1cec3da6 135
igor_v 21:bc8c1cec3da6 136 if ((Device_Mode == DM_EXT_LATCH_DELTA_BINS_PULSE)||((Device_Mode == DM_EXT_LATCH_DELTA_SF_PULSE) && Ext_Latch_ResetEnable)) {
igor_v 21:bc8c1cec3da6 137 PSdif_sum_Vib_32 = 0; //r. инициализировать новый цикл измерения по защелке
igor_v 21:bc8c1cec3da6 138 PSdif_sum_Vib_64 = 0;
igor_v 21:bc8c1cec3da6 139 } else
igor_v 21:bc8c1cec3da6 140 ResetBitsOfWord(&PSdif_sum_Vib_32, 16);
igor_v 21:bc8c1cec3da6 141
igor_v 21:bc8c1cec3da6 142 dif_Curr_32_Ext = Dif_Curr_32 - temp22;//dif_Curr_32_Ext;
igor_v 21:bc8c1cec3da6 143
igor_v 21:bc8c1cec3da6 144 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 21:bc8c1cec3da6 145 PSdif_sum_Vib_64 += dif_Curr_32_Ext; //сохранить остальные счетчики разницы для следующего такта цикла
igor_v 21:bc8c1cec3da6 146
igor_v 21:bc8c1cec3da6 147 } else { //r. защелки на настоящий момент не было
igor_v 21:bc8c1cec3da6 148 //r. продолжаем накапливать сумму из внутренних отсчетов
igor_v 21:bc8c1cec3da6 149 PSdif_sum_Vib_32 += Dif_Curr_32; // PSdif_sum_Vib_32 += Dif_Curr_32 ;
igor_v 21:bc8c1cec3da6 150 PSdif_sum_Vib_64 += Dif_Curr_32; //e. sum for scale factor measurement mode
igor_v 21:bc8c1cec3da6 151 }
igor_v 21:bc8c1cec3da6 152
igor_v 21:bc8c1cec3da6 153 dif_Curr_32_previous = Dif_Curr_32; //r. запоминаем предыдущее число
igor_v 21:bc8c1cec3da6 154 break;
igor_v 21:bc8c1cec3da6 155
igor_v 21:bc8c1cec3da6 156 case RATE_REPER_OR_REFMEANDR:
igor_v 21:bc8c1cec3da6 157
igor_v 21:bc8c1cec3da6 158 if (data_Rdy & HALF_PERIOD) { //e. calculate Cnt_Mns or Cnt_Pls
igor_v 21:bc8c1cec3da6 159 RefMeand_Cnt_Dif = Cnt_curr - Old_Cnt;
igor_v 21:bc8c1cec3da6 160 Old_Cnt = Cnt_curr;
igor_v 21:bc8c1cec3da6 161 // LPC_GPIO2->FIOCLR = 0x10;
igor_v 21:bc8c1cec3da6 162 Cnt_Overload(RefMeand_Cnt_Dif, INT32MAX_DIV2, INT32MIN_DIV2);
igor_v 21:bc8c1cec3da6 163
igor_v 21:bc8c1cec3da6 164 if (LPC_QEI->STAT) { //e. "+" direction //r. стали вращаться в "+" сторону
igor_v 21:bc8c1cec3da6 165 //sumCnt_Mns += -RefMeand_Cnt_Dif; //e. accumulation during 1 sec
igor_v 21:bc8c1cec3da6 166 Cnt_Mns = RefMeand_Cnt_Dif;
igor_v 21:bc8c1cec3da6 167 } else {
igor_v 21:bc8c1cec3da6 168 //r. стали вращаться в "-" сторону
igor_v 21:bc8c1cec3da6 169 //sumCnt_Pls += RefMeand_Cnt_Dif; //e. accumulation during 1 sec
igor_v 21:bc8c1cec3da6 170 Cnt_Pls = -RefMeand_Cnt_Dif;
igor_v 21:bc8c1cec3da6 171 }
igor_v 21:bc8c1cec3da6 172
igor_v 21:bc8c1cec3da6 173 // UART1_SendByte((dif_sum_32>>8) & 0xff);
igor_v 21:bc8c1cec3da6 174 // UART1_SendByte((dif_sum_32) & 0xff);
igor_v 21:bc8c1cec3da6 175
igor_v 21:bc8c1cec3da6 176 if (data_Rdy & WHOLE_PERIOD) { //e. period of vibro elapsed
igor_v 21:bc8c1cec3da6 177 // LPC_GPIO2->FIOSET = 0x10;
igor_v 21:bc8c1cec3da6 178 last_Cnt_Plus = Cnt_Pls;
igor_v 21:bc8c1cec3da6 179 dif_sum_32 += Cnt_Pls - Cnt_Mns;
igor_v 21:bc8c1cec3da6 180 }
igor_v 21:bc8c1cec3da6 181 data_Rdy &= ~RESET_PERIOD;
igor_v 21:bc8c1cec3da6 182 }
igor_v 21:bc8c1cec3da6 183 if (Latch_Rdy) { //e it's time for output
igor_v 21:bc8c1cec3da6 184 LatchPhase = INT32_MAX;
igor_v 21:bc8c1cec3da6 185
igor_v 21:bc8c1cec3da6 186 Output.Str.Tmp_Out[2] = (int)((Curr_Cnt_Vib>>16)& 0xffff);
igor_v 21:bc8c1cec3da6 187 Output.Str.Tmp_Out[3] = (int)(Curr_Cnt_Vib & 0xffff);
igor_v 21:bc8c1cec3da6 188
igor_v 21:bc8c1cec3da6 189 cntPls_sum_32 += last_Cnt_Plus - preLast_Cnt_Plus;
igor_v 21:bc8c1cec3da6 190
igor_v 21:bc8c1cec3da6 191 Output.Str.Cnt_Dif = dif_sum_32;
igor_v 21:bc8c1cec3da6 192 Output.Str.Cnt_Dif += cntPls_sum_32 >> 1;
igor_v 21:bc8c1cec3da6 193
igor_v 21:bc8c1cec3da6 194 dif_sum_32 = 0; //r. и сбросить регистры накопления
igor_v 21:bc8c1cec3da6 195 ResetBitsOfWord(&cntPls_sum_32, 1); //r. оставляем младший разряд, чтобы не терялась точность при суммировании
igor_v 21:bc8c1cec3da6 196 preLast_Cnt_Plus = last_Cnt_Plus; //r. текущий последний отсчет стал предыдущим
igor_v 21:bc8c1cec3da6 197
igor_v 21:bc8c1cec3da6 198 Output.Str.Cnt_Mns = Cnt_Mns;//sumCnt_Mns; //e. rewrite accumulated data to output
igor_v 21:bc8c1cec3da6 199 Output.Str.Cnt_Pls = Cnt_Pls;//sumCnt_Pls;
igor_v 21:bc8c1cec3da6 200
igor_v 21:bc8c1cec3da6 201 //sumCnt_Mns = 0; //e. prepare for new accumulation
igor_v 21:bc8c1cec3da6 202 //sumCnt_Pls = 0;
igor_v 21:bc8c1cec3da6 203 }
igor_v 21:bc8c1cec3da6 204 break; // RATE_REPER_OR_REFMEANDR
igor_v 21:bc8c1cec3da6 205 }
igor_v 21:bc8c1cec3da6 206 //e. WP_scope1, WP_scope2 - variables for control in the Rate3 mode //r. WP_scope1, WP_scope2 - переменные для контроля в режиме rate3
igor_v 21:bc8c1cec3da6 207 Output.Str.WP_scope1 = Dif_Curr_Vib;
igor_v 21:bc8c1cec3da6 208 Output.Str.WP_scope2 = (Dif_Curr_32 >> (SHIFT_TO_FRACT-2)); //r. 2 дробных разряда оставляем для большей наглядности при анализе сигнала rate3
igor_v 21:bc8c1cec3da6 209 }
igor_v 21:bc8c1cec3da6 210 // clc_Pulses
igor_v 21:bc8c1cec3da6 211
Kovalev_D 112:4a96133a1311 212 */
igor_v 0:8ad47e2b6f00 213
igor_v 0:8ad47e2b6f00 214
igor_v 0:8ad47e2b6f00 215
igor_v 0:8ad47e2b6f00 216 /******************************************************************************
igor_v 0:8ad47e2b6f00 217 ** End Of File
igor_v 0:8ad47e2b6f00 218 ******************************************************************************/
igor_v 0:8ad47e2b6f00 219