![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LG2 by
host/Source/App/Dither_Regh.c
- Committer:
- Kovalev_D
- Date:
- 2016-02-03
- Revision:
- 23:12e6183f04d4
File content as of revision 23:12e6183f04d4:
#include "lpc17xx.h" #include "mathDSP.h" #include "InputOutput.h" #include "CyclesSync.h" #include "CntrlGLD.h" #include "stdlib.h" #include "sip.h" //#define FREQ_NEW int out_freq_sum = 0; #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; //e. local value of the Tau regulator //r. âíóòðåííåå çíà÷åíèå êîíòóðà ðåãóëèðîâàíèÿ Òàó int32_t VB_Nmin0; //e. minimum of the output value of a regulator of the period for the Device_blk.Str.TemperNormal temperature //r. ìèíèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal int32_t VB_Nmax0; //e. maximum of the output value of a regulator of the period for the Device_blk.Str.TemperNormal //r. ìàêñèìóì âûõîäíîãî çíà÷åíèÿ ðåãóëÿòîðà ïåðèîäà äëÿ òåìïåðàòóðû Device_blk.Str.TemperNormal uint32_t In_Flag; 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 meandr1; int32_t temp2; int32_t temp3; 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() { Device_blk.Str.VB_N = Output.Str.T_Vibro; LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; SwitchCntInq = 1; } /****************************************************************************** ** 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 == 0) { SampleAndHoldOut = VB_PhasDet_integr; VB_PhasDet_integr = 0; } else { VB_PhasDet_integr += PhaseDetInput; } return (SampleAndHoldOut); } // VB_PhaseDetectorRate /*r. DelayedDithMeander - çàäåðæàííûé ìåàíäð (íà âåëè÷èíó VB_phs) Âûõîä VB_N - êîýôôèöèåíò äåëåíèÿ */ 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; #if defined FREQ_NEW static int RI_diff_old = 0; static int PhaseShift_Old = 0, accum_on = 0; #endif RI_diff = DUP_Filt(Dif_Curr_Vib<<2); if (RI_diff >= 0) { meandr1 = 1; #if defined FREQ_NEW if (RI_diff_old < 0) if (ph_error !=1) ph_error++; #else ph_error = 1; #endif } else { meandr1 = 0; #if !defined FREQ_NEW ph_error = 0; #endif } if (LPC_MCPWM->INTF & 0x0001) { LPC_MCPWM->INTF_CLR |= 0x0001; if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1) { // LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED if (SwitchCntInq) { 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 = 1; #if defined FREQ_NEW if (ph_error > (-1)) ph_error--; #endif } 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 = 0; dith_period++; } } #if !defined FREQ_NEW temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //e. outgoing of the delayed menader signal //r. ôîðìèðîâàíèå çàäåðæàííîãî ñèãíàëà ìåàíäð temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //e. the PD XOR analog out (-1..+1, since const=1) //r. àíàëîãîâûé âûõîä XOR ÔÄ(-1..+1, ò.ê. const=1) accum_error += temp2; #else if ((ph_error !=0) && (accum_on == 0)) // { accum_on = 1; PhaseShift += ph_error; } else if ((ph_error == 0) && (accum_on == 1)) { accum_on = 0; accum_error += (PhaseShift - PhaseShift_Old); PhaseShift_Old = PhaseShift; PhaseShift = 0; } else PhaseShift += ph_error; #endif Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //e. outgoing of the integrated for 1 Sec analog signal of the PD of the dither drive //r. ôîðìèðîâàíèå ïðîèíòåãðèðîâàííîãî çà 1 ñåê àíàëîãîâîãî ñèãíàëà ÔÄ âèáðîïðèâîäà if ( dith_period > DITHER_REG_PERIOD ) //e. checking status of the dith_period counter //r. ïðîâåðêà ñîñòîÿíèÿ ñ÷åò÷èêà dith_period { dith_period = 0; //e. 40 periods - resetting the counter of dither drive periods //r. 40 ïåðèîäîâ - îáíóëåíèå ñ÷åò÷èêà ïåðèîäîâ âèáðîïðèâîäà //e. scaling and summing with rounding and saturation //r. ìàñøòàáèðîâàíèå è ñóììèðîâàíèå ñ îêðóãëåíèåì è íàñûùåíèåì 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); //e. checking upper and lower levels of control range //r. ïðîâåðêà âåðõíåãî äèàïàçîíà ðåãóëèðîâàíèÿ accum_error = 0; //e. resetting the _VB_Uab40 sum //r. îáíóëåíèå ñóììû _VB_Uab40 } if ( loop_is_closed(VB_FREQ_ON) ) //e. it was, check the activation of the stabilization regulator //r. ôðîíò áûë, ïðîâåðèòü âêëþ÷åí ëè êîíòóð ñòàáèëèçàöèè { Output.Str.T_Vibro = Device_blk.Str.VB_N; LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //e. has switched on, load calculated values of period //r. âêëþ÷åí, çàãðóçèòü âû÷èñëåííûå çíà÷åíèÿ ïåðèîäà SwitchCntInq = 1; //e. enable loading counter inquiry timer at the next vibro halfperiod } // 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 //e. dither drive output frequency stabilization //r. ñòàáèëèçàöèÿ ÷àñòîòû ðàñùåïëåíèÿ âèáðîïðèâîäà 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 == 0) //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) ) //e. is stabilization regulator switched on? //r. êîíòóð ñòàáèëèçàöèè âêëþ÷åí? { Output.Str.L_Vibro = Device_blk.Str.VB_tau; //e. otherwise, load new value of //r. èíà÷å çàãðóçèòü íîâîå çíà÷åíèå //e. pulse width of the dither drive //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); 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; } 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; /* Turn On MCPWM PCLK */ LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*/ LPC_PINCON->PINSEL3 &= ~(0x3CF<<18); LPC_PINCON->PINSEL3 |= (0x145 << 18) |(1<<6)|(1<<12); LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 ) LPC_MCPWM->TC0 = 0; //e. initial time counter of channel 0 LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //e. period of the dither drive LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //e. set LPC_MCPWM->MAT0 for defineteness LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; //e. pulse width of the PhA dither drive LPC_MCPWM->MAT1 = ((Device_blk.Str.VB_N - Device_blk.Str.VB_tau)*MULT_7680_12500)>>SHIFT_7680_12500; //e. 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; return; } /****************************************************************************** ** Function name: init_Dither_reg ** ** Descriptions: Initialization of dither regulator. ** ** parameters: None ** Returned value: None ** ******************************************************************************/ void init_Dither_reg() { init_VibroReduce(); VibroDither_Init(); VibroDither_SwitchOn(); init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); MaxDelay = Vibro_Filter_Aperture >> 1; //e. maximal delay of the meander of the dither drive //r. ìàêñ. çàäåðæêà ìåàíäðà âèáðîïðèâîäà CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); } /****************************************************************************** ** End Of File ******************************************************************************/