source: trunk/FACT++/src/feedback.cc@ 12043

Last change on this file since 12043 was 12028, checked in by tbretz, 15 years ago
File size: 19.0 KB
Line 
1#include <valarray>
2
3#include "Dim.h"
4#include "Event.h"
5#include "Shell.h"
6#include "StateMachineDim.h"
7#include "Connection.h"
8#include "Configuration.h"
9#include "Console.h"
10#include "Converter.h"
11#include "DimServiceInfoList.h"
12#include "PixelMap.h"
13
14#include "tools.h"
15
16#include "LocalControl.h"
17
18#include "HeadersFAD.h"
19#include "HeadersBIAS.h"
20
21namespace ba = boost::asio;
22namespace bs = boost::system;
23namespace dummy = ba::placeholders;
24
25using namespace std;
26
27// ------------------------------------------------------------------------
28
29#include "DimDescriptionService.h"
30
31// ------------------------------------------------------------------------
32
33class StateMachineFeedback : public StateMachineDim, public DimInfoHandler
34{
35 /*
36 int Wrap(boost::function<void()> f)
37 {
38 f();
39 return T::GetCurrentState();
40 }
41
42 boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
43 {
44 return bind(&StateMachineMCP::Wrap, this, func);
45 }*/
46
47private:
48 enum states_t
49 {
50 kStateDimNetworkNA = 1,
51 kStateDisconnected,
52 kStateConnecting,
53 kStateConnected,
54 kStateRunning,
55 };
56
57 PixelMap fMap;
58
59 DimServiceInfoList fNetwork;
60
61 pair<Time, int> fStatusDim;
62 pair<Time, int> fStatusFAD;
63 pair<Time, int> fStatusBias;
64
65 DimStampedInfo fDim;
66 DimStampedInfo fFAD;
67 DimStampedInfo fBias;
68 DimStampedInfo *fBiasData;
69
70 DimDescribedService fDimReference;
71 DimDescribedService fDimDeviation;
72
73 vector<vector<float>> fData;
74
75 int fCursor;
76
77 Time fBiasLast;
78 Time fStartTime;
79
80 valarray<double> fPV[3]; // Process variable (intgerated/averaged amplitudes)
81 valarray<double> fSP; // Set point (target amplitudes)
82
83 double fT;
84 double fKp; // Proportional constant
85 double fKi; // Integral constant
86 double fKd; // Derivative constant
87
88 pair<Time, int> GetNewState(DimStampedInfo &info) const
89 {
90 const bool disconnected = info.getSize()==0;
91
92 // Make sure getTimestamp is called _before_ getTimestampMillisecs
93 const int tsec = info.getTimestamp();
94 const int tms = info.getTimestampMillisecs();
95
96 return make_pair(Time(tsec, tms*1000),
97 disconnected ? -2 : info.getQuality());
98 }
99
100 void ResetData()
101 {
102 fData.clear();
103 fData.resize(500);
104 fCursor = 0;
105 fStartTime = Time();
106
107 fSP.resize(416);
108 fSP = valarray<double>(0., 416);
109
110 fPV[0].resize(0);
111 fPV[1].resize(0);
112 fPV[2].resize(0);
113 }
114
115 void infoHandler()
116 {
117 DimInfo *curr = getInfo(); // get current DimInfo address
118 if (!curr)
119 return;
120
121 if (curr==&fBias)
122 {
123 fStatusBias = GetNewState(fBias);
124 return;
125 }
126
127 if (curr==&fFAD)
128 {
129 fStatusFAD = GetNewState(fFAD);
130 return;
131 }
132
133 if (curr==&fDim)
134 {
135 fStatusDim = GetNewState(fDim);
136 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
137 return;
138 }
139
140 if (curr==fBiasData)
141 {
142 if (curr->getSize()==0)
143 return;
144 if (curr->getSize()!=1440*sizeof(float))
145 return;
146
147 // -------- Check age of last stored event --------
148
149 // Must be called in this order
150 const int tsec = curr->getTimestamp();
151 const int tms = curr->getTimestampMillisecs();
152
153 const Time tm(tsec, tms*1000);
154
155 if (Time()-fBiasLast>boost::posix_time::seconds(30))
156 {
157 Warn("Last received event data older than 30s... resetting average calculation.");
158 ResetData();
159 }
160 fBiasLast = tm;
161
162 // -------- Store new event --------
163
164 fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
165 reinterpret_cast<float*>(curr->getData())+1440);
166
167
168 ostringstream out;
169 out << "New event received: " << fCursor;
170 Info(out);
171
172 fCursor++;
173
174 if (fCursor<fData.size())
175 return;
176
177 // -------- Calculate statistics --------
178
179 valarray<double> med(1440);
180
181 for (int ch=0; ch<1440; ch++)
182 {
183 vector<float> arr(fData.size());
184 for (size_t i=0; i<fData.size(); i++)
185 arr[i] = fData[i][ch];
186
187 sort(arr.begin(), arr.end());
188
189 med[ch] = arr[arr.size()/2];
190 }
191
192 /*
193 vector<float> med(1440);
194 vector<float> rms(1440);
195 for (size_t i=0; i<fData.size(); i++)
196 {
197 if (fData[i].size()==0)
198 return;
199
200 for (int j=0; j<1440; j++)
201 {
202 med[j] += fData[i][j];
203 rms[j] += fData[i][j]*fData[i][j];
204 }
205 }
206 */
207
208 vector<double> avg(BIAS::kNumChannels);
209 vector<int> num(BIAS::kNumChannels);
210 for (int i=0; i<1440; i++)
211 {
212 const PixelMapEntry &ch = fMap.hw(i);
213
214 // FIXME: Add a consistency check if the median makes sense...
215 // FIXME: Add a consistency check to remove pixels with bright stars (median?)
216
217 avg[ch.hv()] += med[i];
218 num[ch.hv()]++;
219 }
220
221 for (int i=0; i<BIAS::kNumChannels; i++)
222 {
223 if (num[i])
224 avg[i] /= num[i];
225
226 }
227
228 // -------- Calculate correction --------
229
230 // http://bestune.50megs.com/typeABC.htm
231
232 // CO: Controller output
233 // PV: Process variable
234 // SP: Set point
235 // T: Sampling period (loop update period)
236 // e = SP - PV
237 //
238 // Kp : No units
239 // Ki : per seconds
240 // Kd : seconds
241
242 // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
243
244 if (fCursor%fData.size()==0)
245 {
246 // FIXME: Take out broken / dead boards.
247
248 const Time tm0 = Time();
249
250 const double T21 = (tm0-fStartTime).total_microseconds()/1000000.;
251 const double T10 = fT;
252 fT = T21;
253
254 fStartTime = tm0;
255
256 cout << "Step..." << endl;
257
258 if (fPV[0].size()==0)
259 {
260 fPV[0].resize(avg.size());
261 fPV[0] = valarray<double>(avg.data(), avg.size());
262 }
263 else
264 if (fPV[1].size()==0)
265 {
266 fPV[1].resize(avg.size());
267 fPV[1] = valarray<double>(avg.data(), avg.size());
268 }
269 else
270 if (fPV[2].size()==0)
271 {
272 fPV[2].resize(avg.size());
273 fPV[2] = valarray<double>(avg.data(), avg.size());
274 }
275 else
276 {
277 fPV[0] = fPV[1];
278 fPV[1] = fPV[2];
279
280 fPV[2].resize(avg.size());
281 fPV[2] = valarray<double>(avg.data(), avg.size());
282
283 if (T10<=0 || T21<=0)
284 return;
285
286 cout << "Calculating (" << fCursor << ")... " << endl;
287
288 // fKi[j] = response[j]*gain;
289 // Kp = 0;
290 // Kd = 0;
291
292 // (PV[2]-PV[1] + PV[0]-PV[1])
293 //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
294 //valarray<double> correction =
295 // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
296 const valarray<double> correction =
297 - (fKp+fKd/T21)*(fPV[2] - fPV[1]) + fKi*T21*(fSP-fPV[2]) + fKd/T10*(fPV[1]-fPV[0]);
298
299 vector<float> vec(2*BIAS::kNumChannels);
300 for (int i=0; i<BIAS::kNumChannels; i++)
301 vec[i] = fSP[i]-fPV[2][i];
302
303 for (int i=0; i<BIAS::kNumChannels; i++)
304 vec[i+416] = correction[i];
305
306 fDimDeviation.Update(vec);
307 }
308
309 }
310
311 /*
312 vector<float> correction(416);
313 vector<float> response(416);
314 vector<float> dev(416);
315
316 double gain = 0;
317
318 for (int j=0; j<416; j++)
319 {
320 //avg[j] /= fData.size();
321 //rms[j] /= sqrt((rms[j]*fData.size() - avg[j]*avg[j]))/fData.size();
322
323 dev[j] = avg[j] - target[j];
324
325 // Determine correction from response maxtrix and change voltages
326 correction[j] = -dev[j]*response[j]*gain;
327
328 if (correction[j]==0)
329 continue;
330
331 // Limit voltage steps // Limit changes to 100 mV
332// if (fabs(correction[j]) > 0.1)
333// correction[j] = 0.1*fabs(correction[j])/correction[j];
334
335 // Add voltage change command if not too noisy
336// if (fabs(Average[i]) > 2*Sigma[i])
337// Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";
338
339 }
340 */
341 return;
342 }
343
344 }
345
346 bool CheckEventSize(size_t has, const char *name, size_t size)
347 {
348 if (has==size)
349 return true;
350
351 ostringstream msg;
352 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
353 Fatal(msg);
354 return false;
355 }
356
357 void PrintState(const pair<Time,int> &state, const char *server)
358 {
359 const State rc = fNetwork.GetState(server, state.second);
360
361 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
362 Out() << kBold << server << ": ";
363 Out() << rc.name << "[" << rc.index << "]";
364 Out() << kReset << " - " << kBlue << rc.comment << endl;
365 }
366
367 int Print()
368 {
369 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
370 Out() << kBold << "DIM_DNS: ";
371 if (fStatusDim.second==0)
372 Out() << "Offline" << endl;
373 else
374 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
375
376 PrintState(fStatusFAD, "FAD_CONTROL");
377 PrintState(fStatusBias, "BIAS_CONTROL");
378
379 return GetCurrentState();
380 }
381
382 int SetConstant(const EventImp &evt, int constant)
383 {
384 if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
385 return T::kSM_FatalError;
386
387 switch (constant)
388 {
389 case 0: fKi = evt.GetDouble(); break,
390 case 1: fKp = evt.GetDouble(); break;
391 case 2: fKd = evt.GetDouble(); break;
392 default:
393 Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
394 }
395
396 return GetCurrentState();
397 }
398
399 int StartFeedback()
400 {
401 fBiasData = new DimStampedInfo("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this);
402
403 ResetData();
404
405 return GetCurrentState();
406 }
407
408 int StopFeedback()
409 {
410 delete fBiasData;
411 fBiasData = 0;
412
413 return GetCurrentState();
414 }
415
416 int StoreReference()
417 {
418 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
419 {
420 Warn("No values in memory. Take enough events first!");
421 return GetCurrentState();
422 }
423
424 // FIXME: Check age
425
426 if (!fPV[1].size() && !fPV[2].size())
427 fSP = fPV[0];
428
429 if (!fPV[2].size())
430 fSP = fPV[1];
431 else
432 fSP = fPV[2];
433
434 vector<float> vec(BIAS::kNumChannels);
435 for (int i=0; i<BIAS::kNumChannels; i++)
436 vec[i] = fSP[i];
437 fDimReference.Update(vec);
438
439 return GetCurrentState();
440 }
441
442
443 int Execute()
444 {
445 // Dispatch (execute) at most one handler from the queue. In contrary
446 // to run_one(), it doesn't wait until a handler is available
447 // which can be dispatched, so poll_one() might return with 0
448 // handlers dispatched. The handlers are always dispatched/executed
449 // synchronously, i.e. within the call to poll_one()
450 //poll_one();
451
452 if (fStatusDim.second==0)
453 return kStateDimNetworkNA;
454
455 if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnecting)
456 return kStateDisconnected;
457
458 if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnected)
459 return kStateConnecting;
460
461 return fBiasData ? kStateRunning : kStateConnected;
462 }
463
464public:
465 StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
466 fStatusDim(make_pair(Time(), -2)),
467 fStatusFAD(make_pair(Time(), -2)),
468 fStatusBias(make_pair(Time(), -2)),
469 fDim("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
470 fFAD("FAD_CONTROL/STATE", (void*)NULL, 0, this),
471 fBias("BIAS_CONTROL/STATE", (void*)NULL, 0, this),
472 fDimReference("FEEDBACK/REFERENCE", "F:416", ""),
473 fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416", ""),
474 fBiasData(NULL)
475 {
476 // ba::io_service::work is a kind of keep_alive for the loop.
477 // It prevents the io_service to go to stopped state, which
478 // would prevent any consecutive calls to run()
479 // or poll() to do nothing. reset() could also revoke to the
480 // previous state but this might introduce some overhead of
481 // deletion and creation of threads and more.
482
483 // State names
484 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
485 ".");
486
487 AddStateName(kStateDisconnected, "Disconnected",
488 ".");
489
490 AddStateName(kStateConnecting, "Connecting",
491 ".");
492
493 AddStateName(kStateConnected, "Connected",
494 ".");
495
496 AddStateName(kStateRunning, "Running",
497 ".");
498
499 AddEvent("START")//, kStateIdle)
500 (bind(&StateMachineFeedback::StartFeedback, this))
501 ("");
502
503 AddEvent("STOP")//, kStateIdle)
504 (bind(&StateMachineFeedback::StopFeedback, this))
505 ("");
506
507 AddEvent("STORE_REFERENCE")//, kStateIdle)
508 (bind(&StateMachineFeedback::StoreReference, this))
509 ("Stored the last (averaged) value as new reference (for debug purpose only)");
510
511 AddEvent("SET_Ki", "D:1")//, kStateIdle)
512 (bind(&StateMachineFeedback::SetConstant, this, 0))
513 ("Set integral constant Ki");
514
515 AddEvent("SET_Kp", "D:1")//, kStateIdle)
516 (bind(&StateMachineFeedback::SetConstant, this, 1))
517 ("Set proportional constant Kp");
518
519 AddEvent("SET_Kd", "D:1")//, kStateIdle)
520 (bind(&StateMachineFeedback::SetConstant, this, 2))
521 ("Set derivative constant Kd");
522
523 // Verbosity commands
524// AddEvent("SET_VERBOSE", "B:1")
525// (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
526// ("set verbosity state"
527// "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
528
529 AddEvent("PRINT")
530 (bind(&StateMachineFeedback::Print, this))
531 ("");
532 }
533
534 int EvalOptions(Configuration &)
535 {
536 //SetEndpoint(conf.Get<string>("addr"));
537
538 //fFSC.SetVerbose(!conf.Get<bool>("quiet"));
539
540 if (!fMap.Read("FACTmapV5.txt"))
541 {
542 Error("Reading mapping table from FACTmapV5.txt failed.");
543 return 1;
544 }
545
546 /*
547 if (!fBias.SetNewGapdVoltage(map.Vgapd()))
548 {
549 T::Error("Setting reference voltages failed.");
550 return 6;
551 }*/
552
553 return -1;
554 }
555};
556
557// ------------------------------------------------------------------------
558
559#include "Main.h"
560
561template<class T>
562int RunShell(Configuration &conf)
563{
564 return Main::execute<T, StateMachineFeedback>(conf);
565}
566
567/*
568 Extract usage clause(s) [if any] for SYNOPSIS.
569 Translators: "Usage" and "or" here are patterns (regular expressions) which
570 are used to match the usage synopsis in program output. An example from cp
571 (GNU coreutils) which contains both strings:
572 Usage: cp [OPTION]... [-T] SOURCE DEST
573 or: cp [OPTION]... SOURCE... DIRECTORY
574 or: cp [OPTION]... -t DIRECTORY SOURCE...
575 */
576void PrintUsage()
577{
578 cout <<
579 "The ftmctrl controls the FSC (FACT Slow Control) board.\n"
580 "\n"
581 "The default is that the program is started without user intercation. "
582 "All actions are supposed to arrive as DimCommands. Using the -c "
583 "option, a local shell can be initialized. With h or help a short "
584 "help message about the usuage can be brought to the screen.\n"
585 "\n"
586 "Usage: fscctrl [-c type] [OPTIONS]\n"
587 " or: fscctrl [OPTIONS]\n";
588 cout << endl;
589}
590
591void PrintHelp()
592{
593 /* Additional help text which is printed after the configuration
594 options goes here */
595
596 /*
597 cout << "bla bla bla" << endl << endl;
598 cout << endl;
599 cout << "Environment:" << endl;
600 cout << "environment" << endl;
601 cout << endl;
602 cout << "Examples:" << endl;
603 cout << "test exam" << endl;
604 cout << endl;
605 cout << "Files:" << endl;
606 cout << "files" << endl;
607 cout << endl;
608 */
609}
610
611int main(int argc, const char* argv[])
612{
613 Configuration conf(argv[0]);
614 conf.SetPrintUsage(PrintUsage);
615 Main::SetupConfiguration(conf);
616
617 if (!conf.DoParse(argc, argv, PrintHelp))
618 return -1;
619
620 //try
621 {
622 // No console access at all
623 if (!conf.Has("console"))
624 {
625// if (conf.Get<bool>("no-dim"))
626// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
627// else
628 return RunShell<LocalStream>(conf);
629 }
630 // Cosole access w/ and w/o Dim
631/* if (conf.Get<bool>("no-dim"))
632 {
633 if (conf.Get<int>("console")==0)
634 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
635 else
636 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
637 }
638 else
639*/ {
640 if (conf.Get<int>("console")==0)
641 return RunShell<LocalShell>(conf);
642 else
643 return RunShell<LocalConsole>(conf);
644 }
645 }
646 /*catch (std::exception& e)
647 {
648 cerr << "Exception: " << e.what() << endl;
649 return -1;
650 }*/
651
652 return 0;
653}
Note: See TracBrowser for help on using the repository browser.