Dmitry Kovalev / Mbed 2 deprecated LG2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of LG by igor Apu

DeviceCounters.c@186:c7a9c309086c, 2016-07-31 (annotated)

Committer:
Diletant
Date:
Sun Jul 31 06:19:02 2016 +0000
Revision:
186:c7a9c309086c
Parent:
184:2bd8ec44998f 
Device&... update. More AskGld&Techno functionality.NotFinal!!!

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