Dmitry Kovalev / Mbed 2 deprecated LGstaandart

forkd

Dependencies:   mbed

Fork of LG2 by Dmitry Kovalev

host/Source/App/Dither_Regh.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?

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