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

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