123
Fork of LG by
Diff: DeviceDither.c
- Revision:
- 183:c7a9c309086c
- Parent:
- 182:2bd8ec44998f
--- a/DeviceDither.c Sun Jul 10 05:35:08 2016 +0000 +++ b/DeviceDither.c Sun Jul 31 06:19:02 2016 +0000 @@ -4,7 +4,7 @@ void InitDitherDefaultSettings(void){ //Init pulse default settings - device.dither.pulse.settings.width = 0x00001000; //0.125 + device.dither.pulse.settings.width = 0x00002000; //0.125 device.dither.pulse.settings.min = 0x00000400; //0.031 device.dither.pulse.settings.max = 0x00004000; //0.5 @@ -89,9 +89,9 @@ //Init frequency regulator default settings device.dither.frequency.settings.enabled = 1; - device.dither.frequency.settings.min = 0x01860000; //390 Hz in 16.16 format - device.dither.frequency.settings.max = 0x019A0000; //410 Hz in 16.16 format - device.dither.frequency.settings.scale = 0x00001000; //0.0625Hz + device.dither.frequency.settings.min = 0x01630000;//355Hz 0x01860000; //390 Hz in 16.16 format + device.dither.frequency.settings.max = 0x01680000;//365 0x019A0000; //410 Hz in 16.16 format + device.dither.frequency.settings.scale = 0x00010000; //0.0625Hz device.dither.frequency.settings.transfer.points = 16; device.dither.frequency.settings.transfer.error[0] = -0x140000; //-20.0Hz device.dither.frequency.settings.transfer.correction[0] = 0x10000; //1.0 @@ -214,12 +214,24 @@ } void DeviceStartDither(void){ + LPC_PINCON->PINSEL3 &= ~(0x00<<18); //e. P1.25 is GPIO pin + LPC_PINCON->PINSEL3 |= (0x00<<18); //e. P1.25 is GPIO pin + LPC_PINCON->PINMODE3 |= (3<<18); //e. P1.25 (включениe подтягивающего резистора") + LPC_GPIO1->FIODIR |= (1<<25); //e. P0.5 is output (запись ( 1 ) в 5 бит FIODIR выбор P0.5 как выход) + LPC_GPIO1->FIOCLR |= (1<<25); + + LPC_PINCON->PINSEL3 &= ~(0x00<<24); //e. P1.28 is GPIO pin + LPC_PINCON->PINSEL3 |= (0x00<<24); //e. P1.28 is GPIO pin + LPC_PINCON->PINMODE3 |= (3<<24); //e. P1.28 is GPIO pin (запись ( 11 ) в бит PINMODE0 "для включения подтягивающего резистора") + LPC_GPIO1->FIODIR |= (1<<28); //e. P1.28 is output (запись ( 1 ) в 5 бит FIODIR выбор P0.5 как выход) + LPC_GPIO1->FIOCLR |= (1<<28); + DeviceStartMeasurementTimer(); } //Call from regular cycle interrupt (or from main cycle?) void ditherCycle(void){ - uint8_t pin1, pin2; + int8_t pin1, pin2; //Use LPC_TIM1->TC instead of device.dither.pulse.state.counter? // device.measurement.state.counter + (LPC_TIM1->TC << 16)/LPC_TIM1->MR0 - cycle phase 0...32 in 16.16 format // "+": 100kHz timer will be just 10kHz timer if call DeviceDitherDoCycle() in main cycle @@ -228,6 +240,7 @@ } else { pin1 = 0; pin2 = 0; } + if (device.measurement.counter < 16){ pin2 = 0; } else { @@ -265,7 +278,7 @@ } } -int32_t ditherRestorePhase(uint32_t raw) { +int32_t ditherRestorePhase(int32_t raw) { int32_t r1, r2; for (uint8_t i = 1; i < device.dither.detector.settings.transfer.points; i++) { r1 = device.dither.detector.settings.transfer.raw[i - 1]; @@ -283,7 +296,7 @@ return device.dither.detector.settings.transfer.restored[device.dither.detector.settings.transfer.points - 1]; } -int32_t ditherFrequencyCorrection(uint32_t error) { +int32_t ditherFrequencyCorrection(int32_t error) { int32_t e1, e2; for (uint8_t i = 1; i < device.dither.frequency.settings.transfer.points; i++) { e1 = device.dither.frequency.settings.transfer.error[i - 1]; @@ -301,7 +314,7 @@ return device.dither.frequency.settings.transfer.correction[device.dither.frequency.settings.transfer.points - 1]; } -int32_t ditherPulseCorrection(uint32_t error) { +int32_t ditherPulseCorrection(int32_t error) { int32_t e1, e2; for (uint8_t i = 1; i < device.dither.amplitude.settings.transfer.points; i++) { e1 = device.dither.amplitude.settings.transfer.error[i - 1]; @@ -324,19 +337,21 @@ //x(t) = x0 * cos(w * t - fi) //fi = - arctan(const / (w0^2 - w^2)) int32_t delta = device.counters.dither.state.delta[device.measurement.counter]; - int32_t factor = device.dither.detector.settings.filter.factor[device.measurement.counter]; //-1...+1 in 16.16 format + int32_t factor = device.dither.detector.settings.filter.factor[(device.measurement.counter + 14) % 32]; //-1...+1 in 16.16 format device.dither.detector.state.sum += delta * factor; //-500...+500 in 16.16 format } void ditherProcessPhase(void) { //Raw phase -1...+1 in 16.16 format if (device.counters.dither.state.amplitude > 0) {//0...+2500 in 32.0 format (1MHz/400Hz) - device.dither.detector.state.phase = device.dither.detector.state.sum / device.counters.dither.state.amplitude; //-1...+1 in 16.16 format + device.dither.detector.state.raw = device.dither.detector.state.sum / device.counters.dither.state.amplitude; //-1...+1 in 16.16 format + device.dither.detector.state.sum = 0; } else { - device.dither.detector.state.phase = 0; + device.dither.detector.state.raw = 0; } //TODO: Transform required to actual phase in range -1...+1 or -Pi/2...+Pi/2 or -90...+90 degrees device.dither.detector.state.phase = ditherRestorePhase(device.dither.detector.state.raw); + //sprintf(device.service.buffer,"- %d\r\n", device.dither.detector.state.phase); WriteConcole(); //Development message } void ditherProcessFrequency(void) { @@ -365,6 +380,7 @@ device.dither.amplitude.state.correction = (device.dither.amplitude.state.scale * ditherPulseCorrection(device.dither.amplitude.state.error)) >> 16; if (device.dither.amplitude.state.enabled){ + //Debug!! device.dither.pulse.state.width += device.dither.amplitude.state.correction; if (device.dither.pulse.state.width < device.dither.pulse.state.min) device.dither.pulse.state.width = device.dither.pulse.state.min; if (device.dither.pulse.state.width > device.dither.pulse.state.max) device.dither.pulse.state.width = device.dither.pulse.state.max; @@ -387,14 +403,22 @@ } void ditherProcessPulse(void) { - int32_t max = device.controller.timer[0].state.MR0 / 2; + //Dither period: device.controller.timer[0].state.MR0 * 32 / CCLK + //Timer #1 period: device.controller.timer[1].state.MR0 / CCLK + //Timer #1 ticks in dither period: device.controller.timer[0].state.MR0 * 32 / device.controller.timer[1].state.MR0 + + //int32_t max = device.controller.timer[0].state.MR0 / 2; + int32_t max = device.controller.timer[0].state.MR0 * 16 / device.controller.timer[1].state.MR0; int32_t mid = max / 2; - int32_t half = (max * (device.dither.pulse.state.width + device.dither.noise.state.disturbance)) >> 17; //pulse half width in 10 mks ticks + //int32_t half = (max * (device.dither.pulse.state.width + device.dither.noise.state.disturbance)) >> 17; //pulse half width in 10 mks ticks + //int32_t half = (max * device.dither.pulse.state.width) >> 16; + int32_t half = max/3; //Debug!!! int32_t rise = mid - half; if (rise < 0) rise = 0; + int32_t fall = mid + half; if (fall > max) fall = max; device.dither.pulse.state.rise = rise; - int32_t fall = mid + half; if (fall > max) fall = max; device.dither.pulse.state.fall = fall; + /* int32_t delta = (max * (device.dither.amplitude.state.correction + device.dither.noise.state.disturbance)) >> 17; @@ -426,10 +450,14 @@ uint32_t ditherGetRawAmplitudeSetting(void) { //Value: dither output frequency F - Hz //Internal format: device.dither.amplitude.settings.reference - kHz in 16.16 format - //Output format: 2 * F - dimensionless units in 32.0 format - //TODO - uint32_t r = device.dither.amplitude.state.reference >> 15; - return r; + //Output format: 2 * 1000 * F - dimensionless units in 32.0 format + + //Frequency: device.dither.amplitude.settings.reference / 65536 + //Output: 1000 * device.dither.amplitude.settings.reference / 32768 + + int32_t r = device.dither.amplitude.state.reference * 1000 / 32768; + if (r < 0) r = 0; + return (uint32_t)r; } uint16_t ditherGetRawAmplitudeState(void) { @@ -438,7 +466,9 @@ //Output format: F/16 - dimensionless units in 16.0 format //Result: device.dither.amplitude.settings.frequency / 16 * 1000 / 65536 - uint32_t r = ((device.dither.amplitude.state.frequency >> 5) * 1000) >> 16; + int32_t r = ((device.dither.amplitude.state.frequency >> 5) * 1000) >> 16; + if (r < 0) r = 0; + if (r > 32767) r = 32767; return (uint16_t)r; } @@ -450,7 +480,9 @@ //Numerator: 7680000 * 256 = 7680000 << 8 = 0x75300000 < maxint32 //Denominator: F * 256 = device.dither.frequency.settings.frequency >> 8 = 0x15e00 ... 0x1c200 //Result: numerator/denominator = (7680000 * 256)/(F * 256) = 0x55b6 ... 0x42aa = 21942 ... 17066 < maxint16 - uint32_t d = 0x75300000 / ((device.dither.frequency.settings.max + device.dither.frequency.settings.min) >> 9); + int32_t d = 0x75300000 / ((device.dither.frequency.settings.max + device.dither.frequency.settings.min) >> 9); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -462,7 +494,9 @@ //Numerator: 7680000 * 256 = 7680000 << 8 = 0x75300000 < maxint32 //Denominator: F * 256 = device.dither.frequency.settings.max >> 8 = 0x15e00 ... 0x1c200 //Result: numerator/denominator = (7680000 * 256)/(F * 256) = 0x55b6 ... 0x42aa = 21942 ... 17066 < maxint16 - uint32_t d = 0x75300000 / (device.dither.frequency.settings.min >> 8); + int32_t d = 0x75300000 / (device.dither.frequency.settings.min >> 8); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -474,7 +508,9 @@ //Numerator: 7680000 * 256 = 7680000 << 8 = 0x75300000 < maxint32 //Denominator: F * 256 = device.dither.frequency.settings.frequency >> 8 = 0x15e00 ... 0x1c200 //Result: numerator/denominator = (7680000 * 256)/(F * 256) = 0x55b6 ... 0x42aa = 21942 ... 17066 < maxint16 - uint32_t d = 0x75300000 / (device.dither.frequency.settings.max >> 8); + int32_t d = 0x75300000 / (device.dither.frequency.settings.max >> 8); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -486,7 +522,9 @@ //Numerator: 7680000 * 256 = 7680000 << 8 = 0x75300000 < maxint32 //Denominator: F * 256 = device.dither.frequency.state.frequency >> 8 = 0x15e00 ... 0x1c200 //Result: numerator/denominator = (7680000 * 256)/(F * 256) = 0x55b6 ... 0x42aa = 21942 ... 17066 < maxint16 - uint32_t d = 0x75300000 / (device.dither.frequency.state.frequency >> 8); + int32_t d = 0x75300000 / (device.dither.frequency.state.frequency >> 8); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -498,7 +536,7 @@ //Now: Multiply 512 instead of sum for 1 sec and right shift 16 to transform in format 16.0 (shift 16 - 9 = 7) //Signed right shift implementation-defined!!! Required to replace right shift 7 to division on 128 for implementation independence? int32_t phase = device.dither.detector.state.phase / 128; - //TODO: make summation for 1 sec and tranform 16.0 + //TODO: make summation for 1 sec as in DSP-based program and tranform 16.0 return (int16_t)phase; } @@ -517,7 +555,9 @@ //Numerator: (768000 >> 5) * device.dither.pulse.settings.width = 0 ... 0x5dc00000 < maxint //Denominator: device.dither.frequency.settings.frequency >> 5 = 0xaf000 ... 0xe1000 //Result: numerator/denominator = 0 ... 0x892 < maxint16 - uint32_t d = 0x5dc0 * device.dither.pulse.settings.width / ((device.dither.frequency.settings.max + device.dither.frequency.settings.min) >> 6); + int32_t d = 0x5dc0 * device.dither.pulse.settings.width / ((device.dither.frequency.settings.max + device.dither.frequency.settings.min) >> 6); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -535,7 +575,9 @@ //Numerator: (768000 >> 5) * device.dither.pulse.settings.max = 0 ... 0x5dc00000 < maxint //Denominator: device.dither.frequency.settings.frequency >> 5 = 0xaf000 ... 0xe1000 //Result: numerator/denominator = 0 ... 0x892 < maxint16 - uint32_t d = 0x5dc0 * device.dither.pulse.settings.max / (device.dither.frequency.settings.min >> 5); + int32_t d = 0x5dc0 * device.dither.pulse.settings.max / (device.dither.frequency.settings.min >> 5); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -553,7 +595,9 @@ //Numerator: (768000 >> 5) * device.dither.pulse.settings.min = 0 ... 0x5dc00000 < maxint //Denominator: device.dither.frequency.settings.max >> 5 = 0xaf000 ... 0xe1000 //Result: numerator/denominator = 0 ... 0x892 < maxint16 - uint32_t d = 0x5dc0 * device.dither.pulse.settings.min / (device.dither.frequency.settings.max >> 5); + int32_t d = 0x5dc0 * device.dither.pulse.settings.min / (device.dither.frequency.settings.max >> 5); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; } @@ -571,6 +615,68 @@ //Numerator: (768000 >> 5) * device.dither.pulse.state.width = 0 ... 0x5dc00000 < maxint //Denominator: device.dither.frequency.state.frequency >> 5 = 0xaf000 ... 0xe1000 //Result: numerator/denominator = 0 ... 0x892 < maxint16 - uint32_t d = 0x5dc0 * device.dither.pulse.state.width / (device.dither.frequency.state.frequency >> 5); + int32_t d = 0x5dc0 * device.dither.pulse.state.width / (device.dither.frequency.state.frequency >> 5); + if (d < 0) d = 0; + if (d > 32767) d = 32767; return (uint16_t)d; +} + +//VBN_Tzd s16 adjusted noise period +//int32_t device.dither.noise.settings.period +uint16_t param_VBN_Tzd(void) { + //Value: dither noise period (fixed part) - T [s] + //Internal format: device.dither.noise.state.period - 0...100 of measurement cycles in 16.16 format + //Output format: T [s] * 10000 (in 10000 Hz periods) - dimensionless units in 16.0 format + + //Frequency: F = device.dither.frequency.state.frequency / 65536 + //Period: 1/F = 0.0022 ... 0.0028 s + //Output: O = device.dither.noise.state.period * 1/F * 10000 = + // = device.dither.noise.state.period * 10000 * 65536 / device.dither.frequency.state.frequency = + // = 65536 * 20000 / device.dither.frequency.state.frequency * device.dither.noise.state.period / 2 + + int32_t tzd = 0x4e200000 / device.dither.frequency.state.frequency * device.dither.noise.state.period / 2; + if (tzd < 0) tzd = 0; + if (tzd > 32767) tzd = 32767; + return (uint16_t)tzd; +} + +//VBN_Ran s16 range of the random component of noise +//int32_t device.dither.noise.settings.range +uint16_t param_VBN_Ran(void) { + //Value: dither noise period (random part) - T [s] + //Internal format: device.dither.noise.state.range - 0...100 of measurement cycles in 16.16 format + //Output format: T [s] * 10000 (in 10000 Hz periods) - dimensionless units in 16.0 format + + //Frequency: F = device.dither.frequency.state.frequency / 65536 + //Period: 1/F = 0.0022 ... 0.0028 s + //Output: O = device.dither.noise.state.range * 1/F * 10000 = + // = device.dither.noise.state.range * 10000 * 65536 / device.dither.frequency.state.frequency = + // = 65536 * 20000 / device.dither.frequency.state.frequency * device.dither.noise.state.range / 2 + + int32_t ran = 0x4e200000 / device.dither.frequency.state.frequency * device.dither.noise.state.range / 2; + if (ran < 0) ran = 0; + if (ran > 32767) ran = 32767; + return (uint16_t)ran; +} + +//VBN_k s16 adjusted noise constant: pulse width = VB_tau +/- VBN_k +//int32_t device.dither.noise.settings.amplitude +uint16_t param_VBN_k(void) { + //Value: dither pulse width variation (pulse noise) W - s + //Internal format: device.dither.noise.settings.amplitude - 0...1 of dither half-period in 16.16 format + //Output format: 15360000 * W - dimensionless units in 16.0 format + + //Frequency: F = device.dither.frequency.settings.frequency / 65536 + //Half-period: 1/(2 * F) = 0.0011 ... 0.0014 s + //Percentage: P = device.dither.noise.settings.amplitude / 65536 + //Pulse variation: W = P/(2 * F) + //Output: O = W * 1536000 = 768000 * device.dither.noise.settings.amplitude / device.dither.frequency.settings.frequency + + //Numerator: (768000 >> 5) * device.dither.noise.settings.amplitude = 0 ... 0x5dc00000 < maxint + //Denominator: device.dither.frequency.settings.frequency >> 5 = 0xaf000 ... 0xe1000 + //Result: numerator/denominator = 0 ... 0x892 < maxint16 + int32_t k = 0x5dc0 * device.dither.noise.settings.amplitude / ((device.dither.frequency.settings.max + device.dither.frequency.settings.min) >> 6); + if (k < 0) k = 0; + if (k > 32767) k = 32767; + return (uint16_t)k; } \ No newline at end of file