123
Fork of LG by
Diff: DeviceDither.c
- Revision:
- 198:78dd6d14d108
- Parent:
- 197:7a05523bf588
--- a/DeviceDither.c Wed Oct 19 10:55:05 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,682 +0,0 @@ -#include "Device.h" -extern Device device; -extern unsigned int SystemCoreClock1; - -void InitDitherDefaultSettings(void){ - //Init pulse default settings - device.dither.pulse.settings.width = 0x00002000; //0.125 - device.dither.pulse.settings.min = 0x00000400; //0.031 - device.dither.pulse.settings.max = 0x00004000; //0.5 - - //Init noise default settings - device.dither.noise.settings.period = 20; - device.dither.noise.settings.range = 20; - device.dither.noise.settings.amplitude = 0x00000400; //0.031 - - //Init dither cycle default settings - device.dither.cycle.settings.enabled = 1; - - //Init phase detector default settings - device.dither.detector.settings.filter.factor[0] = -0x00010000; - device.dither.detector.settings.filter.factor[1] = -0x00010000; - device.dither.detector.settings.filter.factor[2] = -0x00010000; - device.dither.detector.settings.filter.factor[3] = -0x00010000; - device.dither.detector.settings.filter.factor[4] = -0x00010000; - device.dither.detector.settings.filter.factor[5] = -0x00010000; - device.dither.detector.settings.filter.factor[6] = -0x00010000; - device.dither.detector.settings.filter.factor[7] = -0x00010000; - - device.dither.detector.settings.filter.factor[8] = -0x00010000; - device.dither.detector.settings.filter.factor[9] = -0x00010000; - device.dither.detector.settings.filter.factor[10] = -0x00010000; - device.dither.detector.settings.filter.factor[11] = -0x00010000; - device.dither.detector.settings.filter.factor[12] = -0x00010000; - device.dither.detector.settings.filter.factor[13] = -0x00010000; - device.dither.detector.settings.filter.factor[14] = -0x00010000; - device.dither.detector.settings.filter.factor[15] = -0x00010000; - - device.dither.detector.settings.filter.factor[16] = 0x00010000; - device.dither.detector.settings.filter.factor[17] = 0x00010000; - device.dither.detector.settings.filter.factor[18] = 0x00010000; - device.dither.detector.settings.filter.factor[19] = 0x00010000; - device.dither.detector.settings.filter.factor[20] = 0x00010000; - device.dither.detector.settings.filter.factor[21] = 0x00010000; - device.dither.detector.settings.filter.factor[22] = 0x00010000; - device.dither.detector.settings.filter.factor[23] = 0x00010000; - - device.dither.detector.settings.filter.factor[24] = 0x00010000; - device.dither.detector.settings.filter.factor[25] = 0x00010000; - device.dither.detector.settings.filter.factor[26] = 0x00010000; - device.dither.detector.settings.filter.factor[27] = 0x00010000; - device.dither.detector.settings.filter.factor[28] = 0x00010000; - device.dither.detector.settings.filter.factor[29] = 0x00010000; - device.dither.detector.settings.filter.factor[30] = 0x00010000; - device.dither.detector.settings.filter.factor[31] = 0x00010000; - - device.dither.detector.settings.transfer.points = 16; - device.dither.detector.settings.transfer.raw[0] = -0x10000; //-1.0 - device.dither.detector.settings.transfer.restored[0] = -0x1921f; //-1.5707963267948966 - device.dither.detector.settings.transfer.raw[1] = -0xfa67; //-0.9781476007338056 - device.dither.detector.settings.transfer.restored[1] = -0x15c81; //-1.361356816555577 - device.dither.detector.settings.transfer.raw[2] = -0xe9de; //-0.9135454576426009 - device.dither.detector.settings.transfer.restored[2] = -0x126e4; //-1.1519173063162575 - device.dither.detector.settings.transfer.raw[3] = -0xcf1b; //-0.8090169943749475 - device.dither.detector.settings.transfer.restored[3] = -0xf146; //-0.9424777960769379 - device.dither.detector.settings.transfer.raw[4] = -0xab4c; //-0.6691306063588582 - device.dither.detector.settings.transfer.restored[4] = -0xbba8; //-0.7330382858376184 - device.dither.detector.settings.transfer.raw[5] = -0x8000; //-0.5 - device.dither.detector.settings.transfer.restored[5] = -0x860a; //-0.5235987755982989 - device.dither.detector.settings.transfer.raw[6] = -0x4f1b; //-0.3090169943749474 - device.dither.detector.settings.transfer.restored[6] = -0x506c; //-0.3141592653589793 - device.dither.detector.settings.transfer.raw[7] = -0x1ac2; //-0.10452846326765361 - device.dither.detector.settings.transfer.restored[7] = -0x1ace; //-0.10471975511965992 - device.dither.detector.settings.transfer.raw[8] = 0x1ac2; //0.10452846326765339 - device.dither.detector.settings.transfer.restored[8] = 0x1ace; //0.1047197551196597 - device.dither.detector.settings.transfer.raw[9] = 0x4f1b; //0.3090169943749474 - device.dither.detector.settings.transfer.restored[9] = 0x506c; //0.3141592653589793 - device.dither.detector.settings.transfer.raw[10] = 0x7fff; //0.49999999999999983 - device.dither.detector.settings.transfer.restored[10] = 0x860a; //0.5235987755982987 - device.dither.detector.settings.transfer.raw[11] = 0xab4c; //0.669130606358858 - device.dither.detector.settings.transfer.restored[11] = 0xbba8; //0.7330382858376181 - device.dither.detector.settings.transfer.raw[12] = 0xcf1b; //0.8090169943749475 - device.dither.detector.settings.transfer.restored[12] = 0xf146; //0.9424777960769379 - device.dither.detector.settings.transfer.raw[13] = 0xe9de; //0.9135454576426009 - device.dither.detector.settings.transfer.restored[13] = 0x126e4; //1.1519173063162573 - device.dither.detector.settings.transfer.raw[14] = 0xfa67; //0.9781476007338056 - device.dither.detector.settings.transfer.restored[14] = 0x15c81; //1.3613568165555767 - device.dither.detector.settings.transfer.raw[15] = 0x10000; //1.0 - device.dither.detector.settings.transfer.restored[15] = 0x1921f; //1.5707963267948966 - - //Init frequency regulator default settings - device.dither.frequency.settings.enabled = 1; - 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 - device.dither.frequency.settings.transfer.error[1] = -0x115555; //-17.333333333333332Hz - device.dither.frequency.settings.transfer.correction[1] = 0xb473; //0.7048888888888889 - device.dither.frequency.settings.transfer.error[2] = -0xeaaaa; //-14.666666666666668Hz - device.dither.frequency.settings.transfer.correction[2] = 0x7aa7; //0.47911111111111115 - device.dither.frequency.settings.transfer.error[3] = -0xc0000; //-12.0Hz - device.dither.frequency.settings.transfer.correction[3] = 0x4fdf; //0.31199999999999994 - device.dither.frequency.settings.transfer.error[4] = -0x95555; //-9.333333333333334Hz - device.dither.frequency.settings.transfer.correction[4] = 0x3161; //0.1928888888888889 - device.dither.frequency.settings.transfer.error[5] = -0x6aaaa; //-6.666666666666668Hz - device.dither.frequency.settings.transfer.correction[5] = 0x1c71; //0.11111111111111113 - device.dither.frequency.settings.transfer.error[6] = -0x40000; //-4.0Hz - device.dither.frequency.settings.transfer.correction[6] = 0xe56; //0.05599999999999999 - device.dither.frequency.settings.transfer.error[7] = -0x15555; //-1.3333333333333357Hz - device.dither.frequency.settings.transfer.correction[7] = 0x452; //0.016888888888888884 - device.dither.frequency.settings.transfer.error[8] = 0x15555; //1.3333333333333321Hz - device.dither.frequency.settings.transfer.correction[8] = -0x452; //-0.016888888888888884 - device.dither.frequency.settings.transfer.error[9] = 0x40000; //4.0Hz - device.dither.frequency.settings.transfer.correction[9] = -0xe56; //-0.05599999999999999 - device.dither.frequency.settings.transfer.error[10] = 0x6aaaa; //6.666666666666664Hz - device.dither.frequency.settings.transfer.correction[10] = -0x1c71; //-0.11111111111111108 - device.dither.frequency.settings.transfer.error[11] = 0x95555; //9.333333333333332Hz - device.dither.frequency.settings.transfer.correction[11] = -0x3161; //-0.1928888888888888 - device.dither.frequency.settings.transfer.error[12] = 0xc0000; //12.0kHz - device.dither.frequency.settings.transfer.correction[12] = -0x4fdf; //-0.31200000000000006 - device.dither.frequency.settings.transfer.error[13] = 0xeaaaa; //14.666666666666664Hz - device.dither.frequency.settings.transfer.correction[13] = -0x7aa7; //-0.47911111111111115 - device.dither.frequency.settings.transfer.error[14] = 0x115555; //17.33333333333333Hz - device.dither.frequency.settings.transfer.correction[14] = -0xb473; //-0.7048888888888889 - device.dither.frequency.settings.transfer.error[15] = 0x140000; //20.0Hz - device.dither.frequency.settings.transfer.correction[15] = -0x10000; //-1.0 - - //Init amplitude regulator default settings - device.dither.amplitude.settings.enabled = 1; - device.dither.amplitude.settings.scale = 0x00001000; //0.0625 (6.25%) of period - device.dither.amplitude.settings.transfer.points = 16; - device.dither.amplitude.settings.transfer.error[0] = -0x140000; //-20.0kHz - device.dither.amplitude.settings.transfer.correction[0] = 0x10000; //1.0 - device.dither.amplitude.settings.transfer.error[1] = -0x115555; //-17.333333333333332kHz - device.dither.amplitude.settings.transfer.correction[1] = 0xb473; //0.7048888888888889 - device.dither.amplitude.settings.transfer.error[2] = -0xeaaaa; //-14.666666666666668kHz - device.dither.amplitude.settings.transfer.correction[2] = 0x7aa7; //0.47911111111111115 - device.dither.amplitude.settings.transfer.error[3] = -0xc0000; //-12.0kHz - device.dither.amplitude.settings.transfer.correction[3] = 0x4fdf; //0.31199999999999994 - device.dither.amplitude.settings.transfer.error[4] = -0x95555; //-9.333333333333334kHz - device.dither.amplitude.settings.transfer.correction[4] = 0x3161; //0.1928888888888889 - device.dither.amplitude.settings.transfer.error[5] = -0x6aaaa; //-6.666666666666668kHz - device.dither.amplitude.settings.transfer.correction[5] = 0x1c71; //0.11111111111111113 - device.dither.amplitude.settings.transfer.error[6] = -0x40000; //-4.0kHz - device.dither.amplitude.settings.transfer.correction[6] = 0xe56; //0.05599999999999999 - device.dither.amplitude.settings.transfer.error[7] = -0x15555; //-1.3333333333333357kHz - device.dither.amplitude.settings.transfer.correction[7] = 0x452; //0.016888888888888884 - device.dither.amplitude.settings.transfer.error[8] = 0x15555; //1.3333333333333321kHz - device.dither.amplitude.settings.transfer.correction[8] = -0x452; //-0.016888888888888884 - device.dither.amplitude.settings.transfer.error[9] = 0x40000; //4.0kHz - device.dither.amplitude.settings.transfer.correction[9] = -0xe56; //-0.05599999999999999 - device.dither.amplitude.settings.transfer.error[10] = 0x6aaaa; //6.666666666666664kHz - device.dither.amplitude.settings.transfer.correction[10] = -0x1c71; //-0.11111111111111108 - device.dither.amplitude.settings.transfer.error[11] = 0x95555; //9.333333333333332kHz - device.dither.amplitude.settings.transfer.correction[11] = -0x3161; //-0.1928888888888888 - device.dither.amplitude.settings.transfer.error[12] = 0xc0000; //12.0kHz - device.dither.amplitude.settings.transfer.correction[12] = -0x4fdf; //-0.31200000000000006 - device.dither.amplitude.settings.transfer.error[13] = 0xeaaaa; //14.666666666666664kHz - device.dither.amplitude.settings.transfer.correction[13] = -0x7aa7; //-0.47911111111111115 - device.dither.amplitude.settings.transfer.error[14] = 0x115555; //17.33333333333333kHz - device.dither.amplitude.settings.transfer.correction[14] = -0xb473; //-0.7048888888888889 - device.dither.amplitude.settings.transfer.error[15] = 0x140000; //20.0kHz - device.dither.amplitude.settings.transfer.correction[15] = -0x10000; //-1.0 -} - -void InitDitherState(void){ - //Init pulse state - device.dither.pulse.state.width = device.dither.pulse.settings.width; - device.dither.pulse.state.min = device.dither.pulse.settings.min; - device.dither.pulse.state.max = device.dither.pulse.settings.max; - device.dither.pulse.state.rise = 0; - device.dither.pulse.state.fall = 0; - device.dither.pulse.state.counter = 0; - - //Init noise state - device.dither.noise.state.enabled = device.dither.noise.settings.enabled; - device.dither.noise.state.period = device.dither.noise.settings.period; - device.dither.noise.state.range = device.dither.noise.settings.range; - device.dither.noise.state.amplitude = device.dither.noise.settings.amplitude; - device.dither.noise.state.counter = 0; - device.dither.noise.state.trigger = 0; - device.dither.noise.state.disturbance = 0; - - //Init dither cycle state - device.dither.cycle.state.enabled = device.dither.cycle.settings.enabled; - device.dither.cycle.state.pin1 = 0; - device.dither.cycle.state.pin2 = 0; - - //Init phase detector state - device.dither.detector.state.sum = 0; - device.dither.detector.state.raw = 0; - device.dither.detector.state.phase = 0; - - //Init frequency regulator state - //TODO: min(temperature), max(temperature) - device.dither.frequency.state.enabled = device.dither.frequency.settings.enabled; - device.dither.frequency.state.min = device.dither.frequency.settings.min; - device.dither.frequency.state.max = device.dither.frequency.settings.max; - device.dither.frequency.state.scale = device.dither.frequency.settings.scale; - device.dither.frequency.state.frequency = (device.dither.frequency.state.max + device.dither.frequency.state.min) / 2; - device.dither.frequency.state.error = 0; - device.dither.frequency.state.correction = 0; - //Set measurement timer - timersSetMeasurementPeriod((unsigned int)(SystemCoreClock1/(device.dither.frequency.state.frequency >> 11))); - - //Init amplitude regulator state - device.dither.amplitude.state.enabled = device.dither.amplitude.settings.enabled; - device.dither.amplitude.state.reference = device.dither.amplitude.settings.reference; - device.dither.amplitude.state.scale = device.dither.amplitude.settings.scale; - device.dither.amplitude.state.frequency = 0; - device.dither.amplitude.state.error = 0; - device.dither.amplitude.state.correction = 0; -} - -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){ - 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 - if ((device.dither.pulse.state.counter > device.dither.pulse.state.rise) && (device.dither.pulse.state.counter < device.dither.pulse.state.fall)){ - pin1 = 1; pin2 = 1; - } else { - pin1 = 0; pin2 = 0; - } - - if (device.measurement.counter < 16){ - pin2 = 0; - } else { - pin1 = 0; - } - if (!device.dither.cycle.state.enabled) { - pin1 = 0; pin2 = 0; - } - if (pin1 > device.dither.cycle.state.pin1) LPC_GPIO1->FIOCLR = 1<<25; - if (pin1 < device.dither.cycle.state.pin1) LPC_GPIO1->FIOSET = 1<<25; - if (pin2 > device.dither.cycle.state.pin2) LPC_GPIO1->FIOCLR = 1<<28; - if (pin2 < device.dither.cycle.state.pin2) LPC_GPIO1->FIOSET = 1<<28; - device.dither.cycle.state.pin1 = pin1; - device.dither.cycle.state.pin2 = pin2; -} - -int32_t ditherInterpolate(int32_t a,int32_t a1,int32_t a2,int32_t b1,int32_t b2) { - int32_t ma, mb; - while (1) { - if (a1 == a) return b1; - if (a2 == a) return b2; - - ma = (a1 + a2) >> 1; - mb = (b1 + b2) >> 1; - - if (a < ma) { - if (a2 == ma) return mb; - if (b1 == mb) return mb; - a2 = ma; b2 = mb; - } else if (a > ma) { - if (a1 == ma) return mb; - if (b2 == mb) return mb; - a1 = ma; b1 = mb; - } else return mb; - } -} - -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]; - if (raw < r1) { - return device.dither.detector.settings.transfer.restored[i - 1]; - } - - r2 = device.dither.detector.settings.transfer.raw[i]; - if (raw < r2) { - int32_t p1 = device.dither.detector.settings.transfer.restored[i - 1]; - int32_t p2 = device.dither.detector.settings.transfer.restored[i]; - return ditherInterpolate(raw, r1, r2, p1, p2); - } - } - return device.dither.detector.settings.transfer.restored[device.dither.detector.settings.transfer.points - 1]; -} - -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]; - if (error < e1) { - return device.dither.frequency.settings.transfer.correction[i - 1]; - } - - e2 = device.dither.frequency.settings.transfer.error[i]; - if (error < e2) { - int32_t c1 = device.dither.frequency.settings.transfer.correction[i - 1]; - int32_t c2 = device.dither.frequency.settings.transfer.correction[i]; - return ditherInterpolate(error, e1, e2, c1, c2); - } - } - return device.dither.frequency.settings.transfer.correction[device.dither.frequency.settings.transfer.points - 1]; -} - -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]; - if (error < e1) { - return device.dither.amplitude.settings.transfer.correction[i - 1]; - } - - e2 = device.dither.amplitude.settings.transfer.error[i]; - if (error < e2) { - int32_t c1 = device.dither.amplitude.settings.transfer.correction[i - 1]; - int32_t c2 = device.dither.amplitude.settings.transfer.correction[i]; - return ditherInterpolate(error, e1, e2, c1, c2); - } - } - return device.dither.amplitude.settings.transfer.correction[device.dither.amplitude.settings.transfer.points - 1]; -} - -void ditherProcessDetector(void) { - //f(t) = f0 * cos(w * t) - //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 + 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.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.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) { - device.dither.frequency.state.error = -device.dither.detector.state.phase; - device.dither.frequency.state.correction = (device.dither.frequency.state.scale * ditherFrequencyCorrection(device.dither.frequency.state.error)) >> 16; - - if (device.dither.frequency.state.enabled){ - device.dither.frequency.state.frequency += device.dither.frequency.state.correction; - - if (device.dither.frequency.state.frequency > device.dither.frequency.state.max) { - device.dither.frequency.state.frequency = device.dither.frequency.state.max; - } else if (device.dither.frequency.state.frequency < device.dither.frequency.state.min) { - device.dither.frequency.state.frequency = device.dither.frequency.state.min; - } - - timersSetMeasurementPeriod((unsigned int)(SystemCoreClock1/(device.dither.frequency.state.frequency >> 11))); - } -} - -void ditherProcessAmplitude(void) { - //Frequency = device.counters.state.frequency * device.dither.frequency.state.frequency / 2; - // 500 * 400Hz (0x01900000) / 2 / 1000 = 100kHz (0x00640000) - // 500 * 0x19000000 / 2000 / 16 = 100kHz = 0x00640000 - device.dither.amplitude.state.frequency = (device.counters.dither.state.amplitude * ((device.dither.frequency.state.frequency << 4) / 2000)) >> 4; - device.dither.amplitude.state.error = device.dither.amplitude.state.frequency - device.dither.amplitude.state.reference; //0...200kHz 16.16 format - 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; - } -} - -void ditherProcessNoise(void) { - if (device.dither.noise.state.counter == device.dither.noise.state.trigger) { - //Set noise update state parameters - device.dither.noise.state.counter = 0; - device.dither.noise.state.trigger = device.dither.noise.state.period + rand() % device.dither.noise.state.range; - - //Set pulse width disturbance - if (device.dither.noise.state.enabled) { - device.dither.noise.state.disturbance = (device.dither.noise.state.amplitude * (-0x00010000 + 2 * rand())) >> 16; //-1...1 - } else { - device.dither.noise.state.disturbance = 0; - } - } -} - -void ditherProcessPulse(void) { - //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) >> 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; - device.dither.pulse.state.fall = fall; - - /* - int32_t delta = (max * (device.dither.amplitude.state.correction + device.dither.noise.state.disturbance)) >> 17; - - device.dither.pulse.state.rise += delta; - if (device.dither.pulse.state.rise < 0) device.dither.pulse.state.rise = 0; - if (device.dither.pulse.state.rise > mid) device.dither.pulse.state.rise = mid; - - device.dither.pulse.state.fall -= delta; - if (device.dither.pulse.state.fall > max) device.dither.pulse.state.fall = max; - if (device.dither.pulse.state.fall < mid) device.dither.pulse.state.fall = mid; - */ -} - -//Call from measurement timer interrupt -void ditherProcess(void) { - ditherProcessDetector(); - if (device.measurement.counter == 31) { - ditherProcessPhase(); - ditherProcessFrequency(); - ditherProcessAmplitude(); - ditherProcessNoise(); - ditherProcessPulse(); - } -} - -// -// Compatibility functions section -// -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 * 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) { - //Value: dither output frequency F - Hz - //Internal format: device.dither.amplitude.settings.frequency - kHz in 16.16 format - //Output format: F/16 - dimensionless units in 16.0 format - - //Result: device.dither.amplitude.settings.frequency / 16 * 1000 / 65536 - 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; -} - -uint16_t ditherGetRawDividerSetting(void) { - //Value: dither oscillation frequency F - Hz - //Internal format: device.dither.frequency.settings.frequency - Hz in 16.16 format - //Output format: frequency divider 7680000/F - dimensionless units in 16.0 format - - //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 - 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; -} - -uint16_t ditherGetRawDividerMaxSetting(void) { - //Value: dither oscillation min frequency F - Hz - //Internal format: device.dither.frequency.settings.min - Hz in 16.16 format - //Output format: frequency max divider 7680000/F - dimensionless units in 16.0 format - - //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 - int32_t d = 0x75300000 / (device.dither.frequency.settings.min >> 8); - if (d < 0) d = 0; - if (d > 32767) d = 32767; - return (uint16_t)d; -} - -uint16_t ditherGetRawDividerMinSetting(void) { - //Value: dither oscillation max frequency F - Hz - //Internal format: device.dither.frequency.settings.max - Hz in 16.16 format - //Output format: frequency min divider 7680000/F - dimensionless units in 16.0 format - - //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 - int32_t d = 0x75300000 / (device.dither.frequency.settings.max >> 8); - if (d < 0) d = 0; - if (d > 32767) d = 32767; - return (uint16_t)d; -} - -uint16_t ditherGetRawDividerState(void) { - //Value: dither oscillation frequency F - Hz - //Internal format: device.dither.frequency.state.frequency - Hz in 16.16 format - //Output format: frequency divider 7680000/F - dimensionless units in 16.0 format - - //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 - int32_t d = 0x75300000 / (device.dither.frequency.state.frequency >> 8); - if (d < 0) d = 0; - if (d > 32767) d = 32767; - return (uint16_t)d; -} - -int16_t ditherGetRawPhaseState(void) { - //Value: dither raw phase - Ph - -1,0,+1 - //Internal format: device.dither.detector.state.phase - -1...1 in 16.16 format - //Output format: sum(Ph, 1 sec) - dimensionless units in 16.0 format - - //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 as in DSP-based program and tranform 16.0 - - return (int16_t)phase; -} - -uint16_t ditherGetRawPulseSetting(void) { - //Value: dither pulse width W - s - //Internal format: device.dither.pulse.settings.width - 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.pulse.settings.width / 65536 - //Pulse width: W = P/(2 * F) - //Output: O = W * 1536000 = 768000 * device.dither.pulse.settings.width / device.dither.frequency.settings.frequency - - //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 - 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; -} - -uint16_t ditherGetRawPulseMaxSetting(void) { - //Value: dither pulse max width W - s - //Internal format: device.dither.pulse.settings.max - 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.min / 65536 - //Half-period: 1/(2 * F) = 0.0011 ... 0.0014 s - //Percentage: P = device.dither.pulse.settings.max / 65536 - //Pulse width: W = P/(2 * F) - //Output: O = W * 1536000 = 768000 * device.dither.pulse.settings.max / device.dither.frequency.settings.min - - //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 - 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; -} - -uint16_t ditherGetRawPulseMinSetting(void) { - //Value: dither pulse min width W - s - //Internal format: device.dither.pulse.settings.min - 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.max / 65536 - //Half-period: 1/(2 * F) = 0.0011 ... 0.0014 s - //Percentage: P = device.dither.pulse.settings.min / 65536 - //Pulse width: W = P/(2 * F) - //Output: O = W * 1536000 = 768000 * device.dither.pulse.settings.min / device.dither.frequency.settings.max - - //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 - 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; -} - -uint16_t ditherGetRawPulseState(void) { - //Value: dither pulse width W - s - //Internal format: device.dither.pulse.state.width - 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.state.frequency / 65536 - //Half-period: 1/(2 * F) = 0.0011 ... 0.0014 s - //Percentage: P = device.dither.pulse.state.width / 65536 - //Pulse width: W = P/(2 * F) - //Output: O = W * 1536000 = 768000 * device.dither.pulse.state.width / device.dither.frequency.state.frequency - - //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 - 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