source: trunk/FACT++/src/ratecontrol.cc@ 13217

Last change on this file since 13217 was 12700, checked in by tbretz, 13 years ago
Always the lower rate should be used, either the camera rate or the scaled board rate.
File size: 20.2 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 "HeadersFTM.h"
19
20namespace ba = boost::asio;
21namespace bs = boost::system;
22namespace dummy = ba::placeholders;
23
24using namespace std;
25
26// ------------------------------------------------------------------------
27
28#include "DimDescriptionService.h"
29
30// ------------------------------------------------------------------------
31
32class StateMachineRateControl : public StateMachineDim, public DimInfoHandler
33{
34private:
35 enum states_t
36 {
37 kStateDimNetworkNA = 1,
38 kStateDisconnected,
39 kStateConnecting,
40 kStateConnected,
41
42 kStateSettingGlobalThreshold,
43 kStateGlobalThresholdSet,
44
45 kStateInProgress,
46 };
47
48 DimServiceInfoList fNetwork;
49
50 pair<Time, int> fStatusDim;
51 pair<Time, int> fStatusFTM;
52 pair<Time, int> fStatusRS;
53
54 DimStampedInfo fDim;
55 DimStampedInfo fFTM;
56 DimStampedInfo fRates;
57 DimStampedInfo fStatic;
58 DimStampedInfo fRateScan;
59
60// DimDescribedService fDimData;
61// DimDescribedService fDimProc;
62
63 float fTargetRate;
64 float fTriggerRate;
65
66 uint16_t fThresholdMin;
67 uint16_t fThresholdReference;
68
69 bool fTriggerOn;
70 bool fVerbose;
71 bool fEnabled;
72
73 uint64_t fCounter;
74
75 pair<Time, int> GetNewState(DimStampedInfo &info) const
76 {
77 const bool disconnected = info.getSize()==0;
78
79 // Make sure getTimestamp is called _before_ getTimestampMillisecs
80 const int tsec = info.getTimestamp();
81 const int tms = info.getTimestampMillisecs();
82
83 return make_pair(Time(tsec, tms*1000),
84 disconnected ? -2 : info.getQuality());
85 }
86
87 bool CheckEventSize(size_t has, const char *name, size_t size)
88 {
89 if (has==size)
90 return true;
91
92 if (has==0)
93 return false;
94
95 ostringstream msg;
96 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
97 Fatal(msg);
98 return false;
99 }
100
101 vector<uint16_t> fThresholds;
102
103 void PrintThresholds(const FTM::DimStaticData &sdata)
104 {
105 //if (!fVerbose)
106 // return;
107
108 if (fThresholds.size()==0)
109 return;
110
111 Out() << "Min. DAC=" << fThresholdMin << endl;
112
113 int t=0;
114 for (t=0; t<160; t++)
115 if (sdata.fThreshold[t]!=fThresholds[t])
116 break;
117
118 if (t==160)
119 return;
120
121 for (int j=0; j<10; j++)
122 {
123 for (int k=0; k<4; k++)
124 {
125 for (int i=0; i<4; i++)
126 if (fThresholds[i+k*4+j*16]!=fThresholdMin)
127 Out() << setw(3) << fThresholds[i+k*4+j*16] << " ";
128 else
129 Out() << " - ";
130 Out() << " ";
131 }
132 Out() << endl;
133 }
134 Out() << endl;
135 }
136
137 void Step(int idx, float step)
138 {
139 uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
140 if (diff<fThresholdMin)
141 diff=fThresholdMin;
142
143 if (diff==fThresholds[idx])
144 return;
145
146 if (fVerbose)
147 {
148 Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
149 Out() << (step>0 ? " += " : " -= ");
150 Out() << step << " (" << diff << ")" << endl;
151 }
152
153 const uint32_t val[2] = { idx, diff };
154 DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
155 }
156
157 void ProcessPatches(const FTM::DimTriggerRates &sdata)
158 {
159
160 // Caluclate Median and deviation
161 vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
162 vector<float> medp(sdata.fPatchRate, sdata.fPatchRate+160);
163
164 sort(medb.begin(), medb.end());
165 sort(medp.begin(), medp.end());
166
167 vector<float> devb(40);
168 for (int i=0; i<40; i++)
169 devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
170
171 vector<float> devp(160);
172 for (int i=0; i<160; i++)
173 devp[i] = fabs(sdata.fPatchRate[i]-medp[i]);
174
175 sort(devb.begin(), devb.end());
176 sort(devp.begin(), devp.end());
177
178 double mb = (medb[19]+medb[20])/2;
179 double mp = (medp[79]+medp[80])/2;
180
181 double db = devb[27];
182 double dp = devp[109];
183
184 // If any is zero there is something wrong
185 if (mb==0 || mp==0 || db==0 || dp==0)
186 return;
187
188 if (fVerbose)
189 {
190 Out() << "Patch: Median=" << mp << " Dev=" << dp << endl;
191 Out() << "Board: Median=" << mb << " Dev=" << db << endl;
192 }
193
194 for (int i=0; i<40; i++)
195 {
196 int maxi = -1;
197
198 const float dif = fabs(sdata.fBoardRate[i]-mb)/db;
199 if (dif>5)
200 {
201 if (fVerbose)
202 Out() << "B" << i << ": " << dif << endl;
203
204 float max = sdata.fPatchRate[i*4];
205 maxi = 0;
206
207 for (int j=1; j<4; j++)
208 if (sdata.fPatchRate[i*4+j]>max)
209 {
210 max = sdata.fPatchRate[i*4+j];
211 maxi = j;
212 }
213 }
214
215 for (int j=0; j<4; j++)
216 {
217 // For the noise pixel correct down to median+3*deviation
218 if (maxi==j)
219 {
220 // This is the step which has to be performed to go from
221 // a NSB rate of sdata.fPatchRate[i*4+j]
222
223
224 const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+5*dp))/0.039;
225 // * (dif-5)/dif
226 Step(i*4+j, step);
227 continue;
228 }
229
230 // For pixels below the meadian correct also back to median+3*deviation
231 if (sdata.fPatchRate[i*4+j]<mp)
232 {
233 const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3*dp))/0.039;
234 Step(i*4+j, step);
235 continue;
236 }
237
238 const float step = -1.5*(log10(mp+dp)-log10(mp))/0.039;
239 Step(i*4+j, step);
240 }
241 }
242 }
243
244 void ProcessCamera(const FTM::DimTriggerRates &sdata)
245 {
246 if (fCounter++==0)
247 return;
248
249 // Caluclate Median and deviation
250 vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
251
252 sort(medb.begin(), medb.end());
253
254 vector<float> devb(40);
255 for (int i=0; i<40; i++)
256 devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
257
258 sort(devb.begin(), devb.end());
259
260 double mb = (medb[19]+medb[20])/2;
261 double db = devb[27];
262
263 // If any is zero there is something wrong
264 if (mb==0 || db==0)
265 {
266 Warn("The median or the deviation of all board rates is zero... cannot calibrate.");
267 return;
268 }
269
270 double avg = 0;
271 int num = 0;
272
273 for (int i=0; i<40; i++)
274 {
275 if ( fabs(sdata.fBoardRate[i]-mb)<2.5*db)
276 {
277 avg += sdata.fBoardRate[i];
278 num++;
279 }
280 }
281
282 fTriggerRate = avg/num * 40;
283
284 if (fVerbose)
285 {
286 Out() << "Board: Median=" << mb << " Dev=" << db << endl;
287 Out() << "Camera: " << fTriggerRate << " (" << sdata.fTriggerRate << ", n=" << num << ")" << endl;
288 Out() << "Target: " << fTargetRate << endl;
289 }
290
291 if (sdata.fTriggerRate<fTriggerRate)
292 fTriggerRate = sdata.fTriggerRate;
293
294 // ----------------------
295
296 /*
297 if (avg>0 && avg<fTargetRate)
298 {
299 // I am assuming here (and at other places) the the answer from the FTM when setting
300 // the new threshold always arrives faster than the next rate update.
301 fThresholdMin = fThresholds[0];
302 Out() << "Setting fThresholdMin to " << fThresholds[0] << endl;
303 }
304 */
305
306 if (fTriggerRate>0 && fTriggerRate<fTargetRate)
307 {
308 fThresholds.assign(160, fThresholdMin);
309 return;
310 }
311
312 // This is a step towards a threshold at which the NSB rate is equal the target rate
313 // +1 to avoid getting a step of 0
314 const float step = (log10(fTriggerRate)-log10(fTargetRate))/0.039 + 1;
315
316 const uint16_t diff = fThresholdMin+int16_t(truncf(step));
317 if (diff<=fThresholdMin)
318 return;
319
320 if (fVerbose)
321 {
322 //Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
323 Out() << fThresholdMin;
324 Out() << (step>0 ? " += " : " -= ");
325 Out() << step << " (" << diff << ")" << endl;
326 }
327
328 const uint32_t val[2] = { -1, diff };
329 DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
330
331 fThresholdMin = diff;
332 }
333
334 void infoHandler()
335 {
336 DimInfo *curr = getInfo(); // get current DimInfo address
337 if (!curr)
338 return;
339
340 if (curr==&fFTM)
341 {
342 fStatusFTM = GetNewState(fFTM);
343 return;
344 }
345
346 if (curr==&fDim)
347 {
348 fStatusDim = GetNewState(fDim);
349 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
350 return;
351 }
352 if (curr==&fRateScan)
353 {
354 fStatusRS = GetNewState(fRateScan);
355 fStatusRS.second = curr->getSize()==4 ? curr->getInt() : 0;
356 return;
357 }
358
359 static vector<uint8_t> counter(160);
360
361 if (curr==&fStatic)
362 {
363 if (!CheckEventSize(curr->getSize(), "infoHandler[DimStaticData]", sizeof(FTM::DimStaticData)))
364 return;
365
366 const FTM::DimStaticData &sdata = *static_cast<FTM::DimStaticData*>(curr->getData());
367 fTriggerOn = sdata.HasTrigger();
368
369 PrintThresholds(sdata);
370
371 fThresholds.assign(sdata.fThreshold, sdata.fThreshold+160);
372 return;
373 }
374
375 if (curr==&fRates)
376 {
377 if (fThresholds.size()==0)
378 return;
379
380 if (!fTriggerOn && !fEnabled)
381 return;
382
383 if (fStatusRS.second==5)
384 return;
385
386 if (!CheckEventSize(curr->getSize(), "infoHandler[DimTriggerRates]", sizeof(FTM::DimTriggerRates)))
387 return;
388
389 const FTM::DimTriggerRates &sdata = *static_cast<FTM::DimTriggerRates*>(curr->getData());
390
391 if (GetCurrentState()==kStateSettingGlobalThreshold)
392 ProcessCamera(sdata);
393
394 if (GetCurrentState()==kStateInProgress)
395 ProcessPatches(sdata);
396 }
397 }
398
399 int Calibrate()
400 {
401 if (!fTriggerOn)
402 return kStateGlobalThresholdSet;
403
404 const int32_t val[2] = { -1, fThresholdReference };
405 Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", val);
406
407 fThresholds.assign(160, fThresholdReference);
408
409 fThresholdMin = fThresholdReference;
410 fTriggerRate = -1;
411 fEnabled = true;
412 fCounter = 0;
413
414 return kStateSettingGlobalThreshold;
415 }
416
417 int StartRC()
418 {
419 fEnabled = true;
420 return GetCurrentState();
421 }
422
423 int StopRC()
424 {
425 fEnabled = false;
426 return GetCurrentState();
427 }
428
429 int SetEnabled(const EventImp &evt)
430 {
431 if (!CheckEventSize(evt.GetSize(), "SetEnabled", 1))
432 return kSM_FatalError;
433
434 fEnabled = evt.GetBool();
435
436 return GetCurrentState();
437 }
438
439 int SetMinThreshold(const EventImp &evt)
440 {
441 if (!CheckEventSize(evt.GetSize(), "SetMinThreshold", 4))
442 return kSM_FatalError;
443
444 // FIXME: Check missing
445
446 fThresholdReference = evt.GetUShort();
447
448 return GetCurrentState();
449 }
450
451 int SetTargetRate(const EventImp &evt)
452 {
453 if (!CheckEventSize(evt.GetSize(), "SetTargetRate", 4))
454 return kSM_FatalError;
455
456 fTargetRate = evt.GetFloat();
457
458 return GetCurrentState();
459 }
460
461 void PrintState(const pair<Time,int> &state, const char *server)
462 {
463 const State rc = fNetwork.GetState(server, state.second);
464
465 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
466 Out() << kBold << server << ": ";
467 Out() << rc.name << "[" << rc.index << "]";
468 Out() << kReset << " - " << kBlue << rc.comment << endl;
469 }
470
471 int Print()
472 {
473 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
474 Out() << kBold << "DIM_DNS: ";
475 if (fStatusDim.second==0)
476 Out() << "Offline" << endl;
477 else
478 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
479
480 PrintState(fStatusFTM, "FTM_CONTROL");
481
482 return GetCurrentState();
483 }
484
485 int SetVerbosity(const EventImp &evt)
486 {
487 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
488 return kSM_FatalError;
489
490 fVerbose = evt.GetBool();
491
492 return GetCurrentState();
493 }
494
495 int Execute()
496 {
497 // Dispatch (execute) at most one handler from the queue. In contrary
498 // to run_one(), it doesn't wait until a handler is available
499 // which can be dispatched, so poll_one() might return with 0
500 // handlers dispatched. The handlers are always dispatched/executed
501 // synchronously, i.e. within the call to poll_one()
502 //poll_one();
503
504 if (fStatusDim.second==0)
505 return kStateDimNetworkNA;
506
507 // All subsystems are not connected
508 if (fStatusFTM.second<FTM::kConnected)
509 return kStateDisconnected;
510
511 if (GetCurrentState()==kStateSettingGlobalThreshold)
512 {
513 if (fTriggerRate<0 || fTriggerRate>fTargetRate)
514 return kStateSettingGlobalThreshold;
515
516 return kStateGlobalThresholdSet;
517 }
518
519 if (GetCurrentState()==kStateGlobalThresholdSet)
520 {
521 if (!fTriggerOn)
522 return kStateGlobalThresholdSet;
523 //return kStateInProgress;
524 }
525
526 // At least one subsystem is not connected
527 // if (fStatusFTM.second>=FTM::kConnected)
528 return fTriggerOn && fEnabled && fStatusRS.second!=5 ? kStateInProgress : kStateConnected;
529 }
530
531public:
532 StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
533 fStatusDim(make_pair(Time(), -2)),
534 fStatusFTM(make_pair(Time(), -2)),
535 fDim("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
536 fFTM("FTM_CONTROL/STATE", (void*)NULL, 0, this),
537 fRates("FTM_CONTROL/TRIGGER_RATES", (void*)NULL, 0, this),
538 fStatic("FTM_CONTROL/STATIC_DATA", (void*)NULL, 0, this)/*,
539 fDimData("RATE_SCAN/DATA", "I:1;F:1;F:1;F:1;F:40;F:160", ""),
540 fDimProc("RATE_SCAN/PROCESS_DATA", "I:1;I:1;I:1",
541 "Rate scan process data"
542 "|min[DAC]:Value at which scan was started"
543 "|max[DAC]:Value at which scan will end"
544 "|step[DAC]:Step size for scan")*/,
545 fTriggerOn(false)
546 {
547 // ba::io_service::work is a kind of keep_alive for the loop.
548 // It prevents the io_service to go to stopped state, which
549 // would prevent any consecutive calls to run()
550 // or poll() to do nothing. reset() could also revoke to the
551 // previous state but this might introduce some overhead of
552 // deletion and creation of threads and more.
553
554 // State names
555 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
556 "The Dim DNS is not reachable.");
557
558 AddStateName(kStateDisconnected, "Disconnected",
559 "The Dim DNS is reachable, but the required subsystems are not available.");
560
561 AddStateName(kStateConnected, "Connected",
562 "All needed subsystems are connected to their hardware, no action is performed.");
563
564 AddStateName(kStateSettingGlobalThreshold, "Calibrating", "");
565 AddStateName(kStateGlobalThresholdSet, "GlobalThresholdSet", "");
566
567 AddStateName(kStateInProgress, "InProgress",
568 "Rate scan in progress.");
569
570 AddEvent("CALIBRATE")
571 (bind(&StateMachineRateControl::Calibrate, this))
572 ("");
573
574 AddEvent("START", "")
575 (bind(&StateMachineRateControl::StartRC, this))
576 ("");
577
578 AddEvent("STOP", "")
579 (bind(&StateMachineRateControl::StopRC, this))
580 ("");
581
582 AddEvent("SET_MIN_THRESHOLD", "I:1")
583 (bind(&StateMachineRateControl::SetMinThreshold, this, placeholders::_1))
584 ("");
585
586 AddEvent("SET_TARGET_RATE", "F:1")
587 (bind(&StateMachineRateControl::SetTargetRate, this, placeholders::_1))
588 ("");
589
590 AddEvent("PRINT")
591 (bind(&StateMachineRateControl::Print, this))
592 ("");
593
594 AddEvent("SET_VERBOSE", "B")
595 (bind(&StateMachineRateControl::SetVerbosity, this, placeholders::_1))
596 ("set verbosity state"
597 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
598
599 }
600
601 int EvalOptions(Configuration &conf)
602 {
603 fVerbose = !conf.Get<bool>("quiet");
604
605 fThresholdReference = 300;
606 fTargetRate = 75;
607
608 return -1;
609 }
610};
611
612// ------------------------------------------------------------------------
613
614#include "Main.h"
615
616template<class T>
617int RunShell(Configuration &conf)
618{
619 return Main::execute<T, StateMachineRateControl>(conf);
620}
621
622void SetupConfiguration(Configuration &conf)
623{
624 po::options_description control("Rate scan options");
625 control.add_options()
626 ("quiet,q", po_bool(), "Disable printing more informations during rate control.")
627 //("max-wait", var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
628 // ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
629 ;
630
631 conf.AddOptions(control);
632}
633
634/*
635 Extract usage clause(s) [if any] for SYNOPSIS.
636 Translators: "Usage" and "or" here are patterns (regular expressions) which
637 are used to match the usage synopsis in program output. An example from cp
638 (GNU coreutils) which contains both strings:
639 Usage: cp [OPTION]... [-T] SOURCE DEST
640 or: cp [OPTION]... SOURCE... DIRECTORY
641 or: cp [OPTION]... -t DIRECTORY SOURCE...
642 */
643void PrintUsage()
644{
645 cout <<
646 "The ratescan program is a tool for automation of rate scans.\n"
647 "\n"
648 "Usage: ratescan [-c type] [OPTIONS]\n"
649 " or: ratescan [OPTIONS]\n";
650 cout << endl;
651}
652
653void PrintHelp()
654{
655 Main::PrintHelp<StateMachineRateControl>();
656
657 /* Additional help text which is printed after the configuration
658 options goes here */
659
660 /*
661 cout << "bla bla bla" << endl << endl;
662 cout << endl;
663 cout << "Environment:" << endl;
664 cout << "environment" << endl;
665 cout << endl;
666 cout << "Examples:" << endl;
667 cout << "test exam" << endl;
668 cout << endl;
669 cout << "Files:" << endl;
670 cout << "files" << endl;
671 cout << endl;
672 */
673}
674
675int main(int argc, const char* argv[])
676{
677 Configuration conf(argv[0]);
678 conf.SetPrintUsage(PrintUsage);
679 Main::SetupConfiguration(conf);
680 SetupConfiguration(conf);
681
682 if (!conf.DoParse(argc, argv, PrintHelp))
683 return -1;
684
685 //try
686 {
687 // No console access at all
688 if (!conf.Has("console"))
689 {
690// if (conf.Get<bool>("no-dim"))
691// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
692// else
693 return RunShell<LocalStream>(conf);
694 }
695 // Cosole access w/ and w/o Dim
696/* if (conf.Get<bool>("no-dim"))
697 {
698 if (conf.Get<int>("console")==0)
699 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
700 else
701 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
702 }
703 else
704*/ {
705 if (conf.Get<int>("console")==0)
706 return RunShell<LocalShell>(conf);
707 else
708 return RunShell<LocalConsole>(conf);
709 }
710 }
711 /*catch (std::exception& e)
712 {
713 cerr << "Exception: " << e.what() << endl;
714 return -1;
715 }*/
716
717 return 0;
718}
Note: See TracBrowser for help on using the repository browser.