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

Last change on this file since 13452 was 13242, checked in by tbretz, 13 years ago
Updated some log-output.
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 control 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 ratecontrol program is a keep the rate reasonable low.\n"
647 "\n"
648 "Usage: ratecontrol [-c type] [OPTIONS]\n"
649 " or: ratecontrol [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.