Diff: DeviceCounters.c

Revision:

178:672ef279c8e0

Parent:

175:0f86eedd511c

Child:

184:2bd8ec44998f

diff -r c718d402410d -r 672ef279c8e0 DeviceCounters.c
--- a/DeviceCounters.c	Thu Jun 30 09:04:22 2016 +0000
+++ b/DeviceCounters.c	Sun Jul 03 13:40:48 2016 +0000
@@ -2,15 +2,40 @@
 extern Device device;
 
 void InitCountersDefaultSettings(void){
+  device.counters.latch.settings.enabled = 0; //External latch disabled
+  device.counters.latch.settings.signal = 0;  //External latch signal 0/1 - RS422/Wire
+  device.counters.latch.settings.format = 0;  //DeltaPS
+  device.counters.latch.settings.reset = 1;   //Reset device.counters.dither.state.angle on latch
+  
+  device.counters.rate.settings.source = 0;   //Dither counters
 }
 
 void InitCountersState(void){
+  device.counters.latch.state.enabled = device.counters.latch.settings.enabled;
+  device.counters.latch.state.signal = device.counters.latch.settings.signal;
+  device.counters.latch.state.format = device.counters.latch.settings.format;
+  device.counters.latch.state.reset = device.counters.latch.settings.reset;
+  device.counters.latch.state.clock = 0;
+  device.counters.latch.state.angle = 0;
+  
+  //Dither latched counters
   for (uint8_t i = 0; i < 32; i++) device.counters.dither.state.delta[i] = 0;
+  device.counters.dither.state.rate = 0;
   device.counters.dither.state.angle = 0;
+  device.counters.dither.state.amplitude = 0;
   device.counters.dither.state.displacement = 0;
-  device.counters.dither.state.amplitude = 0;
-  device.counters.dither.state.frequency = 0;
-  device.counters.dither.state.accumulator = 0;
+  //device.counters.dither.state.frequency = 0;
+  //Meander latched counters
+  device.counters.meander.state.a = 0;
+  device.counters.meander.state.b = 0;
+  device.counters.meander.state.c = 0;
+  device.counters.meander.state.rate[0] = 0;
+  device.counters.meander.state.rate[1] = 0;
+  device.counters.meander.state.angle[0] = 0;
+  device.counters.meander.state.angle[1] = 0;
+  device.counters.meander.state.amplitude = 0;
+  
+  device.counters.rate.state.source = device.counters.rate.settings.source;
 }
 
 void DeviceStartCounters(void){
@@ -18,47 +43,71 @@
 }
 
 void countersProcess(void){
+  //
+  //Process dither latched counters
+  //
   int32_t income = device.controller.QEI.state.delta;
-  int32_t outcome = device.counters.dither.state.delta[(device.measurement.counter-31) & 0xff];
+  int32_t outcome = device.counters.dither.state.delta[device.measurement.counter];
   device.counters.dither.state.delta[device.measurement.counter] = income;
   
   //Angle, accumulated for measurement cycle
   //  Subtract outcoming pulses
-  device.counters.dither.state.angle -= outcome;
+  device.counters.dither.state.rate -= outcome;
   //  Add incoming pulses
-  device.counters.dither.state.angle += income;
-  
-  //Restored meander counters - low accuracy
-  //if (income > 0) a += income
-  //if (income < 0) b += income
+  device.counters.dither.state.rate += income;
+  //Accumulated angle
+  device.counters.dither.state.angle += device.counters.dither.state.rate;
   
   //Displacement (restored sine angle)
   //  Subtract outcoming half wave
   //    Add outcoming half wave end
   device.counters.dither.state.displacement += outcome;
   //    Subtract outcoming half wave start
-  device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-16) & 0xff];
+  device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-16) & 0x1f];
   //  Add incoming half wave
   //    Subtract incoming half wave end
-  device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0xff];
+  device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0x1f];
   //    Add incoming half wave start
   device.counters.dither.state.displacement += income;
   
+  //Amplitude
+  //if (device.counters.dither.state.displacement > 0) device.counters.dither.state.amp += device.counters.dither.state.displacement;
+  //if (device.counters.dither.state.displacement < 0) device.counters.dither.state.amp -= device.counters.dither.state.displacement;
+  
+  //Dither amplitude/Output frequency
+  //  Subtract outcoming pulses
+  if (outcome > 0) device.counters.dither.state.amplitude -= outcome;
+  if (outcome < 0) device.counters.dither.state.amplitude += outcome;
+  //  Add incoming pulses
+  if (income > 0) device.counters.dither.state.amplitude += income;
+  if (income < 0) device.counters.dither.state.amplitude -= income;
+  
+  //
+  //Process restored meander counters - low accuracy
+  //
+  //if (income > 0) a += income
+  //if (income < 0) b += income
+  if (income > 0) device.counters.meander.state.a += income;
+  if (income < 0) device.counters.meander.state.b -= income;
+  if (device.measurement.counter == 31) {
+    device.counters.meander.state.rate[0] = device.counters.meander.state.a - device.counters.meander.state.b;
+    device.counters.meander.state.rate[1] = device.counters.meander.state.a - device.counters.meander.state.c;
+    device.counters.meander.state.amplitude = device.counters.meander.state.a + device.counters.meander.state.b;
+    device.counters.meander.state.angle[0] += device.counters.meander.state.rate[0];
+    device.counters.meander.state.angle[1] += device.counters.meander.state.rate[1];
+    
+    //Process MRate data
+    //device.counters.rate.state.a = device.counters.meander.state.a;
+    //device.counters.rate.state.b = device.counters.meander.state.b;
+    //device.counters.rate.state.angle = device.counters.meander.state.angle[0] + device.counters.meander.state.angle[1] >> 1;
+    
+    //Reset meander latched counters
+    device.counters.meander.state.c = device.counters.meander.state.a;
+    device.counters.meander.state.a = 0;
+    device.counters.meander.state.b = 0;
+  }
+  
   //Virtual zero sensor - low accuracy
   //if (device.counters.state.displacement > 0) a += income
   //if (device.counters.state.displacement < 0) b += income
-  
-  //Amplitude
-  if (device.counters.dither.state.displacement > 0) device.counters.dither.state.amplitude += device.counters.dither.state.displacement;
-  if (device.counters.dither.state.displacement < 0) device.counters.dither.state.amplitude -= device.counters.dither.state.displacement;
-  
-  //Output frequency
-  //  Subtract outcoming pulses
-  if (outcome > 0) device.counters.dither.state.frequency -= outcome;
-  if (outcome < 0) device.counters.dither.state.frequency += outcome;
-  //  Add incoming pulses
-  if (income > 0) device.counters.dither.state.frequency += income;
-  if (income < 0) device.counters.dither.state.frequency -= income;
-  
-  device.counters.dither.state.accumulator += device.counters.dither.state.angle;
 }
\ No newline at end of file