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

Last change on this file since 14905 was 14681, checked in by tbretz, 13 years ago
Allow STOP also in GlobalThresholdSet
File size: 17.8 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
11#include "tools.h"
12
13#include "LocalControl.h"
14
15#include "HeadersFTM.h"
16#include "HeadersRateScan.h"
17#include "HeadersRateControl.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 bool fTriggerOn;
36
37 vector<bool> fBlock;
38
39 DimVersion fDim;
40 DimDescribedState fDimFTM;
41 DimDescribedState fDimRS;
42
43 DimDescribedService fDimThreshold;
44
45 float fTargetRate;
46 float fTriggerRate;
47
48 uint16_t fThresholdMin;
49 uint16_t fThresholdReference;
50
51 bool fVerbose;
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() << fabs(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 if (fBlock[i])
167 {
168 fBlock[i] = false;
169 continue;
170 }
171
172 int maxi = -1;
173
174 const float dif = fabs(sdata.fBoardRate[i]-mb)/db;
175 if (dif>5)
176 {
177 if (fVerbose)
178 Out() << "B" << i << ": " << dif << endl;
179
180 float max = sdata.fPatchRate[i*4];
181 maxi = 0;
182
183 for (int j=1; j<4; j++)
184 if (sdata.fPatchRate[i*4+j]>max)
185 {
186 max = sdata.fPatchRate[i*4+j];
187 maxi = j;
188 }
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 int ProcessCamera(const FTM::DimTriggerRates &sdata)
221 {
222 if (fCounter++==0)
223 return GetCurrentState();
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 GetCurrentState();
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 fDimThreshold.Update(fThresholdMin);
286 return RateControl::State::kGlobalThresholdSet;
287 }
288
289 // This is a step towards a threshold at which the NSB rate is equal the target rate
290 // +1 to avoid getting a step of 0
291 const float step = (log10(fTriggerRate)-log10(fTargetRate))/0.039 + 1;
292
293 const uint16_t diff = fThresholdMin+int16_t(truncf(step));
294 if (diff<=fThresholdMin)
295 {
296 fDimThreshold.Update(fThresholdMin);
297 return RateControl::State::kGlobalThresholdSet;
298 }
299
300 if (fVerbose)
301 {
302 //Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
303 Out() << fThresholdMin;
304 Out() << (step>0 ? " += " : " -= ");
305 Out() << step << " (" << diff << ")" << endl;
306 }
307
308 const uint32_t val[2] = { -1, diff };
309 DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
310
311 fThresholdMin = diff;
312
313 return GetCurrentState();
314 }
315
316 int HandleStaticData(const EventImp &evt)
317 {
318 if (!CheckEventSize(evt, sizeof(FTM::DimStaticData)))
319 return GetCurrentState();
320
321 const FTM::DimStaticData &sdata = *static_cast<const FTM::DimStaticData*>(evt.GetData());
322 fTriggerOn = sdata.HasTrigger();
323
324 PrintThresholds(sdata);
325
326 fThresholds.assign(sdata.fThreshold, sdata.fThreshold+160);
327
328 return GetCurrentState();
329 }
330
331 int HandleTriggerRates(const EventImp &evt)
332 {
333 if (fThresholds.size()==0)
334 return GetCurrentState();
335
336 if (GetCurrentState()<=RateControl::State::kConnected ||
337 GetCurrentState()==RateControl::State::kGlobalThresholdSet)
338 return GetCurrentState();
339
340 if (!CheckEventSize(evt, sizeof(FTM::DimTriggerRates)))
341 return GetCurrentState();
342
343 const FTM::DimTriggerRates &sdata = *static_cast<const FTM::DimTriggerRates*>(evt.GetData());
344
345 if (GetCurrentState()==RateControl::State::kSettingGlobalThreshold)
346 return ProcessCamera(sdata);
347
348 if (GetCurrentState()==RateControl::State::kInProgress)
349 ProcessPatches(sdata);
350
351 return GetCurrentState();
352 }
353
354 int Calibrate()
355 {
356 if (!fTriggerOn)
357 {
358 Info("Trigger not switched on... CALIBRATE command ignored.");
359 return RateControl::State::kGlobalThresholdSet;
360 }
361
362 const int32_t val[2] = { -1, fThresholdReference };
363 Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", val);
364
365 fThresholds.assign(160, fThresholdReference);
366
367 fThresholdMin = fThresholdReference;
368 fTriggerRate = -1;
369 fCounter = 0;
370
371 ostringstream out;
372 out << "Rate calibration started at a threshold of " << fThresholdReference << " with a target rate of " << fTargetRate << " Hz";
373 Info(out);
374
375 return RateControl::State::kSettingGlobalThreshold;
376 }
377
378 int StopRC()
379 {
380 return RateControl::State::kConnected;
381 }
382
383 int SetMinThreshold(const EventImp &evt)
384 {
385 if (!CheckEventSize(evt, 4))
386 return kSM_FatalError;
387
388 // FIXME: Check missing
389
390 fThresholdReference = evt.GetUShort();
391
392 return GetCurrentState();
393 }
394
395 int SetTargetRate(const EventImp &evt)
396 {
397 if (!CheckEventSize(evt, 4))
398 return kSM_FatalError;
399
400 fTargetRate = evt.GetFloat();
401
402 return GetCurrentState();
403 }
404
405 int Print() const
406 {
407 Out() << fDim << endl;
408 Out() << fDimFTM << endl;
409 Out() << fDimRS << endl;
410
411 return GetCurrentState();
412 }
413
414 int SetVerbosity(const EventImp &evt)
415 {
416 if (!CheckEventSize(evt, 1))
417 return kSM_FatalError;
418
419 fVerbose = evt.GetBool();
420
421 return GetCurrentState();
422 }
423
424 int Execute()
425 {
426 if (!fDim.online())
427 return RateControl::State::kDimNetworkNA;
428
429 // All subsystems are not connected
430 if (fDimFTM.state()<FTM::State::kConnected)
431 return RateControl::State::kDisconnected;
432
433 const bool inprog = fTriggerOn && fDimRS.state()<RateScan::State::kConfiguring;
434
435 switch (GetCurrentState())
436 {
437 case RateControl::State::kSettingGlobalThreshold:
438 return RateControl::State::kSettingGlobalThreshold;
439
440 case RateControl::State::kGlobalThresholdSet:
441 if (!inprog)
442 return RateControl::State::kGlobalThresholdSet;
443
444 case RateControl::State::kInProgress:
445 if (inprog)
446 return RateControl::State::kInProgress;
447 }
448
449 return RateControl::State::kConnected;
450 }
451
452public:
453 StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
454 fTriggerOn(false), fBlock(40),
455 fDimFTM("FTM_CONTROL"),
456 fDimRS("RATE_SCAN"),
457 fDimThreshold("RATE_CONTROL/THRESHOLD", "S:1", "Resulting threshold after calibration|threshold[dac]:Resulting threshold from calibration")
458 {
459 // ba::io_service::work is a kind of keep_alive for the loop.
460 // It prevents the io_service to go to stopped state, which
461 // would prevent any consecutive calls to run()
462 // or poll() to do nothing. reset() could also revoke to the
463 // previous state but this might introduce some overhead of
464 // deletion and creation of threads and more.
465
466 fDim.Subscribe(*this);
467 fDimFTM.Subscribe(*this);
468 fDimRS.Subscribe(*this);
469
470 Subscribe("FTM_CONTROL/TRIGGER_RATES")
471 (bind(&StateMachineRateControl::HandleTriggerRates, this, placeholders::_1));
472 Subscribe("FTM_CONTROL/STATIC_DATA")
473 (bind(&StateMachineRateControl::HandleStaticData, this, placeholders::_1));
474
475 // State names
476 AddStateName(RateControl::State::kDimNetworkNA, "DimNetworkNotAvailable",
477 "The Dim DNS is not reachable.");
478
479 AddStateName(RateControl::State::kDisconnected, "Disconnected",
480 "The Dim DNS is reachable, but the required subsystems are not available.");
481
482 AddStateName(RateControl::State::kConnected, "Connected",
483 "All needed subsystems are connected to their hardware, no action is performed.");
484
485 AddStateName(RateControl::State::kSettingGlobalThreshold, "Calibrating",
486 "A global minimum thrshold is currently determined.");
487
488 AddStateName(RateControl::State::kGlobalThresholdSet, "GlobalThresholdSet",
489 "A global threshold has ben set, waiting for the trigger to be switched on.");
490
491 AddStateName(RateControl::State::kInProgress, "InProgress",
492 "Rate control in progress.");
493
494 AddEvent("CALIBRATE")
495 (bind(&StateMachineRateControl::Calibrate, this))
496 ("Start a search for a reasonable minimum global threshold");
497
498 AddEvent("STOP", RateControl::State::kSettingGlobalThreshold, RateControl::State::kGlobalThresholdSet, RateControl::State::kInProgress)
499 (bind(&StateMachineRateControl::StopRC, this))
500 ("Stop a calibration or ratescan in progress");
501
502 AddEvent("SET_MIN_THRESHOLD", "I:1")
503 (bind(&StateMachineRateControl::SetMinThreshold, this, placeholders::_1))
504 ("Set a minimum threshold at which th rate control starts calibrating");
505
506 AddEvent("SET_TARGET_RATE", "F:1")
507 (bind(&StateMachineRateControl::SetTargetRate, this, placeholders::_1))
508 ("Set a target trigger rate for the calibration");
509
510 AddEvent("PRINT")
511 (bind(&StateMachineRateControl::Print, this))
512 ("Print current status");
513
514 AddEvent("SET_VERBOSE", "B")
515 (bind(&StateMachineRateControl::SetVerbosity, this, placeholders::_1))
516 ("set verbosity state"
517 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
518
519 }
520
521 int EvalOptions(Configuration &conf)
522 {
523 fVerbose = !conf.Get<bool>("quiet");
524
525 fThresholdReference = 300;
526 fThresholdMin = 300;
527 fTargetRate = 75;
528
529 return -1;
530 }
531};
532
533// ------------------------------------------------------------------------
534
535#include "Main.h"
536
537template<class T>
538int RunShell(Configuration &conf)
539{
540 return Main::execute<T, StateMachineRateControl>(conf);
541}
542
543void SetupConfiguration(Configuration &conf)
544{
545 po::options_description control("Rate control options");
546 control.add_options()
547 ("quiet,q", po_bool(), "Disable printing more informations during rate control.")
548 //("max-wait", var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
549 // ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
550 ;
551
552 conf.AddOptions(control);
553}
554
555/*
556 Extract usage clause(s) [if any] for SYNOPSIS.
557 Translators: "Usage" and "or" here are patterns (regular expressions) which
558 are used to match the usage synopsis in program output. An example from cp
559 (GNU coreutils) which contains both strings:
560 Usage: cp [OPTION]... [-T] SOURCE DEST
561 or: cp [OPTION]... SOURCE... DIRECTORY
562 or: cp [OPTION]... -t DIRECTORY SOURCE...
563 */
564void PrintUsage()
565{
566 cout <<
567 "The ratecontrol program is a keep the rate reasonable low.\n"
568 "\n"
569 "Usage: ratecontrol [-c type] [OPTIONS]\n"
570 " or: ratecontrol [OPTIONS]\n";
571 cout << endl;
572}
573
574void PrintHelp()
575{
576 Main::PrintHelp<StateMachineRateControl>();
577
578 /* Additional help text which is printed after the configuration
579 options goes here */
580
581 /*
582 cout << "bla bla bla" << endl << endl;
583 cout << endl;
584 cout << "Environment:" << endl;
585 cout << "environment" << endl;
586 cout << endl;
587 cout << "Examples:" << endl;
588 cout << "test exam" << endl;
589 cout << endl;
590 cout << "Files:" << endl;
591 cout << "files" << endl;
592 cout << endl;
593 */
594}
595
596int main(int argc, const char* argv[])
597{
598 Configuration conf(argv[0]);
599 conf.SetPrintUsage(PrintUsage);
600 Main::SetupConfiguration(conf);
601 SetupConfiguration(conf);
602
603 if (!conf.DoParse(argc, argv, PrintHelp))
604 return 127;
605
606 if (!conf.Has("console"))
607 return RunShell<LocalStream>(conf);
608
609 if (conf.Get<int>("console")==0)
610 return RunShell<LocalShell>(conf);
611 else
612 return RunShell<LocalConsole>(conf);
613
614 return 0;
615}
Note: See TracBrowser for help on using the repository browser.