Dmitry Kovalev / Mbed 2 deprecated LG

n

Dependencies:   mbed

Fork of LG by igor Apu

SIP.c@28:1c9acd3b224d, 2016-02-04 (annotated)

Committer:
Kovalev_D
Date:
Thu Feb 04 10:21:57 2016 +0000
Revision:
28:1c9acd3b224d
Parent:
21:bc8c1cec3da6 
13:21

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