Diff: DeviceCounters.c

Revision:

182:2bd8ec44998f

Parent:

177:672ef279c8e0

Child:

183:c7a9c309086c

--- a/DeviceCounters.c	Fri Jul 08 14:58:17 2016 +0000
+++ b/DeviceCounters.c	Sun Jul 10 05:35:08 2016 +0000
@@ -110,4 +110,68 @@
   //Virtual zero sensor - low accuracy
   //if (device.counters.state.displacement > 0) a += income
   //if (device.counters.state.displacement < 0) b += income
+}
+
+int16_t countersGetRateAngle(void) {
+  uint32_t angle = device.counters.meander.state.angle[0] + device.counters.meander.state.angle[1] >> 1;
+  device.counters.meander.state.angle[0] = 0;
+  //device.counters.meander.state.angle[1] -= device.counters.meander.state.angle[1] >> 1;//?
+  
+  if (angle > 32767) {
+    device.counters.meander.state.angle[0] = angle - 32767;
+    angle = 32767;
+  } else if (angle < -32768) {
+    device.counters.meander.state.angle[0] = angle + 32768;
+    angle = -32768;
+  }
+  
+  return (int16_t)angle;
+}
+
+int16_t var_PSdiff(void) {
+  int32_t angle = device.counters.dither.state.angle;
+  device.counters.dither.state.angle = 0;
+  int32_t correction = device.em.error.state.correction >> 16;
+  device.em.error.state.correction -= correction;
+  
+  int32_t res = angle + correction;
+  if (res > 32767) {
+    device.counters.dither.state.angle = res - 32767;
+    res = 32767;
+  } else if (res < -32768) {
+    device.counters.dither.state.angle = res + 32768;
+    res = -32768;
+  }
+  
+  return (int16_t)res;
+}
+
+//
+//Compatibility functions section
+int16_t var_Device_Mode(void) {
+  if (device.counters.latch.state.enabled) {
+    //External latch mode
+    if ((device.counters.latch.state.signal == 0) && (device.counters.latch.state.format == 0))
+      //DM_EXT_LATCH_DELTA_PS_LINE
+      return 3;
+    else if ((device.counters.latch.state.signal == 1) && (device.counters.latch.state.format == 0))
+      //DM_EXT_LATCH_DELTA_PS_PULSE
+      return 4;
+    else if ((device.counters.latch.state.signal == 0) && (device.counters.latch.state.format == 1))
+      //DM_EXT_LATCH_DELTA_BINS_LINE
+      return 5;
+    else if ((device.counters.latch.state.signal == 1) && (device.counters.latch.state.format == 1))
+      //DM_EXT_LATCH_DELTA_BINS_PULSE
+      return 6;
+  } else {
+    //Internal latch mode
+    if (device.counters.rate.state.source == 0) {
+      //DM_INT_SIGN_MEANDER_LATCH
+      return 2;
+    } else {
+      //DM_INT_10KHZ_LATCH
+      return 1;
+    }
+  }
+  return 0;
 }
\ No newline at end of file