Dmitry Kovalev / Mbed 2 deprecated LG

n

Dependencies:   mbed

Fork of LG by igor Apu

Dither_Reg.c@0:8ad47e2b6f00, 2016-01-30 (annotated)

Committer:
igor_v
Date:
Sat Jan 30 13:00:39 2016 +0000
Revision:
0:8ad47e2b6f00
Child:
1:f2adcae3d304
2016_01_30;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 0:8ad47e2b6f00 1 #include "LPC17xx.h"
igor_v 0:8ad47e2b6f00 2 #include "mathDSP.h"
igor_v 0:8ad47e2b6f00 3 #include "InputOutput.h"
igor_v 0:8ad47e2b6f00 4 #include "CyclesSync.h"
igor_v 0:8ad47e2b6f00 5 #include "CntrlGLD.h"
igor_v 0:8ad47e2b6f00 6 #include "stdlib.h"
igor_v 0:8ad47e2b6f00 7 #include "SIP.h"
igor_v 0:8ad47e2b6f00 8 #include "console.h"
igor_v 0:8ad47e2b6f00 9
igor_v 0:8ad47e2b6f00 10
igor_v 0:8ad47e2b6f00 11 #define SHIFT_7680_12500 15 //e. 14 digits for 7680 to 12500 clock converting and 1 division digit
igor_v 0:8ad47e2b6f00 12 #define SHIFT_C_7680_12500 11 //
igor_v 0:8ad47e2b6f00 13 #define DITH_VBN_SHIFT 2 //e. //r. ���������� ����� (������� �� 4) ������������ ������� ������������, ����� ����� ����� �� �������������
igor_v 0:8ad47e2b6f00 14 #define DITH_VB_TAU_SHIFT 2
igor_v 0:8ad47e2b6f00 15
igor_v 0:8ad47e2b6f00 16
igor_v 0:8ad47e2b6f00 17
igor_v 0:8ad47e2b6f00 18 int32_t RI_diff; //e.input signal of "recovery" APS //r. ������� ������ "����������������" ���
igor_v 0:8ad47e2b6f00 19 int32_t MaxDelay;
igor_v 0:8ad47e2b6f00 20 int32_t VB_tau_Ins; //r. ���������� �������� ������� ������������� ���
igor_v 0:8ad47e2b6f00 21 int32_t VB_Nmin0; //r. ������� ��������� �������� ���������� ������� ��� ����������� Device_blk.Str.TemperNormal
igor_v 0:8ad47e2b6f00 22 int32_t VB_Nmax0; //r. �������� ��������� �������� ���������� ������� ��� ����������� Device_blk.Str.TemperNormal
igor_v 0:8ad47e2b6f00 23
igor_v 0:8ad47e2b6f00 24 uint32_t In_Flag = 0;
igor_v 0:8ad47e2b6f00 25 uint32_t SwitchCntInq = 0;
igor_v 0:8ad47e2b6f00 26
igor_v 0:8ad47e2b6f00 27 int32_t accum_error = 0;
igor_v 0:8ad47e2b6f00 28 int32_t ph_error = 0;
igor_v 0:8ad47e2b6f00 29 int32_t accum_error_old = 0;
igor_v 0:8ad47e2b6f00 30 int32_t PhaseShift;
igor_v 0:8ad47e2b6f00 31 int32_t temp2;
igor_v 0:8ad47e2b6f00 32 int32_t temp3;
igor_v 0:8ad47e2b6f00 33
igor_v 0:8ad47e2b6f00 34 #if defined DITHERSIM
igor_v 0:8ad47e2b6f00 35 int32_t timeDither = 0;
igor_v 0:8ad47e2b6f00 36 int32_t LIM0;
igor_v 0:8ad47e2b6f00 37 #endif
igor_v 0:8ad47e2b6f00 38 extern uint32_t Vibro_2_CountIn;
igor_v 0:8ad47e2b6f00 39 void clc_Noise_regulator(void);
igor_v 0:8ad47e2b6f00 40
igor_v 0:8ad47e2b6f00 41 /******************************************************************************
igor_v 0:8ad47e2b6f00 42 ** Function name: VibroDither_Set
igor_v 0:8ad47e2b6f00 43 **
igor_v 0:8ad47e2b6f00 44 ** Descriptions: Set period and pulse width for dither.
igor_v 0:8ad47e2b6f00 45 **
igor_v 0:8ad47e2b6f00 46 ** parameters: duration of vibro pulses, period of dither
igor_v 0:8ad47e2b6f00 47 ** Returned value: None
igor_v 0:8ad47e2b6f00 48 **
igor_v 0:8ad47e2b6f00 49 ******************************************************************************/
igor_v 0:8ad47e2b6f00 50 void VibroDither_Set()
igor_v 0:8ad47e2b6f00 51 {
igor_v 0:8ad47e2b6f00 52 //�����.������� N ������������ (������ ���������) �� = T_Vibro ������������ �������� ������������>>
igor_v 0:8ad47e2b6f00 53
igor_v 0:8ad47e2b6f00 54
igor_v 0:8ad47e2b6f00 55 Device_blk.Str.VB_N = Output.Str.T_Vibro;
igor_v 0:8ad47e2b6f00 56 LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;//#define SHIFT_7680_12500 15 �������� ��� ����������� ������� �� 7680 � 12500
igor_v 0:8ad47e2b6f00 57 #if defined DITHERSIM
igor_v 0:8ad47e2b6f00 58 6565 LIM0 = (Output.Str.T_Vibro*86)>>16;
igor_v 0:8ad47e2b6f00 59 #endif
igor_v 0:8ad47e2b6f00 60 #if !defined CONSTCYCLE
igor_v 0:8ad47e2b6f00 61 5655 SwitchCntInq = 1; //to enable inquiry timer reloading
igor_v 0:8ad47e2b6f00 62 #endif
igor_v 0:8ad47e2b6f00 63 }
igor_v 0:8ad47e2b6f00 64 /******************************************************************************
igor_v 0:8ad47e2b6f00 65 ** Function name: VibroDither_SwitchOn
igor_v 0:8ad47e2b6f00 66 **
igor_v 0:8ad47e2b6f00 67 ** Descriptions: VibroDither switching on.
igor_v 0:8ad47e2b6f00 68 **
igor_v 0:8ad47e2b6f00 69 ** parameters: None
igor_v 0:8ad47e2b6f00 70 ** Returned value: None
igor_v 0:8ad47e2b6f00 71 **
igor_v 0:8ad47e2b6f00 72 ******************************************************************************/
igor_v 0:8ad47e2b6f00 73 void VibroDither_SwitchOn()
igor_v 0:8ad47e2b6f00 74 {
igor_v 0:8ad47e2b6f00 75 LPC_MCPWM->CON_SET = 1<<8; //start vibro dither
igor_v 0:8ad47e2b6f00 76 }
igor_v 0:8ad47e2b6f00 77 /******************************************************************************
igor_v 0:8ad47e2b6f00 78 ** Function name: VibroDither_SwitchOff
igor_v 0:8ad47e2b6f00 79 **
igor_v 0:8ad47e2b6f00 80 ** Descriptions: VibroDither switching off.
igor_v 0:8ad47e2b6f00 81 **
igor_v 0:8ad47e2b6f00 82 ** parameters: None
igor_v 0:8ad47e2b6f00 83 ** Returned value: None
igor_v 0:8ad47e2b6f00 84 **
igor_v 0:8ad47e2b6f00 85 ******************************************************************************/
igor_v 0:8ad47e2b6f00 86 void VibroDither_SwitchOff()
igor_v 0:8ad47e2b6f00 87 {
igor_v 0:8ad47e2b6f00 88 LPC_MCPWM->CON_CLR = 1<<8; //stop vibro dither
igor_v 0:8ad47e2b6f00 89 }
igor_v 0:8ad47e2b6f00 90
igor_v 0:8ad47e2b6f00 91 /******************************************************************************
igor_v 0:8ad47e2b6f00 92 ** Function name: VB_MeanderDelay
igor_v 0:8ad47e2b6f00 93 **
igor_v 0:8ad47e2b6f00 94 ** Descriptions: Routine for addition of delay to meander
igor_v 0:8ad47e2b6f00 95 **
igor_v 0:8ad47e2b6f00 96 ** parameters: meander, delay magnitude, max delay
igor_v 0:8ad47e2b6f00 97 ** Returned value: delayed meander
igor_v 0:8ad47e2b6f00 98 **
igor_v 0:8ad47e2b6f00 99 ******************************************************************************/
igor_v 0:8ad47e2b6f00 100 int VB_MeanderDelay(int VB_Meander, int Delay100uS, int MaxDly)
igor_v 0:8ad47e2b6f00 101 {
igor_v 0:8ad47e2b6f00 102 static int poz_counter = 0, neg_counter = 0, flg_delay;
igor_v 0:8ad47e2b6f00 103
igor_v 0:8ad47e2b6f00 104 if (Delay100uS == 0)
igor_v 0:8ad47e2b6f00 105 {
igor_v 0:8ad47e2b6f00 106 return (VB_Meander);
igor_v 0:8ad47e2b6f00 107 }
igor_v 0:8ad47e2b6f00 108
igor_v 0:8ad47e2b6f00 109 if (Delay100uS > 0)
igor_v 0:8ad47e2b6f00 110 {
igor_v 0:8ad47e2b6f00 111 if (Delay100uS > MaxDly) { Delay100uS = MaxDly; }
igor_v 0:8ad47e2b6f00 112 if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ������������ ������������ �� �������� WP_ref ����� poz_sin_flag
igor_v 0:8ad47e2b6f00 113 {
igor_v 0:8ad47e2b6f00 114 neg_counter = 0;
igor_v 0:8ad47e2b6f00 115 poz_counter++;
igor_v 0:8ad47e2b6f00 116 }
igor_v 0:8ad47e2b6f00 117 else
igor_v 0:8ad47e2b6f00 118 {
igor_v 0:8ad47e2b6f00 119 poz_counter = 0;
igor_v 0:8ad47e2b6f00 120 neg_counter++;
igor_v 0:8ad47e2b6f00 121 }
igor_v 0:8ad47e2b6f00 122 if (poz_counter == Delay100uS) { flg_delay = 1; }
igor_v 0:8ad47e2b6f00 123 if (neg_counter == Delay100uS) { flg_delay = 0; }
igor_v 0:8ad47e2b6f00 124 }
igor_v 0:8ad47e2b6f00 125 else
igor_v 0:8ad47e2b6f00 126 {
igor_v 0:8ad47e2b6f00 127 Delay100uS = -Delay100uS;
igor_v 0:8ad47e2b6f00 128 if (Delay100uS > MaxDly) { Delay100uS = MaxDly; }
igor_v 0:8ad47e2b6f00 129 if (VB_Meander) //e. outgoing WP_flg flag, which delayed by the WP_ref //r. ������������ ������������ �� �������� WP_ref ����� poz_sin_flag
igor_v 0:8ad47e2b6f00 130 {
igor_v 0:8ad47e2b6f00 131 neg_counter = MaxDly + 1;
igor_v 0:8ad47e2b6f00 132 poz_counter--;
igor_v 0:8ad47e2b6f00 133 }
igor_v 0:8ad47e2b6f00 134 else
igor_v 0:8ad47e2b6f00 135 {
igor_v 0:8ad47e2b6f00 136 poz_counter = MaxDly + 1;
igor_v 0:8ad47e2b6f00 137 neg_counter--;
igor_v 0:8ad47e2b6f00 138 }
igor_v 0:8ad47e2b6f00 139 if (poz_counter == Delay100uS) { flg_delay = 0; }
igor_v 0:8ad47e2b6f00 140 if (neg_counter == Delay100uS) { flg_delay = 1; }
igor_v 0:8ad47e2b6f00 141 }
igor_v 0:8ad47e2b6f00 142 return (flg_delay);
igor_v 0:8ad47e2b6f00 143 } // VB_MeanderDelay
igor_v 0:8ad47e2b6f00 144 /******************************************************************************
igor_v 0:8ad47e2b6f00 145 ** Function name: VB_PhaseDetectorRate
igor_v 0:8ad47e2b6f00 146 **
igor_v 0:8ad47e2b6f00 147 ** Descriptions: Routine for accumulation of dither error
igor_v 0:8ad47e2b6f00 148 **
igor_v 0:8ad47e2b6f00 149 ** parameters: None
igor_v 0:8ad47e2b6f00 150 ** Returned value: None
igor_v 0:8ad47e2b6f00 151 **
igor_v 0:8ad47e2b6f00 152 ******************************************************************************/
igor_v 0:8ad47e2b6f00 153 int VB_PhaseDetectorRate(int PhaseDetInput, int IntegrateTime)
igor_v 0:8ad47e2b6f00 154 {
igor_v 0:8ad47e2b6f00 155
igor_v 0:8ad47e2b6f00 156 static int SampleAndHoldOut = 0, VB_PhasDet_integr = 0;
igor_v 0:8ad47e2b6f00 157
igor_v 0:8ad47e2b6f00 158 if (IntegrateTime == DEVICE_SAMPLE_RATE_uks)
igor_v 0:8ad47e2b6f00 159 {
igor_v 0:8ad47e2b6f00 160 SampleAndHoldOut = VB_PhasDet_integr;
igor_v 0:8ad47e2b6f00 161 VB_PhasDet_integr = 0;
igor_v 0:8ad47e2b6f00 162 }
igor_v 0:8ad47e2b6f00 163 else
igor_v 0:8ad47e2b6f00 164 {
igor_v 0:8ad47e2b6f00 165 VB_PhasDet_integr += PhaseDetInput;
igor_v 0:8ad47e2b6f00 166 }
igor_v 0:8ad47e2b6f00 167 return (SampleAndHoldOut);
igor_v 0:8ad47e2b6f00 168 } // VB_PhaseDetectorRate
igor_v 0:8ad47e2b6f00 169
igor_v 0:8ad47e2b6f00 170 /*r.
igor_v 0:8ad47e2b6f00 171
igor_v 0:8ad47e2b6f00 172 DelayedDithMeander - ����������� ������ (�� �������� VB_phs)
igor_v 0:8ad47e2b6f00 173 �����
igor_v 0:8ad47e2b6f00 174 VB_N - ����������� �������
igor_v 0:8ad47e2b6f00 175 */
igor_v 0:8ad47e2b6f00 176 /******************************************************************************
igor_v 0:8ad47e2b6f00 177 ** Function name: clc_Dith_regulator
igor_v 0:8ad47e2b6f00 178 **
igor_v 0:8ad47e2b6f00 179 ** Descriptions: Routine for dither frequency controller
igor_v 0:8ad47e2b6f00 180 **
igor_v 0:8ad47e2b6f00 181 ** parameters: None
igor_v 0:8ad47e2b6f00 182 ** Returned value: None
igor_v 0:8ad47e2b6f00 183 **
igor_v 0:8ad47e2b6f00 184 ******************************************************************************/
igor_v 0:8ad47e2b6f00 185 void clc_Dith_regulator(void)
igor_v 0:8ad47e2b6f00 186 {
igor_v 0:8ad47e2b6f00 187 // static int smooth=0, buf[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, i = 0;
igor_v 0:8ad47e2b6f00 188 // int ph_error;
igor_v 0:8ad47e2b6f00 189 static int dith_period = 0;//, accum_error = 0;
igor_v 0:8ad47e2b6f00 190
igor_v 0:8ad47e2b6f00 191 RI_diff = DUP_Filt(Dif_Curr_Vib<<2);
igor_v 0:8ad47e2b6f00 192
igor_v 0:8ad47e2b6f00 193 if (RI_diff >= 0)
igor_v 0:8ad47e2b6f00 194 ph_error = 1;
igor_v 0:8ad47e2b6f00 195 else
igor_v 0:8ad47e2b6f00 196 ph_error = 0;
igor_v 0:8ad47e2b6f00 197
igor_v 0:8ad47e2b6f00 198 if (LPC_MCPWM->INTF & 0x0001) //vibro pulse has been formed
igor_v 0:8ad47e2b6f00 199 {
igor_v 0:8ad47e2b6f00 200 LPC_MCPWM->INTF_CLR |= 0x0001;
igor_v 0:8ad47e2b6f00 201 if (LPC_MCPWM->MAT2 > LPC_MCPWM->MAT1)
igor_v 0:8ad47e2b6f00 202 {
igor_v 0:8ad47e2b6f00 203 // LPC_GPIO2->FIOSET = 0x000000FF; // turn on the LED
igor_v 0:8ad47e2b6f00 204 if (SwitchCntInq) //inquiry cycle duration must be changed
igor_v 0:8ad47e2b6f00 205 {
igor_v 0:8ad47e2b6f00 206 LPC_PWM1->MR0 = (Output.Str.T_Vibro*Vibro_2_CountIn)>>SHIFT_C_7680_12500;
igor_v 0:8ad47e2b6f00 207 LPC_PWM1->LER = LER0_EN ; //e. enable updating of register
igor_v 0:8ad47e2b6f00 208 SwitchCntInq = 0;
igor_v 0:8ad47e2b6f00 209 }
igor_v 0:8ad47e2b6f00 210 LPC_MCPWM->MAT1 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
igor_v 0:8ad47e2b6f00 211 LPC_MCPWM->MAT2 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
igor_v 0:8ad47e2b6f00 212 In_Flag = 0;
igor_v 0:8ad47e2b6f00 213 }
igor_v 0:8ad47e2b6f00 214 else
igor_v 0:8ad47e2b6f00 215 {
igor_v 0:8ad47e2b6f00 216 // LPC_GPIO2->FIOCLR = 0x000000FF; // turn off the LED
igor_v 0:8ad47e2b6f00 217 LPC_MCPWM->MAT2 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500;
igor_v 0:8ad47e2b6f00 218 LPC_MCPWM->MAT1 = ((Output.Str.T_Vibro - Output.Str.L_Vibro)*MULT_7680_12500)>>SHIFT_7680_12500;
igor_v 0:8ad47e2b6f00 219 In_Flag = 1;
igor_v 0:8ad47e2b6f00 220 dith_period++;
igor_v 0:8ad47e2b6f00 221 }
igor_v 0:8ad47e2b6f00 222 }
igor_v 0:8ad47e2b6f00 223
igor_v 0:8ad47e2b6f00 224 temp3 = VB_MeanderDelay(In_Flag, Device_blk.Str.VB_phs, MaxDelay); //r. ������������ ������������ ������� ������
igor_v 0:8ad47e2b6f00 225 temp2 = ( ( temp3 ^ ph_error ) << 1 ) - 1; //r. ���������� ����� XOR ��(-1..+1, �.�. const=1)
igor_v 0:8ad47e2b6f00 226 accum_error += temp2;
igor_v 0:8ad47e2b6f00 227
igor_v 0:8ad47e2b6f00 228 Output.Str.T_VB_pll = VB_PhaseDetectorRate(temp2, time_1_Sec); //r. ������������ ������������������� �� 1 ��� ����������� ������� �� ������������
igor_v 0:8ad47e2b6f00 229 if ( dith_period > DITHER_REG_PERIOD ) //r. �������� ��������� �������� dith_period
igor_v 0:8ad47e2b6f00 230 {
igor_v 0:8ad47e2b6f00 231 dith_period = 0; //r. 40 �������� - ��������� �������� �������� ������������
igor_v 0:8ad47e2b6f00 232 //r. ��������������� � ������������ � ����������� � ����������
igor_v 0:8ad47e2b6f00 233 if ( loop_is_closed(VB_FREQ_ON) )
igor_v 0:8ad47e2b6f00 234 {
igor_v 0:8ad47e2b6f00 235 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;
igor_v 0:8ad47e2b6f00 236
igor_v 0:8ad47e2b6f00 237 Saturation(Device_blk.Str.VB_N, Device_blk.Str.VB_Nmax, Device_blk.Str.VB_Nmin); //r. �������� �������� ��������� �������������
igor_v 0:8ad47e2b6f00 238 accum_error = 0; //r. ��������� ����� _VB_Uab40
igor_v 0:8ad47e2b6f00 239 }
igor_v 0:8ad47e2b6f00 240 }
igor_v 0:8ad47e2b6f00 241
igor_v 0:8ad47e2b6f00 242 if ( loop_is_closed(VB_FREQ_ON) ) //r. ����� ���, ��������� ������� �� ������ ������������
igor_v 0:8ad47e2b6f00 243 {
igor_v 0:8ad47e2b6f00 244 Output.Str.T_Vibro = Device_blk.Str.VB_N;
igor_v 0:8ad47e2b6f00 245 LPC_MCPWM->LIM0 = (Output.Str.T_Vibro*MULT_7680_12500)>>SHIFT_7680_12500; //r. �������, ��������� ����������� �������� �������
igor_v 0:8ad47e2b6f00 246 }
igor_v 0:8ad47e2b6f00 247 // cyclic built-in test
igor_v 0:8ad47e2b6f00 248 if ((Output.Str.T_Vibro > Device_blk.Str.VB_Nmax) || (Output.Str.T_Vibro < Device_blk.Str.VB_Nmin))
igor_v 0:8ad47e2b6f00 249 {
igor_v 0:8ad47e2b6f00 250 Valid_Data |= DITH_FREQ_ERROR;
igor_v 0:8ad47e2b6f00 251 }
igor_v 0:8ad47e2b6f00 252 } // clc_Dith_regulator
igor_v 0:8ad47e2b6f00 253
igor_v 0:8ad47e2b6f00 254 /******************************************************************************
igor_v 0:8ad47e2b6f00 255 ** Function name: clc_OutFreq_regulator
igor_v 0:8ad47e2b6f00 256 **
igor_v 0:8ad47e2b6f00 257 ** Descriptions: Routine for output frequency controller
igor_v 0:8ad47e2b6f00 258 **
igor_v 0:8ad47e2b6f00 259 ** parameters: None
igor_v 0:8ad47e2b6f00 260 ** Returned value: None
igor_v 0:8ad47e2b6f00 261 **
igor_v 0:8ad47e2b6f00 262 ******************************************************************************/
igor_v 0:8ad47e2b6f00 263 void clc_OutFreq_regulator(void)
igor_v 0:8ad47e2b6f00 264 {
igor_v 0:8ad47e2b6f00 265 static int out_freq_sum = 0;
igor_v 0:8ad47e2b6f00 266 static int temp;
igor_v 0:8ad47e2b6f00 267 /*
igor_v 0:8ad47e2b6f00 268 if (Dif_Curr_Vib > 0) //e. angular speed > 0 //r.�������� ������������
igor_v 0:8ad47e2b6f00 269 {
igor_v 0:8ad47e2b6f00 270 if (RI_diff > 0)
igor_v 0:8ad47e2b6f00 271 out_freq_sum += (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
igor_v 0:8ad47e2b6f00 272 else
igor_v 0:8ad47e2b6f00 273 out_freq_sum -= (Dif_Curr_Vib - (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
igor_v 0:8ad47e2b6f00 274 }
igor_v 0:8ad47e2b6f00 275 else //e. angular speed < 0 //r.�������� ������������
igor_v 0:8ad47e2b6f00 276 {
igor_v 0:8ad47e2b6f00 277 if (RI_diff < 0)
igor_v 0:8ad47e2b6f00 278 out_freq_sum += (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
igor_v 0:8ad47e2b6f00 279 else
igor_v 0:8ad47e2b6f00 280 out_freq_sum -= (Dif_Curr_Vib + (int)(Dif_Curr_32 >> SHIFT_TO_FRACT));
igor_v 0:8ad47e2b6f00 281 }
igor_v 0:8ad47e2b6f00 282 */
igor_v 0:8ad47e2b6f00 283 if(Dif_Curr_Vib>0)
igor_v 0:8ad47e2b6f00 284 out_freq_sum += Dif_Curr_Vib;
igor_v 0:8ad47e2b6f00 285 else
igor_v 0:8ad47e2b6f00 286 out_freq_sum -= Dif_Curr_Vib;
igor_v 0:8ad47e2b6f00 287
igor_v 0:8ad47e2b6f00 288
igor_v 0:8ad47e2b6f00 289 if (time_1_Sec == DEVICE_SAMPLE_RATE_uks) //e. second has elapsed, fix the output frequency value //r. ������� ������, ������������� �������� ������� �����������
igor_v 0:8ad47e2b6f00 290 {
igor_v 0:8ad47e2b6f00 291 if (loop_is_closed(VB_TAU_ON)) //e. the regulator loop is closed //r. ������ �������
igor_v 0:8ad47e2b6f00 292 {
igor_v 0:8ad47e2b6f00 293 temp = Device_blk.Str.VB_Fdf_Hi << 16;
igor_v 0:8ad47e2b6f00 294 temp |= Device_blk.Str.VB_Fdf_Lo;
igor_v 0:8ad47e2b6f00 295 temp = L_sub(out_freq_sum, temp) >> 3; // (out_freq_sum - temp) with saturation, then >> 3
igor_v 0:8ad47e2b6f00 296 Saturation(temp, 32767, -32768); // error saturation if error is out of range
igor_v 0:8ad47e2b6f00 297 //e. scaling and summing with rounding and saturation //r. ��������������� � ������������ � ����������� � ����������
igor_v 0:8ad47e2b6f00 298 VB_tau_Ins = mac_r( VB_tau_Ins << (16 - DITH_VB_TAU_SHIFT),
igor_v 0:8ad47e2b6f00 299 temp,
igor_v 0:8ad47e2b6f00 300 Device_blk.Str.VB_Fsc ); // << DITH_VB_TAU_SHIFT;
igor_v 0:8ad47e2b6f00 301 //e. reduction the VB_Err value to 16 digits (arithmetic right shift to 3 digits) //r. �������� �������� VB_Err � 16 �������� (�������������� ����� ������ �� 3 �������)
igor_v 0:8ad47e2b6f00 302
igor_v 0:8ad47e2b6f00 303 Saturation(VB_tau_Ins, \
igor_v 0:8ad47e2b6f00 304 (int)Device_blk.Str.VB_Tmax >> DITH_VB_TAU_SHIFT, \
igor_v 0:8ad47e2b6f00 305 (int)Device_blk.Str.VB_Tmin >> DITH_VB_TAU_SHIFT); //e. checking upper and lower levels in sign range
igor_v 0:8ad47e2b6f00 306 VB_tau_Ins <<= DITH_VB_TAU_SHIFT;
igor_v 0:8ad47e2b6f00 307 }
igor_v 0:8ad47e2b6f00 308
igor_v 0:8ad47e2b6f00 309 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, ����� ������� � ������������
igor_v 0:8ad47e2b6f00 310 out_freq_sum = 0; //e. reset accumulated values for next cycle of measurement //r. �������� ����������� �������� ��� ���������� ����� ���������
igor_v 0:8ad47e2b6f00 311
igor_v 0:8ad47e2b6f00 312 // cyclic built-in test
igor_v 0:8ad47e2b6f00 313 // if output frequency is less than 3/4 of nominal then data is invalid
igor_v 0:8ad47e2b6f00 314 if (Output.Str.F_ras < ((temp >> 7)*3))
igor_v 0:8ad47e2b6f00 315 {
igor_v 0:8ad47e2b6f00 316 Valid_Data |= OUT_FREQ_ERROR;
igor_v 0:8ad47e2b6f00 317 }
igor_v 0:8ad47e2b6f00 318 else
igor_v 0:8ad47e2b6f00 319 {
igor_v 0:8ad47e2b6f00 320 Valid_Data &= ~OUT_FREQ_ERROR;
igor_v 0:8ad47e2b6f00 321 }
igor_v 0:8ad47e2b6f00 322 }
igor_v 0:8ad47e2b6f00 323
igor_v 0:8ad47e2b6f00 324 clc_Noise_regulator();
igor_v 0:8ad47e2b6f00 325
igor_v 0:8ad47e2b6f00 326 if ( loop_is_closed(VB_TAU_ON) ) //r. ������ ������������ �������?
igor_v 0:8ad47e2b6f00 327 {
igor_v 0:8ad47e2b6f00 328 Output.Str.L_Vibro = Device_blk.Str.VB_tau; //r. ����� ��������� ����� ��������
igor_v 0:8ad47e2b6f00 329 //r. ������������ ��������� ������������
igor_v 0:8ad47e2b6f00 330 }
igor_v 0:8ad47e2b6f00 331
igor_v 0:8ad47e2b6f00 332 } // clc_OutFreq_regulator
igor_v 0:8ad47e2b6f00 333
igor_v 0:8ad47e2b6f00 334 //e. noise regulator //r. ������� ������������ ��������� ������������
igor_v 0:8ad47e2b6f00 335 /*r.
igor_v 0:8ad47e2b6f00 336 PeriodCount (VBN_Cnt) - ������� �������� ������� Meander.
igor_v 0:8ad47e2b6f00 337 Tnoise (VBN_Per)- ������� ������ ���������.
igor_v 0:8ad47e2b6f00 338 PeriodNoise (VBN_Tzd) - ������� ������ ���������, �������� �������������.
igor_v 0:8ad47e2b6f00 339 AmpNoise(VBN_Ran) - ������������ ��������� ������� ��������� (�������� �������������).
igor_v 0:8ad47e2b6f00 340 Delta (VBN_k) - ������� ��������� (�������� �������������).
igor_v 0:8ad47e2b6f00 341 Flag(VBN_Mod) - ���� ����� ��������� ���������.
igor_v 0:8ad47e2b6f00 342 Tu(VBN_Tau) - ������������ �������� �������������.
igor_v 0:8ad47e2b6f00 343 Tp(VBN_tau_Ins) - ������������ �������� �������������, ���������� �������� ����������� ������� �����������.
igor_v 0:8ad47e2b6f00 344 */
igor_v 0:8ad47e2b6f00 345
igor_v 0:8ad47e2b6f00 346 void clc_Noise_regulator(void)
igor_v 0:8ad47e2b6f00 347 {
igor_v 0:8ad47e2b6f00 348 int temp;
igor_v 0:8ad47e2b6f00 349 static uint32_t Flag = 0;
igor_v 0:8ad47e2b6f00 350 static int PeriodCount = 0, Tnoise = 0;
igor_v 0:8ad47e2b6f00 351
igor_v 0:8ad47e2b6f00 352 if ( PeriodCount >= Tnoise )
igor_v 0:8ad47e2b6f00 353 {
igor_v 0:8ad47e2b6f00 354 PeriodCount = 0;
igor_v 0:8ad47e2b6f00 355 srand(Device_blk.Str.VB_N);// Srand(������ ��������� ��) -������������� ���������� ��������� ����� � ������ (VB_N)
igor_v 0:8ad47e2b6f00 356 //�������� ������ ���������
igor_v 0:8ad47e2b6f00 357 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() );
igor_v 0:8ad47e2b6f00 358 if ( Flag ) //e. calculation +dF/-dF //r. ������ +dF/-dF
igor_v 0:8ad47e2b6f00 359 {
igor_v 0:8ad47e2b6f00 360 temp = Device_blk.Str.VBN_k; //r. 25 - �������� ��������� ���������
igor_v 0:8ad47e2b6f00 361 }
igor_v 0:8ad47e2b6f00 362 else
igor_v 0:8ad47e2b6f00 363 {
igor_v 0:8ad47e2b6f00 364 temp = -Device_blk.Str.VBN_k;
igor_v 0:8ad47e2b6f00 365 }
igor_v 0:8ad47e2b6f00 366 ///������������ �������� �� ���������
igor_v 0:8ad47e2b6f00 367 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
igor_v 0:8ad47e2b6f00 368 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. �������� �������� ��������� �������������
igor_v 0:8ad47e2b6f00 369 Flag = !Flag;
igor_v 0:8ad47e2b6f00 370 }
igor_v 0:8ad47e2b6f00 371 else
igor_v 0:8ad47e2b6f00 372 {
igor_v 0:8ad47e2b6f00 373 PeriodCount++;
igor_v 0:8ad47e2b6f00 374 }
igor_v 0:8ad47e2b6f00 375
igor_v 0:8ad47e2b6f00 376 } // clc_Noise_regulator
igor_v 0:8ad47e2b6f00 377 /******************************************************************************
igor_v 0:8ad47e2b6f00 378 ** Function name: VibroDither_Init
igor_v 0:8ad47e2b6f00 379 **
igor_v 0:8ad47e2b6f00 380 ** Descriptions: VibroDither initialization.
igor_v 0:8ad47e2b6f00 381 **
igor_v 0:8ad47e2b6f00 382 ** parameters: None
igor_v 0:8ad47e2b6f00 383 ** Returned value: None
igor_v 0:8ad47e2b6f00 384 **
igor_v 0:8ad47e2b6f00 385 ******************************************************************************/
igor_v 0:8ad47e2b6f00 386 void VibroDither_Init()
igor_v 0:8ad47e2b6f00 387 {
igor_v 0:8ad47e2b6f00 388 LPC_SC->PCONP |= 0x00020000; //��������� ���.
igor_v 0:8ad47e2b6f00 389 LPC_SC->PCLKSEL1 |= 0xC0000000; //CLK=12.5MHz ����� �������
igor_v 0:8ad47e2b6f00 390
igor_v 0:8ad47e2b6f00 391 /* P1.25,1.26 as PhA_vibro; P1.28,1.29 as PhB_vibro*///����� ����� ��� ���� (����) ������� ���(PhA � PhB).
igor_v 0:8ad47e2b6f00 392 // LPC_PINCON->PINSEL3 &= ~(0x3CF<<18);
igor_v 0:8ad47e2b6f00 393 // 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;
igor_v 0:8ad47e2b6f00 394 //������ ��� (MCOA � MCOB) ������ ����������.
igor_v 0:8ad47e2b6f00 395
igor_v 0:8ad47e2b6f00 396 LPC_MCPWM->CON_SET |= 1<<30; //e. set AC mode (Pha, PhB periods are set by LIM0 )
igor_v 0:8ad47e2b6f00 397 //�� ����� (3-� ������ �� �����) ��� ��� ����������
igor_v 0:8ad47e2b6f00 398 //������� ������� � ������� ������ ������ 0.
igor_v 0:8ad47e2b6f00 399
igor_v 0:8ad47e2b6f00 400 LPC_MCPWM->TC0 = 0;// ������������� (���������) ������� 0;
igor_v 0:8ad47e2b6f00 401 LPC_MCPWM->LIM0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; //������ ���(�����������).
igor_v 0:8ad47e2b6f00 402
igor_v 0:8ad47e2b6f00 403 LPC_MCPWM->MAT0 = (Device_blk.Str.VB_N*MULT_7680_12500)>>SHIFT_7680_12500; // set LPC_MCPWM->MAT0 for defineteness | ������������ ��������� ����������
igor_v 0:8ad47e2b6f00 404 LPC_MCPWM->MAT2 = (Device_blk.Str.VB_tau*MULT_7680_12500)>>SHIFT_7680_12500; // pulse width of the PhA dither drive | (MAT) ��� ���������� �������
igor_v 0:8ad47e2b6f00 405 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 | ��������, ��� �� ����������.
igor_v 0:8ad47e2b6f00 406
igor_v 0:8ad47e2b6f00 407 LPC_MCPWM->DT &= ~0x3FF; //e. reset dead timer register
igor_v 0:8ad47e2b6f00 408 LPC_MCPWM->INTEN_SET = 1; //e. enable lim0 interrupt
igor_v 0:8ad47e2b6f00 409
igor_v 0:8ad47e2b6f00 410 LPC_MCPWM->CON_SET |= (1<<8) |1 |(1<<16); //start PWM channel 0,1,2
igor_v 0:8ad47e2b6f00 411
igor_v 0:8ad47e2b6f00 412 VB_tau_Ins = Device_blk.Str.VB_tau; // VB_tau_Ins - ���������� �������� ������� ������������� ���
igor_v 0:8ad47e2b6f00 413
igor_v 0:8ad47e2b6f00 414 Output.Str.L_Vibro = Device_blk.Str.VB_tau; //to update the period and pulse duration for displaying
igor_v 0:8ad47e2b6f00 415 Output.Str.T_Vibro = Device_blk.Str.VB_N; //������ � �������� ����� ������������ � ������� ��������� ��� �����������
igor_v 0:8ad47e2b6f00 416 return;
igor_v 0:8ad47e2b6f00 417 }
igor_v 0:8ad47e2b6f00 418 /******************************************************************************
igor_v 0:8ad47e2b6f00 419 ** Function name: init_Dither_reg
igor_v 0:8ad47e2b6f00 420 **
igor_v 0:8ad47e2b6f00 421 ** Descriptions: Initialization of dither regulator.
igor_v 0:8ad47e2b6f00 422 **
igor_v 0:8ad47e2b6f00 423 ** parameters: None
igor_v 0:8ad47e2b6f00 424 ** Returned value: None
igor_v 0:8ad47e2b6f00 425 **
igor_v 0:8ad47e2b6f00 426 ******************************************************************************/
igor_v 0:8ad47e2b6f00 427 void init_Dither_reg()
igor_v 0:8ad47e2b6f00 428 {
igor_v 0:8ad47e2b6f00 429
igor_v 0:8ad47e2b6f00 430 init_VibroReduce(); // ������ ������������ (����� ��������)
igor_v 0:8ad47e2b6f00 431 Device_blk.Str.VB_N = 29538; //�����.������� N ������������ (������ ���������) �� (? �������� ������ ����� �� ������������ ��� ��� ���������� Vibro_Filter_Aperture � ����������� �������)
igor_v 0:8ad47e2b6f00 432 VibroDither_Init();// ����� ����� ��� ���� ������� ���(1-2(����� 0 ���� ���������)),������ ���,����� � ��.
igor_v 0:8ad47e2b6f00 433 VibroDither_SwitchOn(); //LPC_MCPWM->CON_SET = 1<<8; ����� ������� 1. ����������� 8 ���� mscon_set �������� 8 ��� � �������� mscon (PDF CTP. - 526)
igor_v 0:8ad47e2b6f00 434 init_BandPass(1.0/(float)Vibro_Filter_Aperture, 100.0/(float)DEVICE_SAMPLE_RATE_HZ, DUP); //�������� ������ (�� �� ����� ��� � � ���)��������� ������ ��� ��������� ������� ���������.
igor_v 0:8ad47e2b6f00 435 MaxDelay = Vibro_Filter_Aperture >> 1; //r. ����. �������� ������� ������������ (Vibro_Filter_Aperture ����������� � init_VibroReduce();)
igor_v 0:8ad47e2b6f00 436 CounterIquiryCycle_Init((Device_blk.Str.VB_N*Vibro_2_CountIn)>>SHIFT_C_7680_12500); //������� ������� ������ ��������, ������ ����������.
igor_v 0:8ad47e2b6f00 437 }
igor_v 0:8ad47e2b6f00 438 /******************************************************************************
igor_v 0:8ad47e2b6f00 439 ** End Of File
igor_v 0:8ad47e2b6f00 440 ******************************************************************************/
igor_v 0:8ad47e2b6f00 441