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
igor Apu
