Diff: DeviceDither.c

Revision:

161:efd949e8d536

Parent:

156:e68ee0bcdcda

Child:

167:bedc0a9d559a

--- a/DeviceDither.c	Fri May 13 14:04:22 2016 +0000
+++ b/DeviceDither.c	Sun May 15 19:12:12 2016 +0000
@@ -2,26 +2,64 @@
 extern Device device;
 
 void InitDitherWithDefaults(void){
-  device.dither.settings.enabled = 1;
-  device.dither.settings.rise = 32;
-  device.dither.settings.fall = 64;
+  device.dither.cycle.settings.enabled = 1;
+  device.dither.pulse.settings.rise = 32;
+  device.dither.pulse.settings.fall = 64;
+  
+  device.dither.detector.settings.weight[0] = -1;
+  device.dither.detector.settings.weight[1] = -1;
+  device.dither.detector.settings.weight[2] = -1;
+  device.dither.detector.settings.weight[3] = -1;
+  device.dither.detector.settings.weight[4] = -1;
+  device.dither.detector.settings.weight[5] = -1;
+  device.dither.detector.settings.weight[6] = -1;
+  device.dither.detector.settings.weight[7] = -1;
+  
+  device.dither.detector.settings.weight[8] = 1;
+  device.dither.detector.settings.weight[9] = 1;
+  device.dither.detector.settings.weight[10] = 1;
+  device.dither.detector.settings.weight[11] = 1;
+  device.dither.detector.settings.weight[12] = 1;
+  device.dither.detector.settings.weight[13] = 1;
+  device.dither.detector.settings.weight[14] = 1;
+  device.dither.detector.settings.weight[15] = 1;
+  
+  device.dither.detector.settings.weight[16] = 1;
+  device.dither.detector.settings.weight[17] = 1;
+  device.dither.detector.settings.weight[18] = 1;
+  device.dither.detector.settings.weight[19] = 1;
+  device.dither.detector.settings.weight[20] = 1;
+  device.dither.detector.settings.weight[21] = 1;
+  device.dither.detector.settings.weight[22] = 1;
+  device.dither.detector.settings.weight[23] = 1;
+  
+  device.dither.detector.settings.weight[24] = -1;
+  device.dither.detector.settings.weight[25] = -1;
+  device.dither.detector.settings.weight[26] = -1;
+  device.dither.detector.settings.weight[27] = -1;
+  device.dither.detector.settings.weight[28] = -1;
+  device.dither.detector.settings.weight[29] = -1;
+  device.dither.detector.settings.weight[30] = -1;
+  device.dither.detector.settings.weight[31] = -1;
+  
+  device.dither.detector.settings.offset = 0;
 }
 
 void InitDither(void){
-  device.dither.state.enabled = device.dither.settings.enabled;
-  device.dither.state.rise = device.dither.settings.rise;
-  device.dither.state.fall = device.dither.settings.fall;
-  device.dither.state.counter = 0;
-  device.dither.state.pin1 = 0;
-  device.dither.state.pin2 = 0;
+  device.dither.cycle.state.enabled = device.dither.cycle.settings.enabled;
+  device.dither.pulse.state.rise = device.dither.pulse.settings.rise;
+  device.dither.pulse.state.fall = device.dither.pulse.settings.fall;
+  device.dither.pulse.state.counter = 0;
+  device.dither.cycle.state.pin1 = 0;
+  device.dither.cycle.state.pin2 = 0;
 }
 
 void DeviceStartDither(void){
 }
 
-void DeviceDitherCycle(void){
+void DeviceDitherDoCycle(void){
   uint8_t pin1, pin2;
-  if ((device.dither.state.counter > device.dither.settings.rise) && (device.dither.state.counter < device.dither.settings.fall)){
+  if ((device.dither.pulse.state.counter > device.dither.pulse.settings.rise) && (device.dither.pulse.state.counter < device.dither.pulse.settings.fall)){
     pin1 = 1; pin2 = 1;  
   } else {
     pin1 = 0; pin2 = 0;
@@ -31,13 +69,30 @@
   } else {
       pin1 = 0;
   }
-  if (!device.dither.state.enabled) {
+  if (!device.dither.cycle.state.enabled) {
     pin1 = 0; pin2 = 0;
   }
-  if (pin1 > device.dither.state.pin1) LPC_GPIO1->FIOCLR = 1<<25;
-  if (pin1 < device.dither.state.pin1) LPC_GPIO1->FIOSET = 1<<25;
-  if (pin2 > device.dither.state.pin2) LPC_GPIO1->FIOCLR = 1<<28;
-  if (pin2 < device.dither.state.pin2) LPC_GPIO1->FIOSET = 1<<28;
-  device.dither.state.pin1 = pin1;
-  device.dither.state.pin2 = pin2;
-}
\ No newline at end of file
+  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;
+}
+
+/*
+f(t) = f0 * cos(w * t)
+x(t) = x0 * cos(w * t - fi)
+fi = - arctan(const / (w0^2 - w^2))
+*/
+void DeviceDitherDetectPhase(void) {
+  if (device.measurement.counter == 0) {
+    device.dither.oscillation.state.error = device.dither.detector.state.error;
+    device.dither.detector.state.error = 0;
+  } else {
+    int32_t delta = device.counters.state.delta[device.measurement.counter];
+    uint8_t position = device.measurement.counter + device.dither.detector.settings.offset;
+    int8_t weight = device.dither.detector.settings.weight[position % device.measurement.length];
+    device.dither.detector.state.error += delta * weight;
+  }
+}