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

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