File content as of revision 175:0f86eedd511c:

#include "Device.h"
extern Device device;

void InitCountersDefaultSettings(void){
}

void InitCountersState(void){
  for (uint8_t i = 0; i < 32; i++) device.counters.dither.state.delta[i] = 0;
  device.counters.dither.state.angle = 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;
}

void DeviceStartCounters(void){
  DeviceStartQEI();
}

void countersProcess(void){
  int32_t income = device.controller.QEI.state.delta;
  int32_t outcome = device.counters.dither.state.delta[(device.measurement.counter-31) & 0xff];
  device.counters.dither.state.delta[device.measurement.counter] = income;
  
  //Angle, accumulated for measurement cycle
  //  Subtract outcoming pulses
  device.counters.dither.state.angle -= 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
  
  //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];
  //  Add incoming half wave
  //    Subtract incoming half wave end
  device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0xff];
  //    Add incoming half wave start
  device.counters.dither.state.displacement += income;
  
  //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;
}