123
Fork of LG by
Diff: DevicePLCS.c
- Revision:
- 198:78dd6d14d108
- Parent:
- 197:7a05523bf588
diff -r 7a05523bf588 -r 78dd6d14d108 DevicePLCS.c --- a/DevicePLCS.c Wed Oct 19 10:55:05 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,736 +0,0 @@ -#include "Device.h" -extern Device device; - -void InitPathLengthControlSystemDefaultSettings(void){ - device.plcs.feedback.settings.input = 1; - device.plcs.feedback.settings.output = 0; - device.plcs.feedback.settings.transfer.points = 16; - device.plcs.feedback.settings.transfer.raw[0] = -0x20000; //-2.0 - device.plcs.feedback.settings.transfer.normalized[0] = -0x10000; //-1.0 - device.plcs.feedback.settings.transfer.raw[1] = -0x1bbbb; //-1.7333333333333334 - device.plcs.feedback.settings.transfer.normalized[1] = -0xdddd; //-0.8666666666666667 - device.plcs.feedback.settings.transfer.raw[2] = -0x17777; //-1.4666666666666668 - device.plcs.feedback.settings.transfer.normalized[2] = -0xbbbb; //-0.7333333333333334 - device.plcs.feedback.settings.transfer.raw[3] = -0x13333; //-1.2 - device.plcs.feedback.settings.transfer.normalized[3] = -0x9999; //-0.6 - device.plcs.feedback.settings.transfer.raw[4] = -0xeeee; //-0.9333333333333333 - device.plcs.feedback.settings.transfer.normalized[4] = -0x7777; //-0.4666666666666667 - device.plcs.feedback.settings.transfer.raw[5] = -0xaaaa; //-0.6666666666666667 - device.plcs.feedback.settings.transfer.normalized[5] = -0x5555; //-0.33333333333333337 - device.plcs.feedback.settings.transfer.raw[6] = -0x6666; //-0.3999999999999999 - device.plcs.feedback.settings.transfer.normalized[6] = -0x3333; //-0.19999999999999996 - device.plcs.feedback.settings.transfer.raw[7] = -0x2222; //-0.1333333333333333 - device.plcs.feedback.settings.transfer.normalized[7] = -0x1111; //-0.06666666666666665 - device.plcs.feedback.settings.transfer.raw[8] = 0x2222; //0.1333333333333333 - device.plcs.feedback.settings.transfer.normalized[8] = 0x1111; //0.06666666666666665 - device.plcs.feedback.settings.transfer.raw[9] = 0x6666; //0.3999999999999999 - device.plcs.feedback.settings.transfer.normalized[9] = 0x3333; //0.19999999999999996 - device.plcs.feedback.settings.transfer.raw[10] = 0xaaaa; //0.6666666666666665 - device.plcs.feedback.settings.transfer.normalized[10] = 0x5555; //0.33333333333333326 - device.plcs.feedback.settings.transfer.raw[11] = 0xeeee; //0.9333333333333331 - device.plcs.feedback.settings.transfer.normalized[11] = 0x7777; //0.46666666666666656 - device.plcs.feedback.settings.transfer.raw[12] = 0x13333; //1.2000000000000002 - device.plcs.feedback.settings.transfer.normalized[12] = 0x9999; //0.6000000000000001 - device.plcs.feedback.settings.transfer.raw[13] = 0x17777; //1.4666666666666668 - device.plcs.feedback.settings.transfer.normalized[13] = 0xbbbb; //0.7333333333333334 - device.plcs.feedback.settings.transfer.raw[14] = 0x1bbbb; //1.7333333333333334 - device.plcs.feedback.settings.transfer.normalized[14] = 0xdddd; //0.8666666666666667 - device.plcs.feedback.settings.transfer.raw[15] = 0x20000; //2.0 - device.plcs.feedback.settings.transfer.normalized[15] = 0x10000; //1.0 - - device.plcs.reference.settings.sequencer = 1; - device.plcs.reference.settings.delta = 0; - - device.plcs.regulator.settings.enabled = 1; - device.plcs.regulator.settings.reference = 0; - device.plcs.regulator.settings.scale = 0x00050000; - device.plcs.regulator.settings.transfer.points = 16; - device.plcs.regulator.settings.transfer.error[0] = -0x8000; //-0.5 - device.plcs.regulator.settings.transfer.correction[0] = 0x10000; //1.0 - device.plcs.regulator.settings.transfer.error[1] = -0x6eee; //-0.43333333333333335 - device.plcs.regulator.settings.transfer.correction[1] = 0xb473; //0.7048888888888889 - device.plcs.regulator.settings.transfer.error[2] = -0x5ddd; //-0.3666666666666667 - device.plcs.regulator.settings.transfer.correction[2] = 0x7aa7; //0.47911111111111115 - device.plcs.regulator.settings.transfer.error[3] = -0x4ccc; //-0.3 - device.plcs.regulator.settings.transfer.correction[3] = 0x4fdf; //0.31199999999999994 - device.plcs.regulator.settings.transfer.error[4] = -0x3bbb; //-0.23333333333333334 - device.plcs.regulator.settings.transfer.correction[4] = 0x3161; //0.1928888888888889 - device.plcs.regulator.settings.transfer.error[5] = -0x2aaa; //-0.16666666666666669 - device.plcs.regulator.settings.transfer.correction[5] = 0x1c71; //0.11111111111111113 - device.plcs.regulator.settings.transfer.error[6] = -0x1999; //-0.09999999999999998 - device.plcs.regulator.settings.transfer.correction[6] = 0xe56; //0.05599999999999999 - device.plcs.regulator.settings.transfer.error[7] = -0x888; //-0.033333333333333326 - device.plcs.regulator.settings.transfer.correction[7] = 0x452; //0.016888888888888884 - device.plcs.regulator.settings.transfer.error[8] = 0x888; //0.033333333333333326 - device.plcs.regulator.settings.transfer.correction[8] = -0x452; //-0.016888888888888884 - device.plcs.regulator.settings.transfer.error[9] = 0x1999; //0.09999999999999998 - device.plcs.regulator.settings.transfer.correction[9] = -0xe56; //-0.05599999999999999 - device.plcs.regulator.settings.transfer.error[10] = 0x2aaa; //0.16666666666666663 - device.plcs.regulator.settings.transfer.correction[10] = -0x1c71; //-0.11111111111111108 - device.plcs.regulator.settings.transfer.error[11] = 0x3bbb; //0.23333333333333328 - device.plcs.regulator.settings.transfer.correction[11] = -0x3161; //-0.1928888888888888 - device.plcs.regulator.settings.transfer.error[12] = 0x4ccc; //0.30000000000000004 - device.plcs.regulator.settings.transfer.correction[12] = -0x4fdf; //-0.31200000000000006 - device.plcs.regulator.settings.transfer.error[13] = 0x5ddd; //0.3666666666666667 - device.plcs.regulator.settings.transfer.correction[13] = -0x7aa7; //-0.47911111111111115 - device.plcs.regulator.settings.transfer.error[14] = 0x6eee; //0.43333333333333335 - device.plcs.regulator.settings.transfer.correction[14] = -0xb473; //-0.7048888888888889 - device.plcs.regulator.settings.transfer.error[15] = 0x8000; //0.5 - device.plcs.regulator.settings.transfer.correction[15] = -0x10000; //-1.0 - - //device.plcs.reset.levels.upper = 0xc8000; //12.5V - //device.plcs.reset.levels.lower = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.points = 16; - device.plcs.reset.up.settings.environment.temperature[0] = -0x3c0000; //-60C - device.plcs.reset.up.settings.environment.trigger[0] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[0] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[0] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[1] = -0x320000; //-50C - device.plcs.reset.up.settings.environment.trigger[1] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[1] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[1] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[2] = -0x280000; //-40C - device.plcs.reset.up.settings.environment.trigger[2] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[2] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[2] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[3] = -0x1e0000; //-30C - device.plcs.reset.up.settings.environment.trigger[3] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[3] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[3] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[4] = -0x140000; //-20C - device.plcs.reset.up.settings.environment.trigger[4] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[4] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[4] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[5] = -0xa0000; //-10C - device.plcs.reset.up.settings.environment.trigger[5] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[5] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[5] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[6] = 0x0; //0C - device.plcs.reset.up.settings.environment.trigger[6] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[6] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[6] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[7] = 0xa0000; //10C - device.plcs.reset.up.settings.environment.trigger[7] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[7] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[7] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[8] = 0x140000; //20C - device.plcs.reset.up.settings.environment.trigger[8] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[8] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[8] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[9] = 0x1e0000; //30C - device.plcs.reset.up.settings.environment.trigger[9] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[9] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[9] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[10] = 0x280000; //40C - device.plcs.reset.up.settings.environment.trigger[10] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[10] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[10] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[11] = 0x320000; //50C - device.plcs.reset.up.settings.environment.trigger[11] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[11] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[11] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[12] = 0x3c0000; //60C - device.plcs.reset.up.settings.environment.trigger[12] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[12] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[12] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[13] = 0x460000; //70C - device.plcs.reset.up.settings.environment.trigger[13] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[13] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[13] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[14] = 0x500000; //80C - device.plcs.reset.up.settings.environment.trigger[14] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[14] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[14] = 0x30000; //3s - device.plcs.reset.up.settings.environment.temperature[15] = 0x5a0000; //90C - device.plcs.reset.up.settings.environment.trigger[15] = 0x18000; //1.5V - device.plcs.reset.up.settings.environment.target[15] = 0xc8000; //12.5V - device.plcs.reset.up.settings.environment.duration[15] = 0x30000; //3s - - device.plcs.reset.down.settings.environment.points = 16; - device.plcs.reset.down.settings.environment.temperature[0] = -0x3c0000; //-60C - device.plcs.reset.down.settings.environment.trigger[0] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[0] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[0] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[1] = -0x320000; //-50C - device.plcs.reset.down.settings.environment.trigger[1] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[1] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[1] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[2] = -0x280000; //-40C - device.plcs.reset.down.settings.environment.trigger[2] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[2] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[2] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[3] = -0x1e0000; //-30C - device.plcs.reset.down.settings.environment.trigger[3] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[3] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[3] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[4] = -0x140000; //-20C - device.plcs.reset.down.settings.environment.trigger[4] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[4] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[4] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[5] = -0xa0000; //-10C - device.plcs.reset.down.settings.environment.trigger[5] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[5] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[5] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[6] = 0x0; //0C - device.plcs.reset.down.settings.environment.trigger[6] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[6] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[6] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[7] = 0xa0000; //10C - device.plcs.reset.down.settings.environment.trigger[7] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[7] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[7] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[8] = 0x140000; //20C - device.plcs.reset.down.settings.environment.trigger[8] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[8] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[8] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[9] = 0x1e0000; //30C - device.plcs.reset.down.settings.environment.trigger[9] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[9] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[9] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[10] = 0x280000; //40C - device.plcs.reset.down.settings.environment.trigger[10] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[10] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[10] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[11] = 0x320000; //50C - device.plcs.reset.down.settings.environment.trigger[11] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[11] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[11] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[12] = 0x3c0000; //60C - device.plcs.reset.down.settings.environment.trigger[12] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[12] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[12] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[13] = 0x460000; //70C - device.plcs.reset.down.settings.environment.trigger[13] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[13] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[13] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[14] = 0x500000; //80C - device.plcs.reset.down.settings.environment.trigger[14] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[14] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[14] = 0x30000; //3s - device.plcs.reset.down.settings.environment.temperature[15] = 0x5a0000; //90C - device.plcs.reset.down.settings.environment.trigger[15] = 0xc8000; //12.5V - device.plcs.reset.down.settings.environment.target[15] = 0x18000; //1.5V - device.plcs.reset.down.settings.environment.duration[15] = 0x30000; //3s - - device.plcs.output.settings.sequencer = 1; -} - -void InitPathLengthControlSystemState(void){ - device.plcs.feedback.state.input = device.plcs.feedback.settings.input; - device.plcs.feedback.state.output = device.plcs.feedback.settings.output; - - device.plcs.reference.state.sequencer = device.plcs.reference.settings.sequencer; - device.plcs.reference.state.delta = device.plcs.reference.settings.delta; - - device.plcs.detector.state.out = 1; - - device.plcs.regulator.state.enabled = device.plcs.regulator.settings.enabled; - device.plcs.regulator.state.reference = device.plcs.regulator.settings.reference; - device.plcs.regulator.state.scale = device.plcs.regulator.settings.scale; - - device.plcs.reset.state.countdown = 0; - - device.plcs.output.settings.sequencer = device.plcs.output.state.sequencer; -} - -void DeviceStartPLCS(void){ -} - -/* - //Move to DevicePLCS regulator - uint32_t value; - switch(device.plcs.state.modulation) { - case 1://малое воздействие - value = device.SSP.DAC[1] + Gyro.StrayPLC_Pls; - break; - - case 3://малое воздействие - value = device.SSP.DAC[1] + Gyro.StrayPLC_Mns; - break; - - case 2://большое воздействие - value = device.SSP.DAC[1] + Gyro.StrayPLC_2Mode; - break; - - default://режим без воздействия - value = device.SSP.DAC[1]; - break; - } - LPC_SSP0->DR = device.SSP.DAC[1]; - */ - -int32_t plcsInterpolate(int32_t a,int32_t a1,int32_t a2,int32_t b1,int32_t b2) { - int32_t ma, mb; - while (1) { - if (a1 == a) return b1; - if (a2 == a) return b2; - - ma = (a1 + a2) >> 1; - mb = (b1 + b2) >> 1; - - if (a < ma) { - if (a2 == ma) return mb; - if (b1 == mb) return mb; - a2 = ma; b2 = mb; - } else if (a > ma) { - if (a1 == ma) return mb; - if (b2 == mb) return mb; - a1 = ma; b1 = mb; - } else return mb; - } -} - -int32_t plcsFeedback(int32_t voltage) { - int32_t v1, v2; - for (uint8_t i = 1; i < device.plcs.feedback.settings.transfer.points; i++) { - v1 = device.plcs.feedback.settings.transfer.raw[i - 1]; - if (voltage < v1) { - return device.plcs.feedback.settings.transfer.normalized[i - 1]; - } - - v2 = device.plcs.feedback.settings.transfer.raw[i]; - if (voltage < v2) { - int32_t r1 = device.plcs.feedback.settings.transfer.normalized[i - 1]; - int32_t r2 = device.plcs.feedback.settings.transfer.normalized[i]; - return plcsInterpolate(voltage, v1, v2, r1, r2); - } - } - return device.plcs.feedback.settings.transfer.normalized[device.plcs.feedback.settings.transfer.points - 1]; -} - -int32_t plcsBias(int32_t raw) { - int32_t r1, r2; - for (uint8_t i = 1; i < device.plcs.bias.settings.transfer.points; i++) { - r1 = device.plcs.bias.settings.transfer.raw[i - 1]; - if (raw < r1) { - return device.plcs.bias.settings.transfer.normalized[i - 1]; - } - - r2 = device.plcs.bias.settings.transfer.raw[i]; - if (raw < r2) { - int32_t l1 = device.plcs.bias.settings.transfer.normalized[i - 1]; - int32_t l2 = device.plcs.bias.settings.transfer.normalized[i]; - return plcsInterpolate(raw, r1, r2, l1, l2); - } - } - return device.plcs.bias.settings.transfer.normalized[device.plcs.bias.settings.transfer.points - 1]; -} - -int32_t plcsCorrection(int32_t error) { - int32_t e1, e2; - for (uint8_t i = 1; i < device.plcs.regulator.settings.transfer.points; i++) { - e1 = device.plcs.regulator.settings.transfer.error[i - 1]; - if (error < e1) { - return device.plcs.regulator.settings.transfer.correction[i - 1]; - } - - e2 = device.plcs.regulator.settings.transfer.error[i]; - if (error < e2) { - int32_t c1 = device.plcs.regulator.settings.transfer.correction[i - 1]; - int32_t c2 = device.plcs.regulator.settings.transfer.correction[i]; - return plcsInterpolate(error, e1, e2, c1, c2); - } - } - return device.plcs.regulator.settings.transfer.correction[device.plcs.regulator.settings.transfer.points - 1]; -} - -uint32_t plcsCode(int32_t voltage) { - int32_t v1, v2; - for (uint8_t i = 1; i < device.plcs.output.settings.transfer.points; i++) { - v1 = device.plcs.output.settings.transfer.voltage[i - 1]; - if (voltage < v1) { - return device.plcs.output.settings.transfer.code[i - 1]; - } - - v2 = device.plcs.output.settings.transfer.voltage[i]; - if (voltage < v2) { - int32_t c1 = device.plcs.output.settings.transfer.code[i - 1]; - int32_t c2 = device.plcs.output.settings.transfer.code[i]; - return plcsInterpolate(voltage, v1, v2, c1, c2); //TODO: signed/unsigned - } - } - return device.plcs.output.settings.transfer.code[device.plcs.output.settings.transfer.points - 1]; -} - -void plcsProcessDelta(void) { - //Process detector - int32_t feedback = plcsFeedback(device.plcs.feedback.state.voltage); //-1...+1 in 16.16 format - int32_t delta = feedback - device.plcs.detector.state.in[0]; //-2...+2 in 16.16 format - //if (delta > 0) delta = 1; - //if (delta < 0) delta = -1; - device.plcs.detector.state.in[0] = feedback; - device.plcs.detector.state.out = ((delta >> 2) * (device.plcs.detector.state.in[1] >> 1)) >> 14; //-1...+1 in 16.16 format - - //Process bias -0.5...+0.5 in 16.16 format - device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out); - device.plcs.bias.state.sum += device.plcs.bias.state.raw; - device.plcs.bias.state.counter++; - if (device.plcs.bias.state.counter == 256) { - device.plcs.bias.state.average = device.plcs.bias.state.sum >> 8; - device.plcs.bias.state.sum = 0; - device.plcs.bias.state.counter = 0; - - if (device.plcs.bias.state.average > 0) - device.plcs.detector.state.in[1] = -1; - else - device.plcs.detector.state.in[1] = 1; - } -} - -void plcsProcessSequencer(void) { - //Process detector - device.plcs.detector.state.in[0] = plcsFeedback(device.plcs.feedback.state.voltage) >> 1; //-1...+1 in 17.15 format - device.plcs.detector.state.in[1] = device.sequencer.sampler.state.sample[1] >> 1; //-1...+1 in 17.15 format - device.plcs.detector.state.out = device.plcs.detector.state.in[0] * device.plcs.detector.state.in[1] >> 14;//-1...+1 in 16.16 format - - //Process bias -0.5...+0.5 in 16.16 format - device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out); - device.plcs.bias.state.sum += device.plcs.bias.state.raw; - device.plcs.bias.state.counter++; - if (device.plcs.bias.state.counter == 64) { - device.plcs.bias.state.average = device.plcs.bias.state.sum >> 6; - device.plcs.bias.state.sum = 0; - device.plcs.bias.state.counter = 0; - } -} - -void plcsRegulate(void) { - device.plcs.regulator.state.error = device.plcs.regulator.state.reference - device.plcs.bias.state.average; - device.plcs.regulator.state.correction = (device.plcs.regulator.state.scale * plcsCorrection(device.plcs.regulator.state.error)) >> 16; - - if (device.plcs.output.state.enabled) { - if (device.plcs.reset.state.countdown == 0){ - if (device.plcs.regulator.state.enabled) { - device.plcs.output.state.voltage += device.plcs.regulator.state.correction; - } - } - } -} - -void plcsPreprocessJumpUp(void) { - int32_t t1, t2; - for (uint8_t i = 1; i < device.plcs.reset.up.settings.environment.points; i++) { - t1 = device.plcs.reset.up.settings.environment.temperature[i - 1]; - if (device.tss.temperature.state.celsius < t1) { - device.plcs.reset.up.state.trigger = device.plcs.reset.up.settings.environment.trigger[i - 1]; - device.plcs.reset.up.state.target = device.plcs.reset.up.settings.environment.target[i - 1]; - device.plcs.reset.up.state.duration = device.plcs.reset.up.settings.environment.duration[i - 1]; - return; - } - - t2 = device.plcs.reset.up.settings.environment.temperature[i]; - if (device.tss.temperature.state.celsius < t2) { - int32_t v1 = device.plcs.reset.up.settings.environment.trigger[i - 1]; - int32_t v2 = device.plcs.reset.up.settings.environment.trigger[i]; - device.plcs.reset.up.state.trigger = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - v1 = device.plcs.reset.up.settings.environment.target[i - 1]; - v2 = device.plcs.reset.up.settings.environment.target[i]; - device.plcs.reset.up.state.target = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - int32_t d1 = device.plcs.reset.up.settings.environment.duration[i - 1]; - int32_t d2 = device.plcs.reset.up.settings.environment.duration[i]; - device.plcs.reset.up.state.duration = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, d1, d2); - return; - } - } - device.plcs.reset.up.state.trigger = device.plcs.reset.up.settings.environment.trigger[device.plcs.reset.up.settings.environment.points - 1]; - device.plcs.reset.up.state.target = device.plcs.reset.up.settings.environment.target[device.plcs.reset.up.settings.environment.points - 1]; - device.plcs.reset.up.state.duration = device.plcs.reset.up.settings.environment.duration[device.plcs.reset.up.settings.environment.points - 1]; -} - -void plcsPreprocessJumpDown(void) { - int32_t t1, t2; - for (uint8_t i = 1; i < device.plcs.reset.down.settings.environment.points; i++) { - t1 = device.plcs.reset.down.settings.environment.temperature[i - 1]; - if (device.tss.temperature.state.celsius < t1) { - device.plcs.reset.down.state.trigger = device.plcs.reset.down.settings.environment.trigger[i - 1]; - device.plcs.reset.down.state.target = device.plcs.reset.down.settings.environment.target[i - 1]; - device.plcs.reset.down.state.duration = device.plcs.reset.down.settings.environment.duration[i - 1]; - return; - } - - t2 = device.plcs.reset.down.settings.environment.temperature[i]; - if (device.tss.temperature.state.celsius < t2) { - int32_t v1 = device.plcs.reset.down.settings.environment.trigger[i - 1]; - int32_t v2 = device.plcs.reset.down.settings.environment.trigger[i]; - device.plcs.reset.down.state.trigger = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - v1 = device.plcs.reset.down.settings.environment.target[i - 1]; - v2 = device.plcs.reset.down.settings.environment.target[i]; - device.plcs.reset.down.state.target = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - int32_t d1 = device.plcs.reset.down.settings.environment.duration[i - 1]; - int32_t d2 = device.plcs.reset.down.settings.environment.duration[i]; - device.plcs.reset.down.state.duration = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, d1, d2); - return; - } - } - device.plcs.reset.down.state.trigger = device.plcs.reset.down.settings.environment.trigger[device.plcs.reset.down.settings.environment.points - 1]; - device.plcs.reset.down.state.target = device.plcs.reset.down.settings.environment.target[device.plcs.reset.down.settings.environment.points - 1]; - device.plcs.reset.down.state.duration = device.plcs.reset.down.settings.environment.duration[device.plcs.reset.down.settings.environment.points - 1]; -} - -//Delete this!!! -/*void plcsUp(void) { - int32_t t1, t2; - for (uint8_t i = 1; i < device.plcs.reset.up.points; i++) { - t1 = device.plcs.reset.up.temperature[i - 1]; - if (device.tss.temperature.state.celsius < t1) { - device.plcs.reset.state.voltage = device.plcs.reset.up.settings.environment.voltage[i - 1]; //Delete this!!! - device.plcs.reset.state.countdown = device.plcs.reset.up.settings.environment.duration[i - 1]; - } - - t2 = device.plcs.reset.up.temperature[i]; - if (device.tss.temperature.state.celsius < t2) { - int32_t v1 = device.plcs.reset.up.settings.environment.voltage[i - 1]; - int32_t v2 = device.plcs.reset.up.settings.environment.voltage[i]; - device.plcs.reset.state.voltage = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - int32_t d1 = device.plcs.reset.up.settings.environment.duration[i - 1]; - int32_t d2 = device.plcs.reset.up.settings.environment.duration[i]; - device.plcs.reset.state.countdown = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, d1, d2); - } - } - device.plcs.output.state.voltage = device.plcs.reset.up.settings.environment.voltage[device.plcs.reset.up.settings.environment.points - 1]; - device.plcs.reset.state.countdown = device.plcs.reset.up.settings.environment.duration[device.plcs.reset.up.settings.environment.points - 1]; -}*/ - -//Delete this!!! -/*void plcsDown(void) { - int32_t t1, t2; - for (uint8_t i = 1; i < device.plcs.reset.down.points; i++) { - t1 = device.plcs.reset.down.temperature[i - 1]; - if (device.tss.temperature.state.celsius < t1) { - device.plcs.reset.state.voltage = device.plcs.reset.down.settings.environment.voltage[i - 1]; - device.plcs.reset.state.countdown = device.plcs.reset.down.settings.environment.duration[i - 1]; - } - - t2 = device.plcs.reset.down.settings.environment.temperature[i]; - if (device.tss.temperature.state.celsius < t2) { - int32_t v1 = device.plcs.reset.down.settings.environment.voltage[i - 1]; - int32_t v2 = device.plcs.reset.down.settings.environment.voltage[i]; - device.plcs.reset.state.voltage = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, v1, v2); - int32_t d1 = device.plcs.reset.down.settings.environment.duration[i - 1]; - int32_t d2 = device.plcs.reset.down.settings.environment.duration[i]; - device.plcs.reset.state.countdown = plcsInterpolate(device.tss.temperature.state.celsius, t1, t2, d1, d2); - } - } - device.plcs.reset.state.voltage = device.plcs.reset.down.settings.environment.voltage[device.plcs.reset.down.settings.environment.points - 1]; - device.plcs.reset.state.countdown = device.plcs.reset.down.settings.environment.duration[device.plcs.reset.down.settings.environment.points - 1]; -}*/ - -void plcsReset(void) { - if (device.plcs.reset.state.countdown > 0){ - //Process jump - int32_t period = 0x00000005; //5/65536sec = 75 mks - //TODO: Set processing period from measurement cycle - - device.plcs.reset.state.countdown -= period; - if (device.plcs.reset.state.countdown < 0) device.plcs.reset.state.countdown = 0; - } else { - //Update jump parameters - plcsPreprocessJumpDown(); - plcsPreprocessJumpUp(); - //Detect reset condition - if (device.plcs.output.state.voltage > device.plcs.reset.up.state.trigger) { - //Process reset down - //plcsDown(); - device.plcs.reset.state.countdown = device.plcs.reset.down.state.duration; - device.plcs.output.state.voltage = device.plcs.reset.down.state.target; - } else if (device.plcs.output.state.voltage < device.plcs.reset.down.state.trigger) { - //Process reset up - //plcsUp(); - device.plcs.reset.state.countdown = device.plcs.reset.up.state.duration; - device.plcs.output.state.voltage = device.plcs.reset.down.state.target; - }; - } -} - -void plcsOutput(void) { - if (device.plcs.output.state.enabled) { - //Add sequencer output - if (device.plcs.output.state.sequencer) { - if (device.sequencer.output.analog.state.enabled) { - device.plcs.output.state.voltage += device.sequencer.output.analog.state.voltage; - } - } - - device.plcs.output.state.code = plcsCode(device.plcs.output.state.voltage); - device.controller.SSP.out[1] = device.plcs.output.state.code; - } -} - -//Main PLCS processing function -void plcsProcess(void) { - //Process reset state output voltage - if (device.plcs.output.state.enabled) { - if (device.plcs.reset.state.countdown > 0){ - device.plcs.output.state.voltage = device.plcs.reset.state.voltage; - } - } - //Process input - if (device.plcs.reference.state.delta) { - //Process delta state - if (device.plcs.feedback.state.input) { - device.plcs.feedback.state.voltage = device.isacs.input.state.V; - } else if (device.plcs.feedback.state.output) { - device.plcs.feedback.state.voltage = device.isacs.output.state.voltage; - } - - plcsProcessDelta(); - - } else if (device.plcs.reference.state.sequencer) { - //Process sequencer state - if (device.plcs.feedback.state.input) { - device.plcs.feedback.state.voltage = device.isacs.input.state.V; - } else if (device.plcs.feedback.state.output) { - device.plcs.feedback.state.voltage = device.isacs.output.state.voltage; - } - plcsProcessSequencer(); - - } - //Process regulator - plcsRegulate(); - //Process reset - plcsReset(); - //Process output - plcsOutput(); -} - -int16_t var_WP_reg(void) { - //Value: PLCS output voltage - V - -13.2V...10.7V - //Internal format: device.plcs.output.state.voltage - [V] in 16.16 format - //Output format: -8192 * (V + 1.3) / 3 - dimensionless units in 16.0 format - - //Voltage: V + 1.3 = (device.plcs.output.state.voltage + 0x00014ccd) / 65536 - //Output: -8192 * (V + 1.3) / 3 = -(device.plcs.output.state.voltage + 0x00014ccd) / 24 - - int32_t V = -(device.plcs.output.state.voltage + 0x00014ccd) / 24; - if (V > 32767) V = 32767; - if (V < -32768) V = -32768; - - return (int16_t)V; -} - -int16_t var_WP_pll(void) { - //Value: PLCS error - err - //Internal format: device.plcs.regulator.state.error - -1...1 in 16.16 format - //Output format: sum(err, 1 sec) - dimensionless units in 16.0 format - - //Now: Multiply 512 instead of sum for 1 sec and right shift 16 to transform in format 16.0 (shift 16 - 9 = 7) - //Signed right shift implementation-defined!!! Required to replace right shift 7 to division on 128 for implementation independence? - int32_t err = device.plcs.regulator.state.error / 128; - //TODO: make summation for 1 sec as in DSP-based program and tranform 16.0 - - return (int16_t)err; -} - -uint16_t param_WP_ref(void) { - //Value: PLCS regulator input signal reference voltage - V - //Internal format: device.plcs.regulator.state.reference - [V] in 16.16 format - //Output format: (V / v) * 15000 - dimensionless units in 16.0 format - //TODO: v = ??? Now v = 2.0 - - //Voltage: V = device.isacs.regulator.state.reference / 65536 - //Output: V = device.isacs.regulator.state.reference * 1875 / 16384 - - //TODO: state/settings - - int32_t V = device.plcs.regulator.state.reference * 1875 / 16384; - if (V > 32767) V = 32767; - if (V < 0) V = 0; - - return (uint16_t)V; -} - -//WP_scl s16 PLCS gain factor (1.15) 5..20 -uint16_t param_WP_scl(void) { - //Value: PLCS output regulator scale - S - -13.2V...10.7V - //Internal format: device.plcs.regulator.state.scale - dimensionless units in 16.16 format - //Output format: S / 65536 - dimensionless units in 16.0 format - - int32_t scl = device.plcs.regulator.state.scale / 65536; - if (scl > 32767) scl = 32767; - if (scl < 0) scl = 0; - - return (uint16_t)scl; -} - -//WP_mdy s16 PLCS reset delay in 10kHz cycles -uint16_t param_WP_mdy(void) { - //Value: PLCS reset delay - D [s] - //Internal format: device.plcs.reset.up/down.state.duration - [s] in 16.16 format - //Output format: (D / 65536) * 10000 - dimensionless units in 16.0 format - - //Delay: D = device.plcs.reset.up/down.state.duration / 65536 - //Output: mdy = device.plcs.reset.up/down.state.duration * 625 / 4096 - - //TODO: up/down??? - //Now: average for up and down - - int32_t mdy = (device.plcs.reset.up.state.duration + device.plcs.reset.down.state.duration) * 625 / 8192; - if (mdy > 32767) mdy = 32767; - if (mdy < 0) mdy = 0; - - return (uint16_t)mdy; -} - -//WP_rup dac ttf:PLCS maximum DAC code in heater channel; value:minimal voltage on the heater; value=(ttf>>4)/2048*(-12)-1.3; Range -13.2V...10.7V -uint16_t param_WP_rup(void) { - //Value: PLCS reset up trigger voltage - T [V] - //Internal format: device.plcs.reset.up.state.trigger - [V] in 16.16 format - //Output format: -8192 * (T + 1.3) / 3 - dimensionless units in 16.0 format - - //Voltage: T + 1.3 = (device.plcs.reset.up.state.trigger + 0x00014ccd) / 65536 - //Output: -8192 * (V + 1.3) / 3 = -(device.plcs.reset.up.state.trigger + 0x00014ccd) / 24 - - int32_t rup = -(device.plcs.reset.up.state.trigger + 0x00014ccd) / 24; - if (rup > 32767) rup = 32767; - if (rup < -32768) rup = -32768; - - return (int16_t)rup; -} - -//WP_rdw dac ttf:PLCS minimum DAC code in heater channel; value:maximal voltage on the heater; value=(ttf>>4)/2048*(-12)-1.3; Range -13.2V...10.7V -uint16_t param_WP_rdw(void) { - //Value: PLCS reset down trigger voltage - T [V] - //Internal format: device.plcs.reset.down.state.trigger - [V] in 16.16 format - //Output format: -8192 * (T + 1.3) / 3 - dimensionless units in 16.0 format - - //Voltage: T + 1.3 = (device.plcs.reset.down.state.trigger + 0x00014ccd) / 65536 - //Output: -8192 * (V + 1.3) / 3 = -(device.plcs.reset.down.state.trigger + 0x00014ccd) / 24 - - int32_t rdw = -(device.plcs.reset.down.state.trigger + 0x00014ccd) / 24; - if (rdw > 32767) rdw = 32767; - if (rdw < -32768) rdw = -32768; - - return (int16_t)rdw; -} - -//WP_reset dac ttf:PLCS DAC code after reset at heating; value:heater voltage after reset at heating; value=(ttf>>4)/2048*(-12)-1.3; Range -13.2V...10.7V -uint16_t param_WP_reset(void) { - //Value: PLCS reset up target voltage - T [V] - //Internal format: device.plcs.reset.up.state.target - [V] in 16.16 format - //Output format: -8192 * (T + 1.3) / 3 - dimensionless units in 16.0 format - - //Voltage: T + 1.3 = (device.plcs.reset.up.state.target + 0x00014ccd) / 65536 - //Output: -8192 * (V + 1.3) / 3 = -(device.plcs.reset.up.state.target + 0x00014ccd) / 24 - - int32_t reset = -(device.plcs.reset.up.state.target + 0x00014ccd) / 24; - if (reset > 32767) reset = 32767; - if (reset < -32768) reset = -32768; - - return (int16_t)reset; -} - -//WP_reset2 dac ttf:PLCS DAC code after reset at cooling; value:heater voltage after reset at cooling; value=(ttf>>4)/2048*(-12)-1.3 -uint16_t param_WP_reset2(void) { - //Value: PLCS reset down target voltage - T [V] - //Internal format: device.plcs.reset.down.state.target - [V] in 16.16 format - //Output format: -8192 * (T + 1.3) / 3 - dimensionless units in 16.0 format - - //Voltage: T + 1.3 = (device.plcs.reset.down.state.target + 0x00014ccd) / 65536 - //Output: -8192 * (V + 1.3) / 3 = -(device.plcs.reset.down.state.target + 0x00014ccd) / 24 - - int32_t reset2 = -(device.plcs.reset.down.state.target + 0x00014ccd) / 24; - if (reset2 > 32767) reset2 = 32767; - if (reset2 < -32768) reset2 = -32768; - - return (int16_t)reset2; -} \ No newline at end of file