Diff: DeviceCounters.c

Revision:

198:78dd6d14d108

Parent:

197:7a05523bf588

--- a/DeviceCounters.c	Wed Oct 19 10:55:05 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,178 +0,0 @@
-#include "Device.h"
-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.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){
-  DeviceStartQEI();
-}
-
-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];
-  device.counters.dither.state.delta[device.measurement.counter] = income;
-  
-  //Angle, accumulated for measurement cycle
-  //  Subtract outcoming pulses
-  device.counters.dither.state.rate -= outcome;
-  //  Add incoming pulses
-  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) & 0x1f];
-  //  Add incoming half wave
-  //    Subtract incoming half wave end
-  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. TODO: use QEI reverse interrupt for correct counting in meander mode
-  //
-  //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
-}
-
-//
-//Compatibility functions section
-//
-int16_t var_Cnt_Dif(void) {
-  int32_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;
-}
-
-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