123
Fork of LG by
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