| User | Revision | Line number | New contents of line |
|---|
| Diletant |
149:abbf7663d27d
|
1
|
#include "Device.h"
|
| Diletant |
149:abbf7663d27d
|
2
|
extern Device device;
|
| Diletant |
149:abbf7663d27d
|
3
|
|
| Diletant |
167:bedc0a9d559a
|
4
|
void InitPathLengthControlSystemDefaultSettings(void){
|
| Diletant |
167:bedc0a9d559a
|
5
|
device.plcs.feedback.settings.input = 1;
|
| Diletant |
167:bedc0a9d559a
|
6
|
device.plcs.feedback.settings.output = 0;
|
| Diletant |
167:bedc0a9d559a
|
7
|
|
| Diletant |
167:bedc0a9d559a
|
8
|
device.plcs.reference.settings.sequencer = 1;
|
| Diletant |
167:bedc0a9d559a
|
9
|
device.plcs.reference.settings.delta = 0;
|
| Diletant |
167:bedc0a9d559a
|
10
|
|
| Diletant |
167:bedc0a9d559a
|
11
|
device.plcs.reset.levels.upper = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
12
|
device.plcs.reset.levels.lower = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
13
|
device.plcs.reset.up.points = 16;
|
| Diletant |
167:bedc0a9d559a
|
14
|
device.plcs.reset.up.temperature[0] = -0x3c0000; //-60C
|
| Diletant |
167:bedc0a9d559a
|
15
|
device.plcs.reset.up.voltage[0] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
16
|
device.plcs.reset.up.duration[0] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
17
|
device.plcs.reset.up.temperature[1] = -0x320000; //-50C
|
| Diletant |
167:bedc0a9d559a
|
18
|
device.plcs.reset.up.voltage[1] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
19
|
device.plcs.reset.up.duration[1] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
20
|
device.plcs.reset.up.temperature[2] = -0x280000; //-40C
|
| Diletant |
167:bedc0a9d559a
|
21
|
device.plcs.reset.up.voltage[2] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
22
|
device.plcs.reset.up.duration[2] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
23
|
device.plcs.reset.up.temperature[3] = -0x1e0000; //-30C
|
| Diletant |
167:bedc0a9d559a
|
24
|
device.plcs.reset.up.voltage[3] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
25
|
device.plcs.reset.up.duration[3] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
26
|
device.plcs.reset.up.temperature[4] = -0x140000; //-20C
|
| Diletant |
167:bedc0a9d559a
|
27
|
device.plcs.reset.up.voltage[4] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
28
|
device.plcs.reset.up.duration[4] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
29
|
device.plcs.reset.up.temperature[5] = -0xa0000; //-10C
|
| Diletant |
167:bedc0a9d559a
|
30
|
device.plcs.reset.up.voltage[5] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
31
|
device.plcs.reset.up.duration[5] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
32
|
device.plcs.reset.up.temperature[6] = 0x0; //0C
|
| Diletant |
167:bedc0a9d559a
|
33
|
device.plcs.reset.up.voltage[6] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
34
|
device.plcs.reset.up.duration[6] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
35
|
device.plcs.reset.up.temperature[7] = 0xa0000; //10C
|
| Diletant |
167:bedc0a9d559a
|
36
|
device.plcs.reset.up.voltage[7] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
37
|
device.plcs.reset.up.duration[7] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
38
|
device.plcs.reset.up.temperature[8] = 0x140000; //20C
|
| Diletant |
167:bedc0a9d559a
|
39
|
device.plcs.reset.up.voltage[8] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
40
|
device.plcs.reset.up.duration[8] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
41
|
device.plcs.reset.up.temperature[9] = 0x1e0000; //30C
|
| Diletant |
167:bedc0a9d559a
|
42
|
device.plcs.reset.up.voltage[9] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
43
|
device.plcs.reset.up.duration[9] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
44
|
device.plcs.reset.up.temperature[10] = 0x280000; //40C
|
| Diletant |
167:bedc0a9d559a
|
45
|
device.plcs.reset.up.voltage[10] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
46
|
device.plcs.reset.up.duration[10] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
47
|
device.plcs.reset.up.temperature[11] = 0x320000; //50C
|
| Diletant |
167:bedc0a9d559a
|
48
|
device.plcs.reset.up.voltage[11] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
49
|
device.plcs.reset.up.duration[11] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
50
|
device.plcs.reset.up.temperature[12] = 0x3c0000; //60C
|
| Diletant |
167:bedc0a9d559a
|
51
|
device.plcs.reset.up.voltage[12] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
52
|
device.plcs.reset.up.duration[12] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
53
|
device.plcs.reset.up.temperature[13] = 0x460000; //70C
|
| Diletant |
167:bedc0a9d559a
|
54
|
device.plcs.reset.up.voltage[13] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
55
|
device.plcs.reset.up.duration[13] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
56
|
device.plcs.reset.up.temperature[14] = 0x500000; //80C
|
| Diletant |
167:bedc0a9d559a
|
57
|
device.plcs.reset.up.voltage[14] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
58
|
device.plcs.reset.up.duration[14] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
59
|
device.plcs.reset.up.temperature[15] = 0x5a0000; //90C
|
| Diletant |
167:bedc0a9d559a
|
60
|
device.plcs.reset.up.voltage[15] = 0xc8000; //12.5V
|
| Diletant |
167:bedc0a9d559a
|
61
|
device.plcs.reset.up.duration[15] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
62
|
device.plcs.reset.down.points = 16;
|
| Diletant |
167:bedc0a9d559a
|
63
|
device.plcs.reset.down.temperature[0] = -0x3c0000; //-60C
|
| Diletant |
167:bedc0a9d559a
|
64
|
device.plcs.reset.down.voltage[0] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
65
|
device.plcs.reset.down.duration[0] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
66
|
device.plcs.reset.down.temperature[1] = -0x320000; //-50C
|
| Diletant |
167:bedc0a9d559a
|
67
|
device.plcs.reset.down.voltage[1] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
68
|
device.plcs.reset.down.duration[1] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
69
|
device.plcs.reset.down.temperature[2] = -0x280000; //-40C
|
| Diletant |
167:bedc0a9d559a
|
70
|
device.plcs.reset.down.voltage[2] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
71
|
device.plcs.reset.down.duration[2] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
72
|
device.plcs.reset.down.temperature[3] = -0x1e0000; //-30C
|
| Diletant |
167:bedc0a9d559a
|
73
|
device.plcs.reset.down.voltage[3] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
74
|
device.plcs.reset.down.duration[3] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
75
|
device.plcs.reset.down.temperature[4] = -0x140000; //-20C
|
| Diletant |
167:bedc0a9d559a
|
76
|
device.plcs.reset.down.voltage[4] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
77
|
device.plcs.reset.down.duration[4] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
78
|
device.plcs.reset.down.temperature[5] = -0xa0000; //-10C
|
| Diletant |
167:bedc0a9d559a
|
79
|
device.plcs.reset.down.voltage[5] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
80
|
device.plcs.reset.down.duration[5] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
81
|
device.plcs.reset.down.temperature[6] = 0x0; //0C
|
| Diletant |
167:bedc0a9d559a
|
82
|
device.plcs.reset.down.voltage[6] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
83
|
device.plcs.reset.down.duration[6] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
84
|
device.plcs.reset.down.temperature[7] = 0xa0000; //10C
|
| Diletant |
167:bedc0a9d559a
|
85
|
device.plcs.reset.down.voltage[7] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
86
|
device.plcs.reset.down.duration[7] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
87
|
device.plcs.reset.down.temperature[8] = 0x140000; //20C
|
| Diletant |
167:bedc0a9d559a
|
88
|
device.plcs.reset.down.voltage[8] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
89
|
device.plcs.reset.down.duration[8] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
90
|
device.plcs.reset.down.temperature[9] = 0x1e0000; //30C
|
| Diletant |
167:bedc0a9d559a
|
91
|
device.plcs.reset.down.voltage[9] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
92
|
device.plcs.reset.down.duration[9] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
93
|
device.plcs.reset.down.temperature[10] = 0x280000; //40C
|
| Diletant |
167:bedc0a9d559a
|
94
|
device.plcs.reset.down.voltage[10] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
95
|
device.plcs.reset.down.duration[10] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
96
|
device.plcs.reset.down.temperature[11] = 0x320000; //50C
|
| Diletant |
167:bedc0a9d559a
|
97
|
device.plcs.reset.down.voltage[11] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
98
|
device.plcs.reset.down.duration[11] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
99
|
device.plcs.reset.down.temperature[12] = 0x3c0000; //60C
|
| Diletant |
167:bedc0a9d559a
|
100
|
device.plcs.reset.down.voltage[12] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
101
|
device.plcs.reset.down.duration[12] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
102
|
device.plcs.reset.down.temperature[13] = 0x460000; //70C
|
| Diletant |
167:bedc0a9d559a
|
103
|
device.plcs.reset.down.voltage[13] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
104
|
device.plcs.reset.down.duration[13] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
105
|
device.plcs.reset.down.temperature[14] = 0x500000; //80C
|
| Diletant |
167:bedc0a9d559a
|
106
|
device.plcs.reset.down.voltage[14] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
107
|
device.plcs.reset.down.duration[14] = 0x30000; //3s
|
| Diletant |
167:bedc0a9d559a
|
108
|
device.plcs.reset.down.temperature[15] = 0x5a0000; //90C
|
| Diletant |
167:bedc0a9d559a
|
109
|
device.plcs.reset.down.voltage[15] = 0x18000; //1.5V
|
| Diletant |
167:bedc0a9d559a
|
110
|
device.plcs.reset.down.duration[15] = 0x30000; //3s
|
| Diletant |
173:7f938afb0447
|
111
|
|
| Diletant |
173:7f938afb0447
|
112
|
device.plcs.output.settings.sequencer = 1;
|
| Diletant |
149:abbf7663d27d
|
113
|
}
|
| Diletant |
149:abbf7663d27d
|
114
|
|
| Diletant |
167:bedc0a9d559a
|
115
|
void InitPathLengthControlSystemState(void){
|
| Diletant |
167:bedc0a9d559a
|
116
|
device.plcs.feedback.state.input = device.plcs.feedback.settings.input;
|
| Diletant |
167:bedc0a9d559a
|
117
|
device.plcs.feedback.state.output = device.plcs.feedback.settings.output;
|
| Diletant |
167:bedc0a9d559a
|
118
|
|
| Diletant |
167:bedc0a9d559a
|
119
|
device.plcs.reference.state.sequencer = device.plcs.reference.settings.sequencer;
|
| Diletant |
167:bedc0a9d559a
|
120
|
device.plcs.reference.state.delta = device.plcs.reference.settings.delta;
|
| Diletant |
167:bedc0a9d559a
|
121
|
|
| Diletant |
167:bedc0a9d559a
|
122
|
device.plcs.detector.state.out = 1;
|
| Diletant |
167:bedc0a9d559a
|
123
|
|
| Diletant |
167:bedc0a9d559a
|
124
|
device.plcs.reset.state.countdown = 0;
|
| Diletant |
173:7f938afb0447
|
125
|
|
| Diletant |
173:7f938afb0447
|
126
|
device.plcs.output.settings.sequencer = device.plcs.output.state.sequencer;
|
| Diletant |
167:bedc0a9d559a
|
127
|
}
|
| Diletant |
167:bedc0a9d559a
|
128
|
|
| Diletant |
167:bedc0a9d559a
|
129
|
void DeviceStartPLCS(void){
|
| Diletant |
149:abbf7663d27d
|
130
|
}
|
| Diletant |
149:abbf7663d27d
|
131
|
|
| Diletant |
156:e68ee0bcdcda
|
132
|
/*
|
| Diletant |
156:e68ee0bcdcda
|
133
|
//Move to DevicePLCS regulator
|
| Diletant |
156:e68ee0bcdcda
|
134
|
uint32_t value;
|
| Diletant |
156:e68ee0bcdcda
|
135
|
switch(device.plcs.state.modulation) {
|
| Diletant |
156:e68ee0bcdcda
|
136
|
case 1://малое воздействие
|
| Diletant |
156:e68ee0bcdcda
|
137
|
value = device.SSP.DAC[1] + Gyro.StrayPLC_Pls;
|
| Diletant |
156:e68ee0bcdcda
|
138
|
break;
|
| Diletant |
156:e68ee0bcdcda
|
139
|
|
| Diletant |
156:e68ee0bcdcda
|
140
|
case 3://малое воздействие
|
| Diletant |
156:e68ee0bcdcda
|
141
|
value = device.SSP.DAC[1] + Gyro.StrayPLC_Mns;
|
| Diletant |
156:e68ee0bcdcda
|
142
|
break;
|
| Diletant |
156:e68ee0bcdcda
|
143
|
|
| Diletant |
156:e68ee0bcdcda
|
144
|
case 2://большое воздействие
|
| Diletant |
156:e68ee0bcdcda
|
145
|
value = device.SSP.DAC[1] + Gyro.StrayPLC_2Mode;
|
| Diletant |
156:e68ee0bcdcda
|
146
|
break;
|
| Diletant |
156:e68ee0bcdcda
|
147
|
|
| Diletant |
156:e68ee0bcdcda
|
148
|
default://режим без воздействия
|
| Diletant |
156:e68ee0bcdcda
|
149
|
value = device.SSP.DAC[1];
|
| Diletant |
156:e68ee0bcdcda
|
150
|
break;
|
| Diletant |
156:e68ee0bcdcda
|
151
|
}
|
| Diletant |
156:e68ee0bcdcda
|
152
|
LPC_SSP0->DR = device.SSP.DAC[1];
|
| Diletant |
156:e68ee0bcdcda
|
153
|
*/
|
| Diletant |
156:e68ee0bcdcda
|
154
|
|
| Diletant |
166:c3c0b8a90d81
|
155
|
int32_t plcsInterpolate(int32_t a,int32_t a1,int32_t a2,int32_t b1,int32_t b2) {
|
| Diletant |
166:c3c0b8a90d81
|
156
|
int32_t ma, mb;
|
| Diletant |
166:c3c0b8a90d81
|
157
|
while (1) {
|
| Diletant |
166:c3c0b8a90d81
|
158
|
if (a1 == a) return b1;
|
| Diletant |
166:c3c0b8a90d81
|
159
|
if (a2 == a) return b2;
|
| Diletant |
166:c3c0b8a90d81
|
160
|
|
| Diletant |
166:c3c0b8a90d81
|
161
|
ma = (a1 + a2) >> 1;
|
| Diletant |
166:c3c0b8a90d81
|
162
|
mb = (b1 + b2) >> 1;
|
| Diletant |
166:c3c0b8a90d81
|
163
|
|
| Diletant |
166:c3c0b8a90d81
|
164
|
if (a < ma) {
|
| Diletant |
166:c3c0b8a90d81
|
165
|
if (a2 == ma) return mb;
|
| Diletant |
166:c3c0b8a90d81
|
166
|
if (b1 == mb) return mb;
|
| Diletant |
166:c3c0b8a90d81
|
167
|
a2 = ma; b2 = mb;
|
| Diletant |
166:c3c0b8a90d81
|
168
|
} else if (a > ma) {
|
| Diletant |
166:c3c0b8a90d81
|
169
|
if (a1 == ma) return mb;
|
| Diletant |
166:c3c0b8a90d81
|
170
|
if (b2 == mb) return mb;
|
| Diletant |
166:c3c0b8a90d81
|
171
|
a1 = ma; b1 = mb;
|
| Diletant |
166:c3c0b8a90d81
|
172
|
} else return mb;
|
| Diletant |
166:c3c0b8a90d81
|
173
|
}
|
| Diletant |
166:c3c0b8a90d81
|
174
|
}
|
| Diletant |
166:c3c0b8a90d81
|
175
|
|
| Diletant |
167:bedc0a9d559a
|
176
|
uint32_t plcsFeedback(int32_t voltage) {
|
| Diletant |
166:c3c0b8a90d81
|
177
|
int32_t v1, v2;
|
| Diletant |
167:bedc0a9d559a
|
178
|
for (uint8_t i = 1; i < device.plcs.feedback.settings.transfer.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
179
|
v1 = device.plcs.feedback.settings.transfer.raw[i - 1];
|
| Diletant |
166:c3c0b8a90d81
|
180
|
if (voltage < v1) {
|
| Diletant |
167:bedc0a9d559a
|
181
|
return device.plcs.feedback.settings.transfer.normalized[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
182
|
}
|
| Diletant |
167:bedc0a9d559a
|
183
|
|
| Diletant |
167:bedc0a9d559a
|
184
|
v2 = device.plcs.feedback.settings.transfer.raw[i];
|
| Diletant |
167:bedc0a9d559a
|
185
|
if (voltage < v2) {
|
| Diletant |
167:bedc0a9d559a
|
186
|
int32_t r1 = device.plcs.feedback.settings.transfer.normalized[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
187
|
int32_t r2 = device.plcs.feedback.settings.transfer.normalized[i];
|
| Diletant |
167:bedc0a9d559a
|
188
|
return plcsInterpolate(voltage, v1, v2, r1, r2);
|
| Diletant |
167:bedc0a9d559a
|
189
|
}
|
| Diletant |
167:bedc0a9d559a
|
190
|
}
|
| Diletant |
167:bedc0a9d559a
|
191
|
return device.plcs.feedback.settings.transfer.normalized[device.plcs.feedback.settings.transfer.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
192
|
}
|
| Diletant |
167:bedc0a9d559a
|
193
|
|
| Diletant |
167:bedc0a9d559a
|
194
|
uint32_t plcsBias(int32_t raw) {
|
| Diletant |
167:bedc0a9d559a
|
195
|
int32_t r1, r2;
|
| Diletant |
167:bedc0a9d559a
|
196
|
for (uint8_t i = 1; i < device.plcs.bias.settings.transfer.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
197
|
r1 = device.plcs.bias.settings.transfer.raw[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
198
|
if (raw < r1) {
|
| Diletant |
167:bedc0a9d559a
|
199
|
return device.plcs.bias.settings.transfer.normalized[i - 1];
|
| Diletant |
166:c3c0b8a90d81
|
200
|
}
|
| Diletant |
166:c3c0b8a90d81
|
201
|
|
| Diletant |
167:bedc0a9d559a
|
202
|
r2 = device.plcs.bias.settings.transfer.raw[i];
|
| Diletant |
167:bedc0a9d559a
|
203
|
if (raw < r2) {
|
| Diletant |
167:bedc0a9d559a
|
204
|
int32_t l1 = device.plcs.bias.settings.transfer.normalized[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
205
|
int32_t l2 = device.plcs.bias.settings.transfer.normalized[i];
|
| Diletant |
167:bedc0a9d559a
|
206
|
return plcsInterpolate(raw, r1, r2, l1, l2);
|
| Diletant |
167:bedc0a9d559a
|
207
|
}
|
| Diletant |
167:bedc0a9d559a
|
208
|
}
|
| Diletant |
167:bedc0a9d559a
|
209
|
return device.plcs.bias.settings.transfer.normalized[device.plcs.bias.settings.transfer.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
210
|
}
|
| Diletant |
167:bedc0a9d559a
|
211
|
|
| Diletant |
167:bedc0a9d559a
|
212
|
uint32_t plcsCorrection(int32_t error) {
|
| Diletant |
167:bedc0a9d559a
|
213
|
int32_t e1, e2;
|
| Diletant |
167:bedc0a9d559a
|
214
|
for (uint8_t i = 1; i < device.plcs.regulator.settings.transfer.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
215
|
e1 = device.plcs.regulator.settings.transfer.error[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
216
|
if (error < e1) {
|
| Diletant |
167:bedc0a9d559a
|
217
|
return device.plcs.regulator.settings.transfer.correction[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
218
|
}
|
| Diletant |
167:bedc0a9d559a
|
219
|
|
| Diletant |
167:bedc0a9d559a
|
220
|
e2 = device.plcs.regulator.settings.transfer.error[i];
|
| Diletant |
167:bedc0a9d559a
|
221
|
if (error < e2) {
|
| Diletant |
167:bedc0a9d559a
|
222
|
int32_t c1 = device.plcs.regulator.settings.transfer.correction[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
223
|
int32_t c2 = device.plcs.regulator.settings.transfer.correction[i];
|
| Diletant |
167:bedc0a9d559a
|
224
|
return plcsInterpolate(error, e1, e2, c1, c2);
|
| Diletant |
167:bedc0a9d559a
|
225
|
}
|
| Diletant |
167:bedc0a9d559a
|
226
|
}
|
| Diletant |
167:bedc0a9d559a
|
227
|
return device.plcs.regulator.settings.transfer.correction[device.plcs.regulator.settings.transfer.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
228
|
}
|
| Diletant |
167:bedc0a9d559a
|
229
|
|
| Diletant |
167:bedc0a9d559a
|
230
|
uint32_t plcsCode(int32_t voltage) {
|
| Diletant |
167:bedc0a9d559a
|
231
|
int32_t v1, v2;
|
| Diletant |
167:bedc0a9d559a
|
232
|
for (uint8_t i = 1; i < device.plcs.output.settings.transfer.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
233
|
v1 = device.plcs.output.settings.transfer.voltage[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
234
|
if (voltage < v1) {
|
| Diletant |
167:bedc0a9d559a
|
235
|
return device.plcs.output.settings.transfer.code[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
236
|
}
|
| Diletant |
167:bedc0a9d559a
|
237
|
|
| Diletant |
167:bedc0a9d559a
|
238
|
v2 = device.plcs.output.settings.transfer.voltage[i];
|
| Diletant |
166:c3c0b8a90d81
|
239
|
if (voltage < v2) {
|
| Diletant |
167:bedc0a9d559a
|
240
|
int32_t c1 = device.plcs.output.settings.transfer.code[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
241
|
int32_t c2 = device.plcs.output.settings.transfer.code[i];
|
| Diletant |
166:c3c0b8a90d81
|
242
|
return plcsInterpolate(voltage, v1, v2, c1, c2);
|
| Diletant |
166:c3c0b8a90d81
|
243
|
}
|
| Diletant |
166:c3c0b8a90d81
|
244
|
}
|
| Diletant |
167:bedc0a9d559a
|
245
|
return device.plcs.output.settings.transfer.code[device.plcs.output.settings.transfer.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
246
|
}
|
| Diletant |
167:bedc0a9d559a
|
247
|
|
| Diletant |
167:bedc0a9d559a
|
248
|
void plcsProcessDelta(void) {
|
| Diletant |
167:bedc0a9d559a
|
249
|
//Process detector
|
| Diletant |
167:bedc0a9d559a
|
250
|
int32_t feedback = plcsFeedback(device.plcs.feedback.state.voltage); //-1...+1 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
251
|
int32_t delta = feedback - device.plcs.detector.state.in[0]; //-2...+2 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
252
|
//if (delta > 0) delta = 1;
|
| Diletant |
167:bedc0a9d559a
|
253
|
//if (delta < 0) delta = -1;
|
| Diletant |
167:bedc0a9d559a
|
254
|
device.plcs.detector.state.in[0] = feedback;
|
| Diletant |
167:bedc0a9d559a
|
255
|
device.plcs.detector.state.out = ((delta >> 2) * (device.plcs.detector.state.in[1] >> 1)) >> 14; //-1...+1 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
256
|
|
| Diletant |
167:bedc0a9d559a
|
257
|
//Process bias -0.5...+0.5 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
258
|
device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out);
|
| Diletant |
167:bedc0a9d559a
|
259
|
device.plcs.bias.state.sum += device.plcs.bias.state.raw;
|
| Diletant |
167:bedc0a9d559a
|
260
|
device.plcs.bias.state.counter++;
|
| Diletant |
167:bedc0a9d559a
|
261
|
if (device.plcs.bias.state.counter == 256) {
|
| Diletant |
167:bedc0a9d559a
|
262
|
device.plcs.bias.state.average = device.plcs.bias.state.sum >> 8;
|
| Diletant |
167:bedc0a9d559a
|
263
|
device.plcs.bias.state.sum = 0;
|
| Diletant |
167:bedc0a9d559a
|
264
|
device.plcs.bias.state.counter = 0;
|
| Diletant |
167:bedc0a9d559a
|
265
|
|
| Diletant |
167:bedc0a9d559a
|
266
|
if (device.plcs.bias.state.average > 0)
|
| Diletant |
167:bedc0a9d559a
|
267
|
device.plcs.detector.state.in[1] = -1;
|
| Diletant |
167:bedc0a9d559a
|
268
|
else
|
| Diletant |
167:bedc0a9d559a
|
269
|
device.plcs.detector.state.in[1] = 1;
|
| Diletant |
167:bedc0a9d559a
|
270
|
}
|
| Diletant |
166:c3c0b8a90d81
|
271
|
}
|
| Diletant |
166:c3c0b8a90d81
|
272
|
|
| Diletant |
167:bedc0a9d559a
|
273
|
void plcsProcessSequencer(void) {
|
| Diletant |
167:bedc0a9d559a
|
274
|
//Process detector
|
| Diletant |
167:bedc0a9d559a
|
275
|
device.plcs.detector.state.in[0] = plcsFeedback(device.plcs.feedback.state.voltage) >> 1; //-1...+1 in 17.15 format
|
| Diletant |
173:7f938afb0447
|
276
|
device.plcs.detector.state.in[1] = device.sequencer.sampler.state.sample[1] >> 1; //-1...+1 in 17.15 format
|
| Diletant |
167:bedc0a9d559a
|
277
|
device.plcs.detector.state.out = device.plcs.detector.state.in[0] * device.plcs.detector.state.in[1] >> 14;//-1...+1 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
278
|
|
| Diletant |
167:bedc0a9d559a
|
279
|
//Process bias -0.5...+0.5 in 16.16 format
|
| Diletant |
167:bedc0a9d559a
|
280
|
device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out);
|
| Diletant |
167:bedc0a9d559a
|
281
|
device.plcs.bias.state.sum += device.plcs.bias.state.raw;
|
| Diletant |
167:bedc0a9d559a
|
282
|
device.plcs.bias.state.counter++;
|
| Diletant |
167:bedc0a9d559a
|
283
|
if (device.plcs.bias.state.counter == 64) {
|
| Diletant |
167:bedc0a9d559a
|
284
|
device.plcs.bias.state.average = device.plcs.bias.state.sum >> 6;
|
| Diletant |
167:bedc0a9d559a
|
285
|
device.plcs.bias.state.sum = 0;
|
| Diletant |
167:bedc0a9d559a
|
286
|
device.plcs.bias.state.counter = 0;
|
| Diletant |
167:bedc0a9d559a
|
287
|
}
|
| Diletant |
166:c3c0b8a90d81
|
288
|
}
|
| Diletant |
166:c3c0b8a90d81
|
289
|
|
| Diletant |
167:bedc0a9d559a
|
290
|
void plcsRegulate(void) {
|
| Diletant |
167:bedc0a9d559a
|
291
|
device.plcs.regulator.state.error = device.plcs.regulator.state.reference - device.plcs.bias.state.average;
|
| Diletant |
167:bedc0a9d559a
|
292
|
device.plcs.regulator.state.correction = plcsCorrection(device.plcs.regulator.state.error);
|
| Diletant |
167:bedc0a9d559a
|
293
|
|
| Diletant |
167:bedc0a9d559a
|
294
|
if (device.plcs.output.state.enabled) {
|
| Diletant |
167:bedc0a9d559a
|
295
|
if (device.plcs.reset.state.countdown == 0){
|
| Diletant |
167:bedc0a9d559a
|
296
|
if (device.plcs.regulator.state.enabled) {
|
| Diletant |
167:bedc0a9d559a
|
297
|
device.plcs.output.state.voltage += device.plcs.regulator.state.correction;
|
| Diletant |
167:bedc0a9d559a
|
298
|
}
|
| Diletant |
167:bedc0a9d559a
|
299
|
}
|
| Diletant |
167:bedc0a9d559a
|
300
|
}
|
| Diletant |
167:bedc0a9d559a
|
301
|
}
|
| Diletant |
167:bedc0a9d559a
|
302
|
|
| Diletant |
167:bedc0a9d559a
|
303
|
void plcsUp(void) {
|
| Diletant |
167:bedc0a9d559a
|
304
|
int32_t temperature = 0;
|
| Diletant |
167:bedc0a9d559a
|
305
|
//TODO: set temperature variable from temperature sensor
|
| Diletant |
167:bedc0a9d559a
|
306
|
|
| Diletant |
167:bedc0a9d559a
|
307
|
int32_t t1, t2;
|
| Diletant |
167:bedc0a9d559a
|
308
|
for (uint8_t i = 1; i < device.plcs.reset.up.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
309
|
t1 = device.plcs.reset.up.temperature[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
310
|
if (temperature < t1) {
|
| Diletant |
167:bedc0a9d559a
|
311
|
device.plcs.reset.state.voltage = device.plcs.reset.up.voltage[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
312
|
device.plcs.reset.state.countdown = device.plcs.reset.up.duration[i - 1];
|
| Diletant |
166:c3c0b8a90d81
|
313
|
}
|
| Diletant |
166:c3c0b8a90d81
|
314
|
|
| Diletant |
167:bedc0a9d559a
|
315
|
t2 = device.plcs.reset.up.temperature[i];
|
| Diletant |
167:bedc0a9d559a
|
316
|
if (temperature < t2) {
|
| Diletant |
167:bedc0a9d559a
|
317
|
int32_t v1 = device.plcs.reset.up.voltage[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
318
|
int32_t v2 = device.plcs.reset.up.voltage[i];
|
| Diletant |
167:bedc0a9d559a
|
319
|
device.plcs.reset.state.voltage = plcsInterpolate(temperature, t1, t2, v1, v2);
|
| Diletant |
167:bedc0a9d559a
|
320
|
int32_t d1 = device.plcs.reset.up.duration[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
321
|
int32_t d2 = device.plcs.reset.up.duration[i];
|
| Diletant |
167:bedc0a9d559a
|
322
|
device.plcs.reset.state.countdown = plcsInterpolate(temperature, t1, t2, d1, d2);
|
| Diletant |
166:c3c0b8a90d81
|
323
|
}
|
| Diletant |
166:c3c0b8a90d81
|
324
|
}
|
| Diletant |
167:bedc0a9d559a
|
325
|
device.plcs.output.state.voltage = device.plcs.reset.up.voltage[device.plcs.reset.up.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
326
|
device.plcs.reset.state.countdown = device.plcs.reset.up.duration[device.plcs.reset.up.points - 1];
|
| Diletant |
166:c3c0b8a90d81
|
327
|
}
|
| Diletant |
166:c3c0b8a90d81
|
328
|
|
| Diletant |
167:bedc0a9d559a
|
329
|
void plcsDown(void) {
|
| Diletant |
167:bedc0a9d559a
|
330
|
int32_t temperature = 0;
|
| Diletant |
167:bedc0a9d559a
|
331
|
//TODO: set temperature variable from temperature sensor
|
| Diletant |
166:c3c0b8a90d81
|
332
|
|
| Diletant |
167:bedc0a9d559a
|
333
|
int32_t t1, t2;
|
| Diletant |
167:bedc0a9d559a
|
334
|
for (uint8_t i = 1; i < device.plcs.reset.down.points; i++) {
|
| Diletant |
167:bedc0a9d559a
|
335
|
t1 = device.plcs.reset.down.temperature[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
336
|
if (temperature < t1) {
|
| Diletant |
167:bedc0a9d559a
|
337
|
device.plcs.reset.state.voltage = device.plcs.reset.down.voltage[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
338
|
device.plcs.reset.state.countdown = device.plcs.reset.down.duration[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
339
|
}
|
| Diletant |
167:bedc0a9d559a
|
340
|
|
| Diletant |
167:bedc0a9d559a
|
341
|
t2 = device.plcs.reset.down.temperature[i];
|
| Diletant |
167:bedc0a9d559a
|
342
|
if (temperature < t2) {
|
| Diletant |
167:bedc0a9d559a
|
343
|
int32_t v1 = device.plcs.reset.down.voltage[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
344
|
int32_t v2 = device.plcs.reset.down.voltage[i];
|
| Diletant |
167:bedc0a9d559a
|
345
|
device.plcs.reset.state.voltage = plcsInterpolate(temperature, t1, t2, v1, v2);
|
| Diletant |
167:bedc0a9d559a
|
346
|
int32_t d1 = device.plcs.reset.down.duration[i - 1];
|
| Diletant |
167:bedc0a9d559a
|
347
|
int32_t d2 = device.plcs.reset.down.duration[i];
|
| Diletant |
167:bedc0a9d559a
|
348
|
device.plcs.reset.state.countdown = plcsInterpolate(temperature, t1, t2, d1, d2);
|
| Diletant |
167:bedc0a9d559a
|
349
|
}
|
| Diletant |
167:bedc0a9d559a
|
350
|
}
|
| Diletant |
167:bedc0a9d559a
|
351
|
device.plcs.reset.state.voltage = device.plcs.reset.down.voltage[device.plcs.reset.down.points - 1];
|
| Diletant |
167:bedc0a9d559a
|
352
|
device.plcs.reset.state.countdown = device.plcs.reset.down.duration[device.plcs.reset.down.points - 1];
|
| Diletant |
166:c3c0b8a90d81
|
353
|
}
|
| Diletant |
167:bedc0a9d559a
|
354
|
|
| Diletant |
167:bedc0a9d559a
|
355
|
void plcsReset(void) {
|
| Diletant |
167:bedc0a9d559a
|
356
|
if (device.plcs.reset.state.countdown > 0){
|
| Diletant |
167:bedc0a9d559a
|
357
|
int32_t period = 0x00000005; //5/65536sec = 75 mks
|
| Diletant |
167:bedc0a9d559a
|
358
|
//TODO: Set processing period from measurement cycle
|
| Diletant |
167:bedc0a9d559a
|
359
|
|
| Diletant |
167:bedc0a9d559a
|
360
|
device.plcs.reset.state.countdown -= period;
|
| Diletant |
167:bedc0a9d559a
|
361
|
if (device.plcs.reset.state.countdown < 0) device.plcs.reset.state.countdown = 0;
|
| Diletant |
167:bedc0a9d559a
|
362
|
} else {
|
| Diletant |
167:bedc0a9d559a
|
363
|
if (device.plcs.output.state.voltage > device.plcs.reset.levels.upper) {
|
| Diletant |
167:bedc0a9d559a
|
364
|
plcsDown();
|
| Diletant |
167:bedc0a9d559a
|
365
|
} else if (device.plcs.output.state.voltage < device.plcs.reset.levels.lower) {
|
| Diletant |
167:bedc0a9d559a
|
366
|
plcsUp();
|
| Diletant |
167:bedc0a9d559a
|
367
|
};
|
| Diletant |
167:bedc0a9d559a
|
368
|
}
|
| Diletant |
167:bedc0a9d559a
|
369
|
}
|
| Diletant |
166:c3c0b8a90d81
|
370
|
|
| Diletant |
166:c3c0b8a90d81
|
371
|
void plcsOutput(void) {
|
| Diletant |
166:c3c0b8a90d81
|
372
|
if (device.plcs.output.state.enabled) {
|
| Diletant |
173:7f938afb0447
|
373
|
//Add sequencer output
|
| Diletant |
173:7f938afb0447
|
374
|
if (device.plcs.output.state.sequencer) {
|
| Diletant |
173:7f938afb0447
|
375
|
if (device.sequencer.output.analog.state.enabled) {
|
| Diletant |
173:7f938afb0447
|
376
|
device.plcs.output.state.voltage += device.sequencer.output.analog.state.voltage;
|
| Diletant |
173:7f938afb0447
|
377
|
}
|
| Diletant |
173:7f938afb0447
|
378
|
}
|
| Diletant |
173:7f938afb0447
|
379
|
|
| Diletant |
167:bedc0a9d559a
|
380
|
device.plcs.output.state.code = plcsCode(device.plcs.output.state.voltage);
|
| Diletant |
167:bedc0a9d559a
|
381
|
device.controller.SSP.out[1] = device.plcs.output.state.code;
|
| Diletant |
166:c3c0b8a90d81
|
382
|
}
|
| Diletant |
166:c3c0b8a90d81
|
383
|
}
|
| Diletant |
166:c3c0b8a90d81
|
384
|
|
| Diletant |
167:bedc0a9d559a
|
385
|
//Main PLCS processing function
|
| Diletant |
167:bedc0a9d559a
|
386
|
void plcsProcess(void) {
|
| Diletant |
167:bedc0a9d559a
|
387
|
//Process reset state output voltage
|
| Diletant |
167:bedc0a9d559a
|
388
|
if (device.plcs.output.state.enabled) {
|
| Diletant |
167:bedc0a9d559a
|
389
|
if (device.plcs.reset.state.countdown > 0){
|
| Diletant |
167:bedc0a9d559a
|
390
|
device.plcs.output.state.voltage = device.plcs.reset.state.voltage;
|
| Diletant |
166:c3c0b8a90d81
|
391
|
}
|
| Diletant |
166:c3c0b8a90d81
|
392
|
}
|
| Diletant |
167:bedc0a9d559a
|
393
|
//Process input
|
| Diletant |
167:bedc0a9d559a
|
394
|
if (device.plcs.reference.state.delta) {
|
| Diletant |
167:bedc0a9d559a
|
395
|
//Process delta state
|
| Diletant |
167:bedc0a9d559a
|
396
|
if (device.plcs.feedback.state.input) {
|
| Diletant |
167:bedc0a9d559a
|
397
|
device.plcs.feedback.state.voltage = device.isacs.input.state.average;
|
| Diletant |
167:bedc0a9d559a
|
398
|
} else if (device.plcs.feedback.state.output) {
|
| Diletant |
167:bedc0a9d559a
|
399
|
device.plcs.feedback.state.voltage = device.isacs.output.state.voltage;
|
| Diletant |
166:c3c0b8a90d81
|
400
|
}
|
| Diletant |
166:c3c0b8a90d81
|
401
|
|
| Diletant |
167:bedc0a9d559a
|
402
|
/*
|
| Diletant |
167:bedc0a9d559a
|
403
|
device.plcs.feedback.state.sum += device.plcs.feedback.state.voltage;
|
| Diletant |
166:c3c0b8a90d81
|
404
|
if (device.measurement.counter == 31) {
|
| Diletant |
167:bedc0a9d559a
|
405
|
if (device.plcs.feedback.state.counter == 255) {
|
| Diletant |
167:bedc0a9d559a
|
406
|
device.plcs.feedback.state.average = device.plcs.feedback.state.sum >> 8;
|
| Diletant |
167:bedc0a9d559a
|
407
|
device.plcs.feedback.state.sum = 0;
|
| Diletant |
167:bedc0a9d559a
|
408
|
device.plcs.feedback.state.counter = 0;
|
| Diletant |
166:c3c0b8a90d81
|
409
|
plcsProcessDelta();
|
| Diletant |
166:c3c0b8a90d81
|
410
|
} else {
|
| Diletant |
167:bedc0a9d559a
|
411
|
device.plcs.feedback.state.counter++;
|
| Diletant |
166:c3c0b8a90d81
|
412
|
}
|
| Diletant |
166:c3c0b8a90d81
|
413
|
}
|
| Diletant |
167:bedc0a9d559a
|
414
|
*/
|
| Diletant |
167:bedc0a9d559a
|
415
|
plcsProcessDelta();
|
| Diletant |
166:c3c0b8a90d81
|
416
|
|
| Diletant |
167:bedc0a9d559a
|
417
|
} else if (device.plcs.reference.state.sequencer) {
|
| Diletant |
167:bedc0a9d559a
|
418
|
//Process sequencer state
|
| Diletant |
167:bedc0a9d559a
|
419
|
if (device.plcs.feedback.state.input) {
|
| Diletant |
167:bedc0a9d559a
|
420
|
device.plcs.feedback.state.voltage = device.isacs.input.state.voltage;
|
| Diletant |
167:bedc0a9d559a
|
421
|
} else if (device.plcs.feedback.state.output) {
|
| Diletant |
167:bedc0a9d559a
|
422
|
device.plcs.feedback.state.voltage = device.isacs.output.state.voltage;
|
| Diletant |
166:c3c0b8a90d81
|
423
|
}
|
| Diletant |
166:c3c0b8a90d81
|
424
|
plcsProcessSequencer();
|
| Diletant |
166:c3c0b8a90d81
|
425
|
|
| Diletant |
166:c3c0b8a90d81
|
426
|
}
|
| Diletant |
167:bedc0a9d559a
|
427
|
//Process regulator
|
| Diletant |
167:bedc0a9d559a
|
428
|
plcsRegulate();
|
| Diletant |
167:bedc0a9d559a
|
429
|
//Process reset
|
| Diletant |
167:bedc0a9d559a
|
430
|
plcsReset();
|
| Diletant |
167:bedc0a9d559a
|
431
|
//Process output
|
| Diletant |
167:bedc0a9d559a
|
432
|
plcsOutput();
|
| Diletant |
149:abbf7663d27d
|
433
|
} |
igor Apu
