Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of LG by
Dither_Reg.c
- Committer:
- igor_v
- Date:
- 2016-01-30
- Revision:
- 0:8ad47e2b6f00
- Child:
- 1:f2adcae3d304
File content as of revision 0:8ad47e2b6f00:
#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 ******************************************************************************/