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

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