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

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