123
Fork of LG by
DeviceDither.c@161:efd949e8d536, 2016-05-15 (annotated)
- Committer:
- Diletant
- Date:
- Sun May 15 19:12:12 2016 +0000
- Revision:
- 161:efd949e8d536
- Parent:
- 156:e68ee0bcdcda
- Child:
- 167:bedc0a9d559a
Device & ... update. Not final!!!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Diletant | 149:abbf7663d27d | 1 | #include "Device.h" |
Diletant | 149:abbf7663d27d | 2 | extern Device device; |
Diletant | 149:abbf7663d27d | 3 | |
Diletant | 149:abbf7663d27d | 4 | void InitDitherWithDefaults(void){ |
Diletant | 161:efd949e8d536 | 5 | device.dither.cycle.settings.enabled = 1; |
Diletant | 161:efd949e8d536 | 6 | device.dither.pulse.settings.rise = 32; |
Diletant | 161:efd949e8d536 | 7 | device.dither.pulse.settings.fall = 64; |
Diletant | 161:efd949e8d536 | 8 | |
Diletant | 161:efd949e8d536 | 9 | device.dither.detector.settings.weight[0] = -1; |
Diletant | 161:efd949e8d536 | 10 | device.dither.detector.settings.weight[1] = -1; |
Diletant | 161:efd949e8d536 | 11 | device.dither.detector.settings.weight[2] = -1; |
Diletant | 161:efd949e8d536 | 12 | device.dither.detector.settings.weight[3] = -1; |
Diletant | 161:efd949e8d536 | 13 | device.dither.detector.settings.weight[4] = -1; |
Diletant | 161:efd949e8d536 | 14 | device.dither.detector.settings.weight[5] = -1; |
Diletant | 161:efd949e8d536 | 15 | device.dither.detector.settings.weight[6] = -1; |
Diletant | 161:efd949e8d536 | 16 | device.dither.detector.settings.weight[7] = -1; |
Diletant | 161:efd949e8d536 | 17 | |
Diletant | 161:efd949e8d536 | 18 | device.dither.detector.settings.weight[8] = 1; |
Diletant | 161:efd949e8d536 | 19 | device.dither.detector.settings.weight[9] = 1; |
Diletant | 161:efd949e8d536 | 20 | device.dither.detector.settings.weight[10] = 1; |
Diletant | 161:efd949e8d536 | 21 | device.dither.detector.settings.weight[11] = 1; |
Diletant | 161:efd949e8d536 | 22 | device.dither.detector.settings.weight[12] = 1; |
Diletant | 161:efd949e8d536 | 23 | device.dither.detector.settings.weight[13] = 1; |
Diletant | 161:efd949e8d536 | 24 | device.dither.detector.settings.weight[14] = 1; |
Diletant | 161:efd949e8d536 | 25 | device.dither.detector.settings.weight[15] = 1; |
Diletant | 161:efd949e8d536 | 26 | |
Diletant | 161:efd949e8d536 | 27 | device.dither.detector.settings.weight[16] = 1; |
Diletant | 161:efd949e8d536 | 28 | device.dither.detector.settings.weight[17] = 1; |
Diletant | 161:efd949e8d536 | 29 | device.dither.detector.settings.weight[18] = 1; |
Diletant | 161:efd949e8d536 | 30 | device.dither.detector.settings.weight[19] = 1; |
Diletant | 161:efd949e8d536 | 31 | device.dither.detector.settings.weight[20] = 1; |
Diletant | 161:efd949e8d536 | 32 | device.dither.detector.settings.weight[21] = 1; |
Diletant | 161:efd949e8d536 | 33 | device.dither.detector.settings.weight[22] = 1; |
Diletant | 161:efd949e8d536 | 34 | device.dither.detector.settings.weight[23] = 1; |
Diletant | 161:efd949e8d536 | 35 | |
Diletant | 161:efd949e8d536 | 36 | device.dither.detector.settings.weight[24] = -1; |
Diletant | 161:efd949e8d536 | 37 | device.dither.detector.settings.weight[25] = -1; |
Diletant | 161:efd949e8d536 | 38 | device.dither.detector.settings.weight[26] = -1; |
Diletant | 161:efd949e8d536 | 39 | device.dither.detector.settings.weight[27] = -1; |
Diletant | 161:efd949e8d536 | 40 | device.dither.detector.settings.weight[28] = -1; |
Diletant | 161:efd949e8d536 | 41 | device.dither.detector.settings.weight[29] = -1; |
Diletant | 161:efd949e8d536 | 42 | device.dither.detector.settings.weight[30] = -1; |
Diletant | 161:efd949e8d536 | 43 | device.dither.detector.settings.weight[31] = -1; |
Diletant | 161:efd949e8d536 | 44 | |
Diletant | 161:efd949e8d536 | 45 | device.dither.detector.settings.offset = 0; |
Diletant | 149:abbf7663d27d | 46 | } |
Diletant | 149:abbf7663d27d | 47 | |
Diletant | 149:abbf7663d27d | 48 | void InitDither(void){ |
Diletant | 161:efd949e8d536 | 49 | device.dither.cycle.state.enabled = device.dither.cycle.settings.enabled; |
Diletant | 161:efd949e8d536 | 50 | device.dither.pulse.state.rise = device.dither.pulse.settings.rise; |
Diletant | 161:efd949e8d536 | 51 | device.dither.pulse.state.fall = device.dither.pulse.settings.fall; |
Diletant | 161:efd949e8d536 | 52 | device.dither.pulse.state.counter = 0; |
Diletant | 161:efd949e8d536 | 53 | device.dither.cycle.state.pin1 = 0; |
Diletant | 161:efd949e8d536 | 54 | device.dither.cycle.state.pin2 = 0; |
Diletant | 149:abbf7663d27d | 55 | } |
Diletant | 149:abbf7663d27d | 56 | |
Diletant | 156:e68ee0bcdcda | 57 | void DeviceStartDither(void){ |
Diletant | 149:abbf7663d27d | 58 | } |
Diletant | 149:abbf7663d27d | 59 | |
Diletant | 161:efd949e8d536 | 60 | void DeviceDitherDoCycle(void){ |
Diletant | 149:abbf7663d27d | 61 | uint8_t pin1, pin2; |
Diletant | 161:efd949e8d536 | 62 | if ((device.dither.pulse.state.counter > device.dither.pulse.settings.rise) && (device.dither.pulse.state.counter < device.dither.pulse.settings.fall)){ |
Diletant | 149:abbf7663d27d | 63 | pin1 = 1; pin2 = 1; |
Diletant | 149:abbf7663d27d | 64 | } else { |
Diletant | 149:abbf7663d27d | 65 | pin1 = 0; pin2 = 0; |
Diletant | 149:abbf7663d27d | 66 | } |
Diletant | 156:e68ee0bcdcda | 67 | if (device.measurement.counter < 16){ |
Diletant | 149:abbf7663d27d | 68 | pin2 = 0; |
Diletant | 149:abbf7663d27d | 69 | } else { |
Diletant | 149:abbf7663d27d | 70 | pin1 = 0; |
Diletant | 149:abbf7663d27d | 71 | } |
Diletant | 161:efd949e8d536 | 72 | if (!device.dither.cycle.state.enabled) { |
Diletant | 149:abbf7663d27d | 73 | pin1 = 0; pin2 = 0; |
Diletant | 149:abbf7663d27d | 74 | } |
Diletant | 161:efd949e8d536 | 75 | if (pin1 > device.dither.cycle.state.pin1) LPC_GPIO1->FIOCLR = 1<<25; |
Diletant | 161:efd949e8d536 | 76 | if (pin1 < device.dither.cycle.state.pin1) LPC_GPIO1->FIOSET = 1<<25; |
Diletant | 161:efd949e8d536 | 77 | if (pin2 > device.dither.cycle.state.pin2) LPC_GPIO1->FIOCLR = 1<<28; |
Diletant | 161:efd949e8d536 | 78 | if (pin2 < device.dither.cycle.state.pin2) LPC_GPIO1->FIOSET = 1<<28; |
Diletant | 161:efd949e8d536 | 79 | device.dither.cycle.state.pin1 = pin1; |
Diletant | 161:efd949e8d536 | 80 | device.dither.cycle.state.pin2 = pin2; |
Diletant | 161:efd949e8d536 | 81 | } |
Diletant | 161:efd949e8d536 | 82 | |
Diletant | 161:efd949e8d536 | 83 | /* |
Diletant | 161:efd949e8d536 | 84 | f(t) = f0 * cos(w * t) |
Diletant | 161:efd949e8d536 | 85 | x(t) = x0 * cos(w * t - fi) |
Diletant | 161:efd949e8d536 | 86 | fi = - arctan(const / (w0^2 - w^2)) |
Diletant | 161:efd949e8d536 | 87 | */ |
Diletant | 161:efd949e8d536 | 88 | void DeviceDitherDetectPhase(void) { |
Diletant | 161:efd949e8d536 | 89 | if (device.measurement.counter == 0) { |
Diletant | 161:efd949e8d536 | 90 | device.dither.oscillation.state.error = device.dither.detector.state.error; |
Diletant | 161:efd949e8d536 | 91 | device.dither.detector.state.error = 0; |
Diletant | 161:efd949e8d536 | 92 | } else { |
Diletant | 161:efd949e8d536 | 93 | int32_t delta = device.counters.state.delta[device.measurement.counter]; |
Diletant | 161:efd949e8d536 | 94 | uint8_t position = device.measurement.counter + device.dither.detector.settings.offset; |
Diletant | 161:efd949e8d536 | 95 | int8_t weight = device.dither.detector.settings.weight[position % device.measurement.length]; |
Diletant | 161:efd949e8d536 | 96 | device.dither.detector.state.error += delta * weight; |
Diletant | 161:efd949e8d536 | 97 | } |
Diletant | 161:efd949e8d536 | 98 | } |