igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

DeviceCounters.c@182:2bd8ec44998f, 2016-07-10 (annotated)

Committer:
Diletant
Date:
Sun Jul 10 05:35:08 2016 +0000
Revision:
182:2bd8ec44998f
Parent:
177:672ef279c8e0
Child:
183:c7a9c309086c
Device & ... update. More AskGld & Techno functionality. Not final!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Diletant 156:e68ee0bcdcda 1 #include "Device.h"
Diletant 156:e68ee0bcdcda 2 extern Device device;
Diletant 156:e68ee0bcdcda 3
Diletant 167:bedc0a9d559a 4 void InitCountersDefaultSettings(void){
Diletant 177:672ef279c8e0 5 device.counters.latch.settings.enabled = 0; //External latch disabled
Diletant 177:672ef279c8e0 6 device.counters.latch.settings.signal = 0; //External latch signal 0/1 - RS422/Wire
Diletant 177:672ef279c8e0 7 device.counters.latch.settings.format = 0; //DeltaPS
Diletant 177:672ef279c8e0 8 device.counters.latch.settings.reset = 1; //Reset device.counters.dither.state.angle on latch
Diletant 177:672ef279c8e0 9
Diletant 177:672ef279c8e0 10 device.counters.rate.settings.source = 0; //Dither counters
Diletant 173:7f938afb0447 11 }
Diletant 173:7f938afb0447 12
Diletant 173:7f938afb0447 13 void InitCountersState(void){
Diletant 177:672ef279c8e0 14 device.counters.latch.state.enabled = device.counters.latch.settings.enabled;
Diletant 177:672ef279c8e0 15 device.counters.latch.state.signal = device.counters.latch.settings.signal;
Diletant 177:672ef279c8e0 16 device.counters.latch.state.format = device.counters.latch.settings.format;
Diletant 177:672ef279c8e0 17 device.counters.latch.state.reset = device.counters.latch.settings.reset;
Diletant 177:672ef279c8e0 18 device.counters.latch.state.clock = 0;
Diletant 177:672ef279c8e0 19 device.counters.latch.state.angle = 0;
Diletant 177:672ef279c8e0 20
Diletant 177:672ef279c8e0 21 //Dither latched counters
Diletant 174:0f86eedd511c 22 for (uint8_t i = 0; i < 32; i++) device.counters.dither.state.delta[i] = 0;
Diletant 177:672ef279c8e0 23 device.counters.dither.state.rate = 0;
Diletant 174:0f86eedd511c 24 device.counters.dither.state.angle = 0;
Diletant 177:672ef279c8e0 25 device.counters.dither.state.amplitude = 0;
Diletant 174:0f86eedd511c 26 device.counters.dither.state.displacement = 0;
Diletant 177:672ef279c8e0 27 //device.counters.dither.state.frequency = 0;
Diletant 177:672ef279c8e0 28 //Meander latched counters
Diletant 177:672ef279c8e0 29 device.counters.meander.state.a = 0;
Diletant 177:672ef279c8e0 30 device.counters.meander.state.b = 0;
Diletant 177:672ef279c8e0 31 device.counters.meander.state.c = 0;
Diletant 177:672ef279c8e0 32 device.counters.meander.state.rate[0] = 0;
Diletant 177:672ef279c8e0 33 device.counters.meander.state.rate[1] = 0;
Diletant 177:672ef279c8e0 34 device.counters.meander.state.angle[0] = 0;
Diletant 177:672ef279c8e0 35 device.counters.meander.state.angle[1] = 0;
Diletant 177:672ef279c8e0 36 device.counters.meander.state.amplitude = 0;
Diletant 177:672ef279c8e0 37
Diletant 177:672ef279c8e0 38 device.counters.rate.state.source = device.counters.rate.settings.source;
Diletant 156:e68ee0bcdcda 39 }
Diletant 156:e68ee0bcdcda 40
Diletant 173:7f938afb0447 41 void DeviceStartCounters(void){
Diletant 173:7f938afb0447 42 DeviceStartQEI();
Diletant 167:bedc0a9d559a 43 }
Diletant 167:bedc0a9d559a 44
Diletant 173:7f938afb0447 45 void countersProcess(void){
Diletant 177:672ef279c8e0 46 //
Diletant 177:672ef279c8e0 47 //Process dither latched counters
Diletant 177:672ef279c8e0 48 //
Diletant 174:0f86eedd511c 49 int32_t income = device.controller.QEI.state.delta;
Diletant 177:672ef279c8e0 50 int32_t outcome = device.counters.dither.state.delta[device.measurement.counter];
Diletant 174:0f86eedd511c 51 device.counters.dither.state.delta[device.measurement.counter] = income;
Diletant 156:e68ee0bcdcda 52
Diletant 174:0f86eedd511c 53 //Angle, accumulated for measurement cycle
Diletant 174:0f86eedd511c 54 // Subtract outcoming pulses
Diletant 177:672ef279c8e0 55 device.counters.dither.state.rate -= outcome;
Diletant 174:0f86eedd511c 56 // Add incoming pulses
Diletant 177:672ef279c8e0 57 device.counters.dither.state.rate += income;
Diletant 177:672ef279c8e0 58 //Accumulated angle
Diletant 177:672ef279c8e0 59 device.counters.dither.state.angle += device.counters.dither.state.rate;
Diletant 156:e68ee0bcdcda 60
Diletant 174:0f86eedd511c 61 //Displacement (restored sine angle)
Diletant 174:0f86eedd511c 62 // Subtract outcoming half wave
Diletant 174:0f86eedd511c 63 // Add outcoming half wave end
Diletant 174:0f86eedd511c 64 device.counters.dither.state.displacement += outcome;
Diletant 174:0f86eedd511c 65 // Subtract outcoming half wave start
Diletant 177:672ef279c8e0 66 device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-16) & 0x1f];
Diletant 174:0f86eedd511c 67 // Add incoming half wave
Diletant 174:0f86eedd511c 68 // Subtract incoming half wave end
Diletant 177:672ef279c8e0 69 device.counters.dither.state.displacement -= device.counters.dither.state.delta[(device.measurement.counter-15) & 0x1f];
Diletant 174:0f86eedd511c 70 // Add incoming half wave start
Diletant 174:0f86eedd511c 71 device.counters.dither.state.displacement += income;
Diletant 156:e68ee0bcdcda 72
Diletant 177:672ef279c8e0 73 //Amplitude
Diletant 177:672ef279c8e0 74 //if (device.counters.dither.state.displacement > 0) device.counters.dither.state.amp += device.counters.dither.state.displacement;
Diletant 177:672ef279c8e0 75 //if (device.counters.dither.state.displacement < 0) device.counters.dither.state.amp -= device.counters.dither.state.displacement;
Diletant 177:672ef279c8e0 76
Diletant 177:672ef279c8e0 77 //Dither amplitude/Output frequency
Diletant 177:672ef279c8e0 78 // Subtract outcoming pulses
Diletant 177:672ef279c8e0 79 if (outcome > 0) device.counters.dither.state.amplitude -= outcome;
Diletant 177:672ef279c8e0 80 if (outcome < 0) device.counters.dither.state.amplitude += outcome;
Diletant 177:672ef279c8e0 81 // Add incoming pulses
Diletant 177:672ef279c8e0 82 if (income > 0) device.counters.dither.state.amplitude += income;
Diletant 177:672ef279c8e0 83 if (income < 0) device.counters.dither.state.amplitude -= income;
Diletant 177:672ef279c8e0 84
Diletant 177:672ef279c8e0 85 //
Diletant 177:672ef279c8e0 86 //Process restored meander counters - low accuracy
Diletant 177:672ef279c8e0 87 //
Diletant 177:672ef279c8e0 88 //if (income > 0) a += income
Diletant 177:672ef279c8e0 89 //if (income < 0) b += income
Diletant 177:672ef279c8e0 90 if (income > 0) device.counters.meander.state.a += income;
Diletant 177:672ef279c8e0 91 if (income < 0) device.counters.meander.state.b -= income;
Diletant 177:672ef279c8e0 92 if (device.measurement.counter == 31) {
Diletant 177:672ef279c8e0 93 device.counters.meander.state.rate[0] = device.counters.meander.state.a - device.counters.meander.state.b;
Diletant 177:672ef279c8e0 94 device.counters.meander.state.rate[1] = device.counters.meander.state.a - device.counters.meander.state.c;
Diletant 177:672ef279c8e0 95 device.counters.meander.state.amplitude = device.counters.meander.state.a + device.counters.meander.state.b;
Diletant 177:672ef279c8e0 96 device.counters.meander.state.angle[0] += device.counters.meander.state.rate[0];
Diletant 177:672ef279c8e0 97 device.counters.meander.state.angle[1] += device.counters.meander.state.rate[1];
Diletant 177:672ef279c8e0 98
Diletant 177:672ef279c8e0 99 //Process MRate data
Diletant 177:672ef279c8e0 100 //device.counters.rate.state.a = device.counters.meander.state.a;
Diletant 177:672ef279c8e0 101 //device.counters.rate.state.b = device.counters.meander.state.b;
Diletant 177:672ef279c8e0 102 //device.counters.rate.state.angle = device.counters.meander.state.angle[0] + device.counters.meander.state.angle[1] >> 1;
Diletant 177:672ef279c8e0 103
Diletant 177:672ef279c8e0 104 //Reset meander latched counters
Diletant 177:672ef279c8e0 105 device.counters.meander.state.c = device.counters.meander.state.a;
Diletant 177:672ef279c8e0 106 device.counters.meander.state.a = 0;
Diletant 177:672ef279c8e0 107 device.counters.meander.state.b = 0;
Diletant 177:672ef279c8e0 108 }
Diletant 177:672ef279c8e0 109
Diletant 174:0f86eedd511c 110 //Virtual zero sensor - low accuracy
Diletant 174:0f86eedd511c 111 //if (device.counters.state.displacement > 0) a += income
Diletant 174:0f86eedd511c 112 //if (device.counters.state.displacement < 0) b += income
Diletant 182:2bd8ec44998f 113 }
Diletant 182:2bd8ec44998f 114
Diletant 182:2bd8ec44998f 115 int16_t countersGetRateAngle(void) {
Diletant 182:2bd8ec44998f 116 uint32_t angle = device.counters.meander.state.angle[0] + device.counters.meander.state.angle[1] >> 1;
Diletant 182:2bd8ec44998f 117 device.counters.meander.state.angle[0] = 0;
Diletant 182:2bd8ec44998f 118 //device.counters.meander.state.angle[1] -= device.counters.meander.state.angle[1] >> 1;//?
Diletant 182:2bd8ec44998f 119
Diletant 182:2bd8ec44998f 120 if (angle > 32767) {
Diletant 182:2bd8ec44998f 121 device.counters.meander.state.angle[0] = angle - 32767;
Diletant 182:2bd8ec44998f 122 angle = 32767;
Diletant 182:2bd8ec44998f 123 } else if (angle < -32768) {
Diletant 182:2bd8ec44998f 124 device.counters.meander.state.angle[0] = angle + 32768;
Diletant 182:2bd8ec44998f 125 angle = -32768;
Diletant 182:2bd8ec44998f 126 }
Diletant 182:2bd8ec44998f 127
Diletant 182:2bd8ec44998f 128 return (int16_t)angle;
Diletant 182:2bd8ec44998f 129 }
Diletant 182:2bd8ec44998f 130
Diletant 182:2bd8ec44998f 131 int16_t var_PSdiff(void) {
Diletant 182:2bd8ec44998f 132 int32_t angle = device.counters.dither.state.angle;
Diletant 182:2bd8ec44998f 133 device.counters.dither.state.angle = 0;
Diletant 182:2bd8ec44998f 134 int32_t correction = device.em.error.state.correction >> 16;
Diletant 182:2bd8ec44998f 135 device.em.error.state.correction -= correction;
Diletant 182:2bd8ec44998f 136
Diletant 182:2bd8ec44998f 137 int32_t res = angle + correction;
Diletant 182:2bd8ec44998f 138 if (res > 32767) {
Diletant 182:2bd8ec44998f 139 device.counters.dither.state.angle = res - 32767;
Diletant 182:2bd8ec44998f 140 res = 32767;
Diletant 182:2bd8ec44998f 141 } else if (res < -32768) {
Diletant 182:2bd8ec44998f 142 device.counters.dither.state.angle = res + 32768;
Diletant 182:2bd8ec44998f 143 res = -32768;
Diletant 182:2bd8ec44998f 144 }
Diletant 182:2bd8ec44998f 145
Diletant 182:2bd8ec44998f 146 return (int16_t)res;
Diletant 182:2bd8ec44998f 147 }
Diletant 182:2bd8ec44998f 148
Diletant 182:2bd8ec44998f 149 //
Diletant 182:2bd8ec44998f 150 //Compatibility functions section
Diletant 182:2bd8ec44998f 151 int16_t var_Device_Mode(void) {
Diletant 182:2bd8ec44998f 152 if (device.counters.latch.state.enabled) {
Diletant 182:2bd8ec44998f 153 //External latch mode
Diletant 182:2bd8ec44998f 154 if ((device.counters.latch.state.signal == 0) && (device.counters.latch.state.format == 0))
Diletant 182:2bd8ec44998f 155 //DM_EXT_LATCH_DELTA_PS_LINE
Diletant 182:2bd8ec44998f 156 return 3;
Diletant 182:2bd8ec44998f 157 else if ((device.counters.latch.state.signal == 1) && (device.counters.latch.state.format == 0))
Diletant 182:2bd8ec44998f 158 //DM_EXT_LATCH_DELTA_PS_PULSE
Diletant 182:2bd8ec44998f 159 return 4;
Diletant 182:2bd8ec44998f 160 else if ((device.counters.latch.state.signal == 0) && (device.counters.latch.state.format == 1))
Diletant 182:2bd8ec44998f 161 //DM_EXT_LATCH_DELTA_BINS_LINE
Diletant 182:2bd8ec44998f 162 return 5;
Diletant 182:2bd8ec44998f 163 else if ((device.counters.latch.state.signal == 1) && (device.counters.latch.state.format == 1))
Diletant 182:2bd8ec44998f 164 //DM_EXT_LATCH_DELTA_BINS_PULSE
Diletant 182:2bd8ec44998f 165 return 6;
Diletant 182:2bd8ec44998f 166 } else {
Diletant 182:2bd8ec44998f 167 //Internal latch mode
Diletant 182:2bd8ec44998f 168 if (device.counters.rate.state.source == 0) {
Diletant 182:2bd8ec44998f 169 //DM_INT_SIGN_MEANDER_LATCH
Diletant 182:2bd8ec44998f 170 return 2;
Diletant 182:2bd8ec44998f 171 } else {
Diletant 182:2bd8ec44998f 172 //DM_INT_10KHZ_LATCH
Diletant 182:2bd8ec44998f 173 return 1;
Diletant 182:2bd8ec44998f 174 }
Diletant 182:2bd8ec44998f 175 }
Diletant 182:2bd8ec44998f 176 return 0;
Diletant 156:e68ee0bcdcda 177 }