123
Fork of LG by
Diff: DeviceDither.c
- Revision:
- 161:efd949e8d536
- Parent:
- 156:e68ee0bcdcda
- Child:
- 167:bedc0a9d559a
diff -r 6170df6f5a5c -r efd949e8d536 DeviceDither.c --- a/DeviceDither.c Fri May 13 14:04:22 2016 +0000 +++ b/DeviceDither.c Sun May 15 19:12:12 2016 +0000 @@ -2,26 +2,64 @@ extern Device device; void InitDitherWithDefaults(void){ - device.dither.settings.enabled = 1; - device.dither.settings.rise = 32; - device.dither.settings.fall = 64; + device.dither.cycle.settings.enabled = 1; + device.dither.pulse.settings.rise = 32; + device.dither.pulse.settings.fall = 64; + + device.dither.detector.settings.weight[0] = -1; + device.dither.detector.settings.weight[1] = -1; + device.dither.detector.settings.weight[2] = -1; + device.dither.detector.settings.weight[3] = -1; + device.dither.detector.settings.weight[4] = -1; + device.dither.detector.settings.weight[5] = -1; + device.dither.detector.settings.weight[6] = -1; + device.dither.detector.settings.weight[7] = -1; + + device.dither.detector.settings.weight[8] = 1; + device.dither.detector.settings.weight[9] = 1; + device.dither.detector.settings.weight[10] = 1; + device.dither.detector.settings.weight[11] = 1; + device.dither.detector.settings.weight[12] = 1; + device.dither.detector.settings.weight[13] = 1; + device.dither.detector.settings.weight[14] = 1; + device.dither.detector.settings.weight[15] = 1; + + device.dither.detector.settings.weight[16] = 1; + device.dither.detector.settings.weight[17] = 1; + device.dither.detector.settings.weight[18] = 1; + device.dither.detector.settings.weight[19] = 1; + device.dither.detector.settings.weight[20] = 1; + device.dither.detector.settings.weight[21] = 1; + device.dither.detector.settings.weight[22] = 1; + device.dither.detector.settings.weight[23] = 1; + + device.dither.detector.settings.weight[24] = -1; + device.dither.detector.settings.weight[25] = -1; + device.dither.detector.settings.weight[26] = -1; + device.dither.detector.settings.weight[27] = -1; + device.dither.detector.settings.weight[28] = -1; + device.dither.detector.settings.weight[29] = -1; + device.dither.detector.settings.weight[30] = -1; + device.dither.detector.settings.weight[31] = -1; + + device.dither.detector.settings.offset = 0; } void InitDither(void){ - device.dither.state.enabled = device.dither.settings.enabled; - device.dither.state.rise = device.dither.settings.rise; - device.dither.state.fall = device.dither.settings.fall; - device.dither.state.counter = 0; - device.dither.state.pin1 = 0; - device.dither.state.pin2 = 0; + device.dither.cycle.state.enabled = device.dither.cycle.settings.enabled; + device.dither.pulse.state.rise = device.dither.pulse.settings.rise; + device.dither.pulse.state.fall = device.dither.pulse.settings.fall; + device.dither.pulse.state.counter = 0; + device.dither.cycle.state.pin1 = 0; + device.dither.cycle.state.pin2 = 0; } void DeviceStartDither(void){ } -void DeviceDitherCycle(void){ +void DeviceDitherDoCycle(void){ uint8_t pin1, pin2; - if ((device.dither.state.counter > device.dither.settings.rise) && (device.dither.state.counter < device.dither.settings.fall)){ + if ((device.dither.pulse.state.counter > device.dither.pulse.settings.rise) && (device.dither.pulse.state.counter < device.dither.pulse.settings.fall)){ pin1 = 1; pin2 = 1; } else { pin1 = 0; pin2 = 0; @@ -31,13 +69,30 @@ } else { pin1 = 0; } - if (!device.dither.state.enabled) { + if (!device.dither.cycle.state.enabled) { pin1 = 0; pin2 = 0; } - if (pin1 > device.dither.state.pin1) LPC_GPIO1->FIOCLR = 1<<25; - if (pin1 < device.dither.state.pin1) LPC_GPIO1->FIOSET = 1<<25; - if (pin2 > device.dither.state.pin2) LPC_GPIO1->FIOCLR = 1<<28; - if (pin2 < device.dither.state.pin2) LPC_GPIO1->FIOSET = 1<<28; - device.dither.state.pin1 = pin1; - device.dither.state.pin2 = pin2; -} \ No newline at end of file + if (pin1 > device.dither.cycle.state.pin1) LPC_GPIO1->FIOCLR = 1<<25; + if (pin1 < device.dither.cycle.state.pin1) LPC_GPIO1->FIOSET = 1<<25; + if (pin2 > device.dither.cycle.state.pin2) LPC_GPIO1->FIOCLR = 1<<28; + if (pin2 < device.dither.cycle.state.pin2) LPC_GPIO1->FIOSET = 1<<28; + device.dither.cycle.state.pin1 = pin1; + device.dither.cycle.state.pin2 = pin2; +} + +/* +f(t) = f0 * cos(w * t) +x(t) = x0 * cos(w * t - fi) +fi = - arctan(const / (w0^2 - w^2)) +*/ +void DeviceDitherDetectPhase(void) { + if (device.measurement.counter == 0) { + device.dither.oscillation.state.error = device.dither.detector.state.error; + device.dither.detector.state.error = 0; + } else { + int32_t delta = device.counters.state.delta[device.measurement.counter]; + uint8_t position = device.measurement.counter + device.dither.detector.settings.offset; + int8_t weight = device.dither.detector.settings.weight[position % device.measurement.length]; + device.dither.detector.state.error += delta * weight; + } +}