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

Last change on this file since 13976 was 13956, checked in by tbretz, 13 years ago
(Hopefully) improved the state handling.
File size: 18.3 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 int maxi = -1;
170
171 const float dif = fabs(sdata.fBoardRate[i]-mb)/db;
172 if (dif>5)
173 {
174 if (fVerbose)
175 Out() << "B" << i << ": " << dif << endl;
176
177 float max = sdata.fPatchRate[i*4];
178 maxi = 0;
179
180 for (int j=1; j<4; j++)
181 if (sdata.fPatchRate[i*4+j]>max)
182 {
183 max = sdata.fPatchRate[i*4+j];
184 maxi = j;
185 }
186 }
187
188 if (fBlock[i])
189 {
190 fBlock[i] = false;
191 continue;
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 if (GetCurrentState()==RateControl::State::kSettingGlobalThreshold)
444 return RateControl::State::kSettingGlobalThreshold;
445
446 if (GetCurrentState()==RateControl::State::kGlobalThresholdSet)
447 return RateControl::State::kGlobalThresholdSet;
448
449 if (fEnabled && fTriggerOn && fDimRS.state()<RateScan::State::kConfiguring)
450 return RateControl::State::kInProgress;
451
452 return RateControl::State::kConnected;
453 }
454
455public:
456 StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
457 fTriggerOn(false), fBlock(40),
458 fDimFTM("FTM_CONTROL"),
459 fDimRS("RATE_SCAN"),
460 fDimThreshold("THRESHOLD", "S:1", "Resulting threshold after calibration|threshold[dac]:Resulting threshold from calibration")
461 {
462 // ba::io_service::work is a kind of keep_alive for the loop.
463 // It prevents the io_service to go to stopped state, which
464 // would prevent any consecutive calls to run()
465 // or poll() to do nothing. reset() could also revoke to the
466 // previous state but this might introduce some overhead of
467 // deletion and creation of threads and more.
468
469 fDim.Subscribe(*this);
470 fDimFTM.Subscribe(*this);
471 fDimRS.Subscribe(*this);
472
473 Subscribe("FTM_CONTROL/TRIGGER_RATES")
474 (bind(&StateMachineRateControl::HandleTriggerRates, this, placeholders::_1));
475 Subscribe("FTM_CONTROL/STATIC_DATA")
476 (bind(&StateMachineRateControl::HandleStaticData, this, placeholders::_1));
477
478 // State names
479 AddStateName(RateControl::State::kDimNetworkNA, "DimNetworkNotAvailable",
480 "The Dim DNS is not reachable.");
481
482 AddStateName(RateControl::State::kDisconnected, "Disconnected",
483 "The Dim DNS is reachable, but the required subsystems are not available.");
484
485 AddStateName(RateControl::State::kConnected, "Connected",
486 "All needed subsystems are connected to their hardware, no action is performed.");
487
488 AddStateName(RateControl::State::kSettingGlobalThreshold, "Calibrating", "");
489 AddStateName(RateControl::State::kGlobalThresholdSet, "GlobalThresholdSet", "");
490
491 AddStateName(RateControl::State::kInProgress, "InProgress",
492 "Rate scan in progress.");
493
494 AddEvent("CALIBRATE")
495 (bind(&StateMachineRateControl::Calibrate, this))
496 ("");
497
498 AddEvent("START", "", RateControl::State::kConnected, RateControl::State::kGlobalThresholdSet)
499 (bind(&StateMachineRateControl::StartRC, this))
500 ("");
501
502 AddEvent("STOP", "", RateControl::State::kSettingGlobalThreshold, RateControl::State::kInProgress)
503 (bind(&StateMachineRateControl::StopRC, this))
504 ("");
505
506 AddEvent("SET_MIN_THRESHOLD", "I:1")
507 (bind(&StateMachineRateControl::SetMinThreshold, this, placeholders::_1))
508 ("");
509
510 AddEvent("SET_TARGET_RATE", "F:1")
511 (bind(&StateMachineRateControl::SetTargetRate, this, placeholders::_1))
512 ("");
513
514 AddEvent("PRINT")
515 (bind(&StateMachineRateControl::Print, this))
516 ("");
517
518 AddEvent("SET_VERBOSE", "B")
519 (bind(&StateMachineRateControl::SetVerbosity, this, placeholders::_1))
520 ("set verbosity state"
521 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
522
523 }
524
525 int EvalOptions(Configuration &conf)
526 {
527 fVerbose = !conf.Get<bool>("quiet");
528
529 fThresholdReference = 300;
530 fTargetRate = 75;
531
532 return -1;
533 }
534};
535
536// ------------------------------------------------------------------------
537
538#include "Main.h"
539
540template<class T>
541int RunShell(Configuration &conf)
542{
543 return Main::execute<T, StateMachineRateControl>(conf);
544}
545
546void SetupConfiguration(Configuration &conf)
547{
548 po::options_description control("Rate control options");
549 control.add_options()
550 ("quiet,q", po_bool(), "Disable printing more informations during rate control.")
551 //("max-wait", var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
552 // ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
553 ;
554
555 conf.AddOptions(control);
556}
557
558/*
559 Extract usage clause(s) [if any] for SYNOPSIS.
560 Translators: "Usage" and "or" here are patterns (regular expressions) which
561 are used to match the usage synopsis in program output. An example from cp
562 (GNU coreutils) which contains both strings:
563 Usage: cp [OPTION]... [-T] SOURCE DEST
564 or: cp [OPTION]... SOURCE... DIRECTORY
565 or: cp [OPTION]... -t DIRECTORY SOURCE...
566 */
567void PrintUsage()
568{
569 cout <<
570 "The ratecontrol program is a keep the rate reasonable low.\n"
571 "\n"
572 "Usage: ratecontrol [-c type] [OPTIONS]\n"
573 " or: ratecontrol [OPTIONS]\n";
574 cout << endl;
575}
576
577void PrintHelp()
578{
579 Main::PrintHelp<StateMachineRateControl>();
580
581 /* Additional help text which is printed after the configuration
582 options goes here */
583
584 /*
585 cout << "bla bla bla" << endl << endl;
586 cout << endl;
587 cout << "Environment:" << endl;
588 cout << "environment" << endl;
589 cout << endl;
590 cout << "Examples:" << endl;
591 cout << "test exam" << endl;
592 cout << endl;
593 cout << "Files:" << endl;
594 cout << "files" << endl;
595 cout << endl;
596 */
597}
598
599int main(int argc, const char* argv[])
600{
601 Configuration conf(argv[0]);
602 conf.SetPrintUsage(PrintUsage);
603 Main::SetupConfiguration(conf);
604 SetupConfiguration(conf);
605
606 if (!conf.DoParse(argc, argv, PrintHelp))
607 return -1;
608
609 //try
610 {
611 // No console access at all
612 if (!conf.Has("console"))
613 {
614// if (conf.Get<bool>("no-dim"))
615// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
616// else
617 return RunShell<LocalStream>(conf);
618 }
619 // Cosole access w/ and w/o Dim
620/* if (conf.Get<bool>("no-dim"))
621 {
622 if (conf.Get<int>("console")==0)
623 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
624 else
625 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
626 }
627 else
628*/ {
629 if (conf.Get<int>("console")==0)
630 return RunShell<LocalShell>(conf);
631 else
632 return RunShell<LocalConsole>(conf);
633 }
634 }
635 /*catch (std::exception& e)
636 {
637 cerr << "Exception: " << e.what() << endl;
638 return -1;
639 }*/
640
641 return 0;
642}
Note: See TracBrowser for help on using the repository browser.