![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LG2 by
Diff: host/Source/App/Dither_Reg.c
- Revision:
- 23:12e6183f04d4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/host/Source/App/Dither_Reg.c Wed Feb 03 10:44:42 2016 +0300 @@ -0,0 +1,440 @@ +#include "lpc17xx.h" +#include "mathDSP.h" +#include "InputOutput.h" +#include "CyclesSync.h" +#include "CntrlGLD.h" +#include "stdlib.h" +#include "sip.h" +#include "console.h" + + +#define SHIFT_7680_12500 15 //e. 14 digits for 7680 to 12500 clock converting and 1 division digit +#define SHIFT_C_7680_12500 11 // +#define DITH_VBN_SHIFT 2 //e. //r. îïðåäåëÿåò ñäâèã (äåëåíèå íà 4) êîýôôèöèåíòà äåëåíèÿ âèáðîïðèâîäà, ÷òîáû èìåòü çàïàñ íà ðåãóëèðîâàíèå +#define DITH_VB_TAU_SHIFT 2 + + + + int32_t RI_diff; //e.input signal of "recovery" APS //r. âõîäíîé ñèãíàë "âîññòàíîâëåííîãî" ÄÓÏ + int32_t MaxDelay; + int32_t VB_tau_Ins; //r. âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó + int32_t VB_Nmin0; //r. ìèíèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal + int32_t VB_Nmax0; //r. ìàêñèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal + +uint32_t In_Flag = 0; +uint32_t SwitchCntInq = 0; + + int32_t accum_error = 0; + int32_t ph_error = 0; + int32_t accum_error_old = 0; + int32_t PhaseShift; + int32_t temp2; + int32_t temp3; + +#if defined DITHERSIM + int32_t timeDither = 0; + int32_t LIM0; +#endif +extern uint32_t Vibro_2_CountIn; +void clc_Noise_regulator(void); + +/****************************************************************************** +** Function name: VibroDither_Set +** +** Descriptions: Set period and pulse width for dither. +** +** parameters: duration of vibro pulses, period of dither +** Returned value: None +** +******************************************************************************/ +void VibroDither_Set() +{ +//êîýôô.äåëåíèÿ N âèáðîïðèâîäà (ïåðèîä êîëåáàíèé) ÂÏ = T_Vibro äëèòåëüíîñòü èìïóëüñà âèáðîïðèâîäà>> + + + Device_blk.Str.VB_N = Output.Str.T_Vibro; + LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define SHIFT_7680_12500 15 ñìåøåíèå äëÿ êîíâåðòàöèè ÷àñòîòû èç 7680 â 12500 +#if defined DITHERSIM + 6565 LIM0 = (Output.Str.T_Vibro*86)>>16; +#endif +#if !defined CONSTCYCLE + 5655 SwitchCntInq = 1; //to enable inquiry timer reloading +#endif +} +/****************************************************************************** +** Function name: VibroDither_SwitchOn +** +** Descriptions: VibroDither switching on. +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void VibroDither_SwitchOn() +{ + LPC_MCPWM->CON_SET = 1<<8; //start vibro dither +} +/****************************************************************************** +** Function name: VibroDither_SwitchOff +** +** Descriptions: VibroDither switching off. +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void VibroDither_SwitchOff() +{ + LPC_MCPWM->CON_CLR = 1<<8; //stop vibro dither +} + +/****************************************************************************** +** Function name: VB_MeanderDelay +** +** Descriptions: Routine for addition of delay to meander +** +** parameters: meander, delay magnitude, max delay +** Returned value: delayed meander +** +******************************************************************************/ +int VB_MeanderDelay(int VB_Meander, int Delay100uS, int MaxDly) +{ + static int poz_counter = 0, neg_counter = 0, flg_delay; + + if (Delay100uS == 0) + { + return (VB_Meander); + } + + if (Delay100uS > 0) + { + if (Delay100uS > MaxDly) { Delay100uS = MaxDly; } + if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag + { + neg_counter = 0; + poz_counter++; + } + else + { + poz_counter = 0; + neg_counter++; + } + if (poz_counter == Delay100uS) { flg_delay = 1; } + if (neg_counter == Delay100uS) { flg_delay = 0; } + } + else + { + Delay100uS = -Delay100uS; + if (Delay100uS > MaxDly) { Delay100uS = MaxDly; } + if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ôîðìèðîâàíèå çàäåðæàííîãî íà âåëè÷èíó WP_ref ôëàãà poz_sin_flag + { + neg_counter = MaxDly + 1; + poz_counter--; + } + else + { + poz_counter = MaxDly + 1; + neg_counter--; + } + if (poz_counter == Delay100uS) { flg_delay = 0; } + if (neg_counter == Delay100uS) { flg_delay = 1; } + } + return (flg_delay); +} // VB_MeanderDelay +/****************************************************************************** +** Function name: VB_PhaseDetectorRate +** +** Descriptions: Routine for accumulation of dither error +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime) +{ + + static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0; + + if (IntegrateTime == DEVICE_SAMPLE_RATE_uks) + { + SampleAndHoldOut = VB_PhasDet_integr; + VB_PhasDet_integr = 0; + } + else + { + VB_PhasDet_integr += PhaseDetInput; + } + return (SampleAndHoldOut); +} // VB_PhaseDetectorRate + +/*r. + + DelayedDithMeander - çàäåðæàííûé ìåàíäð (íà âåëè÷èíó VB_phs) +Âûõîä + VB_N - êîýôôèöèåíò äåëåíèÿ +*/ +/****************************************************************************** +** Function name: clc_Dith_regulator +** +** Descriptions: Routine for dither frequency controller +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void clc_Dith_regulator(void) +{ +// static int smooth=0, buf[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, i = 0; +// int ph_error; + static int dith_period = 0;//, accum_error = 0; + + RI_diff = DUP_Filt(Dif_Curr_Vib<<2); + + if (RI_diff >= 0) + ph_error = 1; + else + ph_error = 0; + + if (LPC_MCPWM->INTF & 0x0001) //vibro pulse has been formed + { + LPC_MCPWM->INTF_CLR |= 0x0001; + if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1) + { +// LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED + if (SwitchCntInq) //inquiry cycle duration must be changed + { + LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500; + LPC_PWM1->LER = LER0_EN ; //e. enable updating of register + SwitchCntInq = 0; + } + LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; + LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; + In_Flag = 0; + } + else + { +// LPC_GPIO2->FIOCLR = 0x000000FF; // turn off the LED + LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; + LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500; + In_Flag = 1; + dith_period++; + } + } + + temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. ôîðìèðîâàíèå çàäåðæàííîãî ñèãíàëà ìåàíäð + temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //r. àíàëîãîâûé âûõîä XOR ÔÄ(-1..+1, ò.ê. const=1) + accum_error += temp2; + + Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. ôîðìèðîâàíèå ïðîèíòåãðèðîâàííîãî çà 1 ñåê àíàëîãîâîãî ñèãíàëà ÔÄ âèáðîïðèâîäà + if ( dith_period > DITHER_REG_PERIOD ) //r. ïðîâåðêà ñîñòîÿíèÿ ñ÷åò÷èêà dith_period + { + dith_period = 0; //r. 40 ïåðèîäîâ - îáíóëåíèå ñ÷åò÷èêà ïåðèîäîâ âèáðîïðèâîäà + //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì + if ( loop_is_closed(VB_FREQ_ON) ) + { + Device_blk.Str.VB_N = mac_r(Device_blk.Str.VB_N << (16 - DITH_VBN_SHIFT),-accum_error,Device_blk.Str.VB_scl) << DITH_VBN_SHIFT; + + Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin); //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ + accum_error = 0; //r. îáíóëåíèå ñóììû _VB_Uab40 + } + } + + if ( loop_is_closed(VB_FREQ_ON) ) //r. ôðîíò áûë, ïðîâåðèòü âêëþ÷åí ëè êîíòóð ñòàáèëèçàöèè + { + Output.Str.T_Vibro = Device_blk.Str.VB_N; + LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //r. âêëþ÷åí, çàãðóçèòü âû÷èñëåííûå çíà÷åíèÿ ïåðèîäà + } + // cyclic built-in test + if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin)) + { + Valid_Data |= DITH_FREQ_ERROR; + } +} // clc_Dith_regulator + +/****************************************************************************** +** Function name: clc_OutFreq_regulator +** +** Descriptions: Routine for output frequency controller +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void clc_OutFreq_regulator(void) +{ + static int out_freq_sum = 0; + static int temp; +/* + if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.ñêîðîñòü ïîëîæèòåëüíà + { + if (RI_diff > 0) + out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + else + out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + } + else //e. angular speed < 0 //r.ñêîðîñòü îòðèöàòåëüíà + { + if (RI_diff < 0) + out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + else + out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT)); + } +*/ + if(Dif_Curr_Vib>0) + out_freq_sum += Dif_Curr_Vib; + else + out_freq_sum -= Dif_Curr_Vib; + + + if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) //e. second has elapsed, fix the output frequency value //r. ñåêóíäà ïðîøëà, çàôèêñèðîâàòü çíà÷åíèå ÷àñòîòû ðàñùåïëåíèÿ + { + if (loop_is_closed(VB_TAU_ON)) //e. the regulator loop is closed //r. êîíòóð çàìêíóò + { + temp = Device_blk.Str.VB_Fdf_Hi << 16; + temp |= Device_blk.Str.VB_Fdf_Lo; + temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3 + Saturation(temp, 32767, -32768); // error saturation if error is out of range + //e. scaling and summing with rounding and saturation //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì + VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT), + temp, + Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT; + //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. ñâåäåíèå âåëè÷èíû VB_Err ê 16 ðàçðÿäàì (àðèôìåòè÷åñêèé ñäâèã âïðàâî íà 3 ðàçðÿäà) + + Saturation(VB_tau_Ins, \ + (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \ + (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT); //e. checking upper and lower levels in sign range + VB_tau_Ins <<= DITH_VB_TAU_SHIFT; + } + + Output.Str.F_ras = out_freq_sum >> 5; //e. once more divide output frequency by 2, in order to coincide with frequency meter //r. ïîäåëèòü ÷àñòîòó ðàñùåïëåíèÿ åùå íà 2, ÷òîáû ñîâïàëî ñ ÷àñòîòîìåðîì + out_freq_sum = 0; //e. reset accumulated values for next cycle of measurement //r. ñáðîñèòü íàêîïëåííûå çíà÷åíèÿ äëÿ ñëåäóþùåãî öèêëà èçìåðåíèÿ + + // cyclic built-in test + // if output frequency is less than 3/4 of nominal then data is invalid + if (Output.Str.F_ras < ((temp >> 7)*3)) + { + Valid_Data |= OUT_FREQ_ERROR; + } + else + { + Valid_Data &= ~OUT_FREQ_ERROR; + } + } + + clc_Noise_regulator(); + + if ( loop_is_closed(VB_TAU_ON) ) //r. êîíòóð ñòàáèëèçàöèè âêëþ÷åí? + { + Output.Str.L_Vibro = Device_blk.Str.VB_tau; //r. èíà÷å çàãðóçèòü íîâîå çíà÷åíèå + //r. äëèòåëüíîñòè èìïóëüñîâ âèáðîïðèâîäà + } + +} // clc_OutFreq_regulator + +//e. noise regulator //r. ñèñòåìà ýëåêòðîííîãî îøóìëåíèÿ âèáðîïðèâîäà +/*r. +PeriodCount (VBN_Cnt) - ñ÷åò÷èê ïåðèîäîâ ñèãíàëà Meander. +Tnoise (VBN_Per)- òåêóùèé ïåðèîä îøóìëåíèÿ. +PeriodNoise (VBN_Tzd) - ñðåäíèé ïåðèîä îøóìëåíèÿ, çàäàííûé ïîëüçîâàòåëåì. +AmpNoise(VBN_Ran) - ìàêñèìàëüíàÿ àìïëèòóäà ïåðèîäà îøóìëåíèÿ (çàäàåòñÿ ïîëüçîâàòåëåì). +Delta (VBN_k) - ãëóáèíà îøóìëåíèÿ (çàäàåòñÿ ïîëüçîâàòåëåì). +Flag(VBN_Mod) - ôëàã çíàêà èçìåíåíèÿ àìïëèòóäû. +Tu(VBN_Tau) - äëèòåëüíîñòü èìïóëüñà îäíîâèáðàòîðà. +Tp(VBN_tau_Ins) - äëèòåëüíîñòü èìïóëüñà îäíîâèáðàòîðà, çàäàâàåìàÿ ñèñòåìîé ðåãóëèðîâêè ÷àñòîòû ðàñùåïëåíèÿ. +*/ + +void clc_Noise_regulator(void) +{ + int temp; + static uint32_t Flag = 0; + static int PeriodCount = 0, Tnoise = 0; + + if ( PeriodCount >= Tnoise ) + { + PeriodCount = 0; + srand(Device_blk.Str.VB_N);// Srand(ïåðèîä êîëåáàíèé ÂÏ) -èíèöèàëèçàöèÿ ãåíåðàòîðà ñëó÷àéíûõ ÷èñåë ñ çåðíîì (VB_N) + //çàäàííûé ïåðèîä îøóìëåíèÿ + Tnoise = add( Device_blk.Str.VBN_Tzd, mult_r(Device_blk.Str.VBN_Ran, rand())); // Tnoise = Device_blk.Str.VBN_Tzd + MULT_RND_SAT( Device_blk.Str.VBN_Ran, rand() ); + if ( Flag ) //e. calculation +dF/-dF //r. ðàñ÷åò +dF/-dF + { + temp = Device_blk.Str.VBN_k; //r. 25 - çàäàííàÿ êîíñòàíòà îøóìëåíèÿ + } + else + { + temp = -Device_blk.Str.VBN_k; + } + ///Äëèòåëüíîñòü èìïóëüñà äî îøóìëåíèÿ + Device_blk.Str.VB_tau = add(VB_tau_Ins, (mult_r( VB_tau_Ins, temp ) << 1)); // VB_tau = VB_tau_Ins + VB_tau_Ins * temp; with saturation + Saturation(Device_blk.Str.VB_tau, Device_blk.Str.VB_Tmax, Device_blk.Str.VB_Tmin); //e. checking upper and lower levels of control range //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ + Flag = !Flag; + } + else + { + PeriodCount++; + } + +} // clc_Noise_regulator +/****************************************************************************** +** Function name: VibroDither_Init +** +** Descriptions: VibroDither initialization. +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void VibroDither_Init() +{ + LPC_SC->PCONP |= 0x00020000; //âêëþ÷åíèå ØÈÌ. + LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz âûáîð ÷àñòîòû + + /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///âûáîð íîæåê äëÿ äâóõ (òðåõ) êàíàëîâ ØÈÌ(PhA è PhB). + // LPC_PINCON->PINSEL3 &= ~(0x3CF<<18); + // LPC_PINCON->PINSEL3 |= (0x145 << 18) |(1<<6)|(1<<12);//P1.19 - MCOA0; P1.22 - MCOB0; P1.25 - MCOA1; P1.26 - MCOB1; P1.28 - MCOA2; P1.29 - MCOB2; + //âûõîäû ØÈÌ (MCOA è MCOB) ðàçíîé ïîëÿðíîñòè. + + LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 ) + //ÀÑ ðåæèì (3-õ ôàçíûé ÀÑ ðåæèì) âñå ØÈÌ èñïîëüçóþò + //ñ÷åò÷èê âðåìåíè è ðåãèñòð ïåðèîä êàíàëà 0. + + LPC_MCPWM->TC0 = 0;// èíèöèàëèçàöèÿ (îáíóëåíèå) òàéìåðà 0; + LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //ïåðèîä ØÈÌ(Âèðîïðèâîäà). + + LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | óñòàíîâëåíèå âðåìåííûõ èíòåðâàëîâ + LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) ïðè äîñòèæåíèè êîòîðûõ + LPC_MCPWM->MAT1 = ((Device_blk.Str.VB_N - Device_blk.Str.VB_tau)*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhB dither drive at first time | òàéìåðîì, ÷òî òî ïðîèñõîäèò. + + LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register + LPC_MCPWM->INTEN_SET = 1; //e. enable lim0 interrupt + + LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16); //start PWM channel 0,1,2 + + VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó + + Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying + Output.Str.T_Vibro = Device_blk.Str.VB_N; //çàïèñü â âûõîäíîé ìàñèâ äëèòåëüíîñòè è ïåðèóäà èìïóëüñîâ äëÿ îòîáðàæåíèÿ + return; +} +/****************************************************************************** +** Function name: init_Dither_reg +** +** Descriptions: Initialization of dither regulator. +** +** parameters: None +** Returned value: None +** +******************************************************************************/ +void init_Dither_reg() +{ + + init_VibroReduce(); // ðàñ÷åò êîýôèöèåíòîâ (âèáðî àïåðòóðû) + Device_blk.Str.VB_N = 29538; //êîýôô.äåëåíèÿ N âèáðîïðèâîäà (ïåðèîä êîëåáàíèé) ÂÏ (? çàäàåòñÿ òîëüêî ñäåñü íî èñïîëüçóåòñÿ óæå ïðè âû÷èñëåíèè Vibro_Filter_Aperture â ïðåäúèäóùåé ôóíêöèè) + VibroDither_Init();// Âûáîð íîæåê äëÿ äâóõ êàíàëîâ ØÈÌ(1-2(Êàíàë 0 òîæå îïðåäåëåí)),ïåðèîä ØÈÌ,ðåæèì è òä. + VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; ñòàðò òàéìåðà 1. âûñòàâëåíèå 8 áèòà mscon_set èçìåíÿåò 8 áèò â ðåãèñòðå mscon (PDF CTP. - 526) + init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //ëèíåéíûé ôèëüòð (òî æå ñàìîå ÷òî è â ÑÐÏ)ïîëîñîôîé ôèëüòð äëÿ âûäåëåíèÿ ÷àñòîòû êîëåáàíèÿ. + MaxDelay = Vibro_Filter_Aperture >> 1; //r. ìàêñ. çàäåðæêà ìåàíäðà âèáðîïðèâîäà (Vibro_Filter_Aperture îïðåäåëÿåòñ â init_VibroReduce();) + CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); //çàäàíèå ïåðèîäà ñáðîñà ñ÷åò÷èêà, çàïðåò ïðåðûâàíèÿ. +} +/****************************************************************************** +** End Of File +******************************************************************************/