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