Diff: DevicePLCS.c
- Revision:
- 198:78dd6d14d108
- Parent:
- 197:7a05523bf588
--- 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
igor Apu
