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

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