igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

DeviceDither.c@167:bedc0a9d559a, 2016-06-05 (annotated)

Committer:
Diletant
Date:
Sun Jun 05 11:03:33 2016 +0000
Revision:
167:bedc0a9d559a
Parent:
161:efd949e8d536
Child:
173:7f938afb0447
Device & ... update. Not final!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Diletant 149:abbf7663d27d 1 #include "Device.h"
Diletant 149:abbf7663d27d 2 extern Device device;
Diletant 149:abbf7663d27d 3
Diletant 167:bedc0a9d559a 4 void InitDitherDefaultSettings(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 167:bedc0a9d559a 48 void InitDitherState(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 }