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

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