123
Fork of LG by
DeviceCounters.c
- Committer:
- Diletant
- Date:
- 2016-07-31
- Revision:
- 183:c7a9c309086c
- Parent:
- 182:2bd8ec44998f
File content as of revision 183:c7a9c309086c:
#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; }