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

Last change on this file since 15030 was 15029, checked in by tbretz, 13 years ago
Some fixes to get the new calibrate_by_currents working.
File size: 22.4 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 "HeadersDrive.h"
17#include "HeadersRateScan.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 DimDescribedState fDimDrive;
44
45 DimDescribedService fDimThreshold;
46
47 float fTargetRate;
48 float fTriggerRate;
49
50 uint16_t fThresholdMin;
51 uint16_t fThresholdReference;
52
53 deque<pair<Time,float>> fCurrents;
54
55 bool fVerbose;
56 bool fCalibrateByCurrent;
57
58 uint64_t fCounter;
59
60 Time fCalibrationTimeStart;
61
62 bool CheckEventSize(const EventImp &evt, size_t size)
63 {
64 if (size_t(evt.GetSize())==size)
65 return true;
66
67 if (evt.GetSize()==0)
68 return false;
69
70 ostringstream msg;
71 msg << evt.GetName() << " - Received event has " << evt.GetSize() << " bytes, but expected " << size << ".";
72 Fatal(msg);
73 return false;
74 }
75
76 vector<uint16_t> fThresholds;
77
78 void PrintThresholds(const FTM::DimStaticData &sdata)
79 {
80 //if (!fVerbose)
81 // return;
82
83 if (fThresholds.size()==0)
84 return;
85
86 Out() << "Min. DAC=" << fThresholdMin << endl;
87
88 int t=0;
89 for (t=0; t<160; t++)
90 if (sdata.fThreshold[t]!=fThresholds[t])
91 break;
92
93 if (t==160)
94 return;
95
96 for (int j=0; j<10; j++)
97 {
98 for (int k=0; k<4; k++)
99 {
100 for (int i=0; i<4; i++)
101 if (fThresholds[i+k*4+j*16]!=fThresholdMin)
102 Out() << setw(3) << fThresholds[i+k*4+j*16] << " ";
103 else
104 Out() << " - ";
105 Out() << " ";
106 }
107 Out() << endl;
108 }
109 Out() << endl;
110 }
111
112 void Step(int idx, float step)
113 {
114 uint16_t diff = fThresholds[idx]+int16_t(truncf(step));
115 if (diff<fThresholdMin)
116 diff=fThresholdMin;
117
118 if (diff==fThresholds[idx])
119 return;
120
121 if (fVerbose)
122 {
123 Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
124 Out() << (step>0 ? " += " : " -= ");
125 Out() << fabs(step) << " (" << diff << ")" << endl;
126 }
127
128 const uint32_t val[2] = { idx, diff };
129 DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
130
131 fBlock[idx/4] = true;
132 }
133
134 void ProcessPatches(const FTM::DimTriggerRates &sdata)
135 {
136
137 // Caluclate Median and deviation
138 vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
139 vector<float> medp(sdata.fPatchRate, sdata.fPatchRate+160);
140
141 sort(medb.begin(), medb.end());
142 sort(medp.begin(), medp.end());
143
144 vector<float> devb(40);
145 for (int i=0; i<40; i++)
146 devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
147
148 vector<float> devp(160);
149 for (int i=0; i<160; i++)
150 devp[i] = fabs(sdata.fPatchRate[i]-medp[i]);
151
152 sort(devb.begin(), devb.end());
153 sort(devp.begin(), devp.end());
154
155 double mb = (medb[19]+medb[20])/2;
156 double mp = (medp[79]+medp[80])/2;
157
158 double db = devb[27];
159 double dp = devp[109];
160
161 // If any is zero there is something wrong
162 if (mb==0 || mp==0 || db==0 || dp==0)
163 return;
164
165 if (fVerbose)
166 {
167 Out() << "Patch: Median=" << mp << " Dev=" << dp << endl;
168 Out() << "Board: Median=" << mb << " Dev=" << db << endl;
169 }
170
171 for (int i=0; i<40; i++)
172 {
173 if (fBlock[i])
174 {
175 fBlock[i] = false;
176 continue;
177 }
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 for (int j=0; j<4; j++)
199 {
200 // For the noise pixel correct down to median+3*deviation
201 if (maxi==j)
202 {
203 // This is the step which has to be performed to go from
204 // a NSB rate of sdata.fPatchRate[i*4+j]
205
206
207 const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+5*dp))/0.039;
208 // * (dif-5)/dif
209 Step(i*4+j, step);
210 continue;
211 }
212
213 // For pixels below the meadian correct also back to median+3*deviation
214 if (sdata.fPatchRate[i*4+j]<mp)
215 {
216 const float step = (log10(sdata.fPatchRate[i*4+j])-log10(mp+3*dp))/0.039;
217 Step(i*4+j, step);
218 continue;
219 }
220
221 const float step = -1.5*(log10(mp+dp)-log10(mp))/0.039;
222 Step(i*4+j, step);
223 }
224 }
225 }
226
227 int ProcessCamera(const FTM::DimTriggerRates &sdata)
228 {
229 if (fCounter++==0)
230 return GetCurrentState();
231
232 // Caluclate Median and deviation
233 vector<float> medb(sdata.fBoardRate, sdata.fBoardRate+40);
234
235 sort(medb.begin(), medb.end());
236
237 vector<float> devb(40);
238 for (int i=0; i<40; i++)
239 devb[i] = fabs(sdata.fBoardRate[i]-medb[i]);
240
241 sort(devb.begin(), devb.end());
242
243 double mb = (medb[19]+medb[20])/2;
244 double db = devb[27];
245
246 // If any is zero there is something wrong
247 if (mb==0 || db==0)
248 {
249 Warn("The median or the deviation of all board rates is zero... cannot calibrate.");
250 return GetCurrentState();
251 }
252
253 double avg = 0;
254 int num = 0;
255
256 for (int i=0; i<40; i++)
257 {
258 if ( fabs(sdata.fBoardRate[i]-mb)<2.5*db)
259 {
260 avg += sdata.fBoardRate[i];
261 num++;
262 }
263 }
264
265 fTriggerRate = avg/num * 40;
266
267 if (fVerbose)
268 {
269 Out() << "Board: Median=" << mb << " Dev=" << db << endl;
270 Out() << "Camera: " << fTriggerRate << " (" << sdata.fTriggerRate << ", n=" << num << ")" << endl;
271 Out() << "Target: " << fTargetRate << endl;
272 }
273
274 if (sdata.fTriggerRate<fTriggerRate)
275 fTriggerRate = sdata.fTriggerRate;
276
277 // ----------------------
278
279 /*
280 if (avg>0 && avg<fTargetRate)
281 {
282 // I am assuming here (and at other places) the the answer from the FTM when setting
283 // the new threshold always arrives faster than the next rate update.
284 fThresholdMin = fThresholds[0];
285 Out() << "Setting fThresholdMin to " << fThresholds[0] << endl;
286 }
287 */
288
289 if (fTriggerRate>0 && fTriggerRate<fTargetRate)
290 {
291 fThresholds.assign(160, fThresholdMin);
292
293 const RateControl::DimThreshold data = { fThresholdMin, fCalibrationTimeStart.Mjd(), Time().Mjd() };
294 fDimThreshold.setQuality(0);
295 fDimThreshold.Update(data);
296
297 ostringstream out;
298 out << setprecision(3);
299 out << "Measured rate " << fTriggerRate << "Hz below target rate " << fTargetRate << "... mininum threshold set to " << fThresholdMin;
300 Info(out);
301
302 return RateControl::State::kGlobalThresholdSet;
303 }
304
305 // This is a step towards a threshold at which the NSB rate is equal the target rate
306 // +1 to avoid getting a step of 0
307 const float step = (log10(fTriggerRate)-log10(fTargetRate))/0.039 + 1;
308
309 const uint16_t diff = fThresholdMin+int16_t(truncf(step));
310 if (diff<=fThresholdMin)
311 {
312 const RateControl::DimThreshold data = { fThresholdMin, fCalibrationTimeStart.Mjd(), Time().Mjd() };
313 fDimThreshold.setQuality(1);
314 fDimThreshold.Update(data);
315
316 ostringstream out;
317 out << setprecision(3);
318 out << "Next step would be 0... mininum threshold set to " << fThresholdMin;
319 Info(out);
320
321 return RateControl::State::kGlobalThresholdSet;
322 }
323
324 if (fVerbose)
325 {
326 //Out() << idx/40 << "|" << (idx/4)%10 << "|" << idx%4;
327 Out() << fThresholdMin;
328 Out() << (step>0 ? " += " : " -= ");
329 Out() << step << " (" << diff << ")" << endl;
330 }
331
332 const uint32_t val[2] = { -1, diff };
333 DimClient::sendCommandNB("FTM_CONTROL/SET_THRESHOLD", (void*)val, 8);
334
335 fThresholdMin = diff;
336
337 return GetCurrentState();
338 }
339
340 int HandleStaticData(const EventImp &evt)
341 {
342 if (!CheckEventSize(evt, sizeof(FTM::DimStaticData)))
343 return GetCurrentState();
344
345 const FTM::DimStaticData &sdata = *static_cast<const FTM::DimStaticData*>(evt.GetData());
346 fTriggerOn = sdata.HasTrigger();
347
348 PrintThresholds(sdata);
349
350 fThresholds.assign(sdata.fThreshold, sdata.fThreshold+160);
351
352 return GetCurrentState();
353 }
354
355 int HandleTriggerRates(const EventImp &evt)
356 {
357 if (fThresholds.size()==0)
358 return GetCurrentState();
359
360 if (GetCurrentState()<=RateControl::State::kConnected ||
361 GetCurrentState()==RateControl::State::kGlobalThresholdSet)
362 return GetCurrentState();
363
364 if (!CheckEventSize(evt, sizeof(FTM::DimTriggerRates)))
365 return GetCurrentState();
366
367 const FTM::DimTriggerRates &sdata = *static_cast<const FTM::DimTriggerRates*>(evt.GetData());
368
369 if (GetCurrentState()==RateControl::State::kSettingGlobalThreshold && !fCalibrateByCurrent)
370 return ProcessCamera(sdata);
371
372 if (GetCurrentState()==RateControl::State::kInProgress)
373 ProcessPatches(sdata);
374
375 return GetCurrentState();
376 }
377
378 int HandleCalibratedCurrents(const EventImp &evt)
379 {
380 // Check if received event is valid
381 if (!CheckEventSize(evt, (416+6)*4))
382 return GetCurrentState();
383
384 // Record only currents when the drive is tracking to avoid
385 // bias from the movement
386 if (fDimDrive.state()<Drive::State::kTracking)
387 return GetCurrentState();
388
389 // Get time and median current (FIXME: check N?)
390 const Time &time = evt.GetTime();
391 const float med = evt.Get<float>(416*4+4+4);
392
393 // Keep all median currents of the past 10 seconds
394 fCurrents.push_back(make_pair(time, med));
395 while (!fCurrents.empty())
396 {
397 if (time-fCurrents.front().first<boost::posix_time::seconds(10))
398 break;
399
400 fCurrents.pop_front();
401 }
402
403 // If we are not doing a calibration no further action necessary
404 if (!fCalibrateByCurrent)
405 return GetCurrentState();
406
407 if (GetCurrentState()!=RateControl::State::kSettingGlobalThreshold)
408 return GetCurrentState();
409
410 // We want at least 8 values for averaging
411 if (fCurrents.size()<8)
412 return GetCurrentState();
413
414 // Calculate avera and rms of median
415 double avg = 0;
416 double rms = 0;
417 for (auto it=fCurrents.begin(); it!=fCurrents.end(); it++)
418 {
419 avg += it->second;
420 rms += it->second*it->second;
421 }
422 avg /= fCurrents.size();
423 rms /= fCurrents.size();
424
425 rms = sqrt(rms-avg*avg);
426
427 fThresholdMin = 36.0833*pow(avg, 0.638393)+184.037;
428 fThresholdReference = fThresholdMin;
429 fThresholds.assign(160, fThresholdMin);
430
431 const RateControl::DimThreshold data = { fThresholdMin, fCalibrationTimeStart.Mjd(), Time().Mjd() };
432 fDimThreshold.setQuality(0);
433 fDimThreshold.Update(data);
434
435 ostringstream out;
436 out << setprecision(3);
437 out << "Measured average current " << avg << "uA +- " << rms << "uA [N=" << fCurrents.size() << "]... mininum threshold set to " << fThresholdMin;
438 Info(out);
439
440 return RateControl::State::kGlobalThresholdSet;
441 }
442
443 int Calibrate()
444 {
445 if (!fTriggerOn)
446 {
447 Info("Physics trigger not enabled... CALIBRATE command ignored.");
448 return RateControl::State::kGlobalThresholdSet;
449 }
450
451 const int32_t val[2] = { -1, fThresholdReference };
452 Dim::SendCommand("FTM_CONTROL/SET_THRESHOLD", val);
453
454 fThresholds.assign(160, fThresholdReference);
455
456 fThresholdMin = fThresholdReference;
457 fTriggerRate = -1;
458 fCounter = 0;
459
460 fCalibrateByCurrent = false;
461 fCalibrationTimeStart = Time();
462
463 ostringstream out;
464 out << "Rate calibration started at a threshold of " << fThresholdReference << " with a target rate of " << fTargetRate << " Hz";
465 Info(out);
466
467 return RateControl::State::kSettingGlobalThreshold;
468 }
469
470 int CalibrateByCurrent()
471 {
472 if (!fTriggerOn)
473 {
474 Info("Physics trigger not enabled... CALIBRATE command ignored.");
475 return RateControl::State::kGlobalThresholdSet;
476 }
477
478 cout << "-1-" << endl;
479
480 if (fDimDrive.state()<Drive::State::kMoving)
481 Warn("Drive not even moving...");
482
483 cout << "-2-" << endl;
484
485 fCounter = 0;
486 fCalibrateByCurrent = true;
487 fCalibrationTimeStart = Time();
488
489 cout << "-3-" << endl;
490
491 ostringstream out;
492 out << "Rate calibration by current " << fThresholdReference << " with a target rate of " << fTargetRate << " Hz";
493 Info(out);
494
495 cout << "START" << endl;
496
497 return RateControl::State::kSettingGlobalThreshold;
498 }
499
500 int StopRC()
501 {
502 return RateControl::State::kConnected;
503 }
504
505 int SetMinThreshold(const EventImp &evt)
506 {
507 if (!CheckEventSize(evt, 4))
508 return kSM_FatalError;
509
510 // FIXME: Check missing
511
512 fThresholdReference = evt.GetUShort();
513
514 return GetCurrentState();
515 }
516
517 int SetTargetRate(const EventImp &evt)
518 {
519 if (!CheckEventSize(evt, 4))
520 return kSM_FatalError;
521
522 fTargetRate = evt.GetFloat();
523
524 return GetCurrentState();
525 }
526
527 int Print() const
528 {
529 cout << "PRINT" << endl;
530 Out() << fDim << endl;
531 Out() << fDimFTM << endl;
532 Out() << fDimRS << endl;
533 Out() << fDimDrive << endl;
534
535 return GetCurrentState();
536 }
537
538 int SetVerbosity(const EventImp &evt)
539 {
540 if (!CheckEventSize(evt, 1))
541 return kSM_FatalError;
542
543 fVerbose = evt.GetBool();
544
545 return GetCurrentState();
546 }
547
548 int Execute()
549 {
550 if (!fDim.online())
551 return RateControl::State::kDimNetworkNA;
552
553 // All subsystems are not connected
554 if (fDimFTM.state()<FTM::State::kConnected || fDimDrive.state()<Drive::State::kConnected)
555 return RateControl::State::kDisconnected;
556
557 const bool inprog = fTriggerOn && fDimRS.state()<RateScan::State::kConfiguring;
558
559 switch (GetCurrentState())
560 {
561 case RateControl::State::kSettingGlobalThreshold:
562 return RateControl::State::kSettingGlobalThreshold;
563
564 case RateControl::State::kGlobalThresholdSet:
565 if (!inprog)
566 return RateControl::State::kGlobalThresholdSet;
567
568 case RateControl::State::kInProgress:
569 if (inprog)
570 return RateControl::State::kInProgress;
571 }
572
573 return RateControl::State::kConnected;
574 }
575
576public:
577 StateMachineRateControl(ostream &out=cout) : StateMachineDim(out, "RATE_CONTROL"),
578 fTriggerOn(false), fBlock(40),
579 fDimFTM("FTM_CONTROL"),
580 fDimRS("RATE_SCAN"),
581 fDimDrive("DRIVE_CONTROL"),
582 fDimThreshold("RATE_CONTROL/THRESHOLD", "S:1;D:1;D:1",
583 "Resulting threshold after calibration"
584 "|threshold[dac]:Resulting threshold from calibration"
585 "|begin[mjd]:Start time of calibration"
586 "|end[mjd]:End time of calibration")
587 {
588 // ba::io_service::work is a kind of keep_alive for the loop.
589 // It prevents the io_service to go to stopped state, which
590 // would prevent any consecutive calls to run()
591 // or poll() to do nothing. reset() could also revoke to the
592 // previous state but this might introduce some overhead of
593 // deletion and creation of threads and more.
594
595 fDim.Subscribe(*this);
596 fDimFTM.Subscribe(*this);
597 fDimRS.Subscribe(*this);
598 fDimDrive.Subscribe(*this);
599
600 Subscribe("FTM_CONTROL/TRIGGER_RATES")
601 (bind(&StateMachineRateControl::HandleTriggerRates, this, placeholders::_1));
602 Subscribe("FTM_CONTROL/STATIC_DATA")
603 (bind(&StateMachineRateControl::HandleStaticData, this, placeholders::_1));
604 Subscribe("FEEDBACK/CALIBRATED_CURRENTS")
605 (bind(&StateMachineRateControl::HandleCalibratedCurrents, this, placeholders::_1));
606
607 // State names
608 AddStateName(RateControl::State::kDimNetworkNA, "DimNetworkNotAvailable",
609 "The Dim DNS is not reachable.");
610
611 AddStateName(RateControl::State::kDisconnected, "Disconnected",
612 "The Dim DNS is reachable, but the required subsystems are not available.");
613
614 AddStateName(RateControl::State::kConnected, "Connected",
615 "All needed subsystems are connected to their hardware, no action is performed.");
616
617 AddStateName(RateControl::State::kSettingGlobalThreshold, "Calibrating",
618 "A global minimum thrshold is currently determined.");
619
620 AddStateName(RateControl::State::kGlobalThresholdSet, "GlobalThresholdSet",
621 "A global threshold has ben set, waiting for the trigger to be switched on.");
622
623 AddStateName(RateControl::State::kInProgress, "InProgress",
624 "Rate control in progress.");
625
626 AddEvent("CALIBRATE")
627 (bind(&StateMachineRateControl::Calibrate, this))
628 ("Start a search for a reasonable minimum global threshold");
629
630 AddEvent("CALIBRATE_BY_CURRENT")
631 (bind(&StateMachineRateControl::CalibrateByCurrent, this))
632 ("Set the global threshold from the median current");
633
634 AddEvent("STOP", RateControl::State::kSettingGlobalThreshold, RateControl::State::kGlobalThresholdSet, RateControl::State::kInProgress)
635 (bind(&StateMachineRateControl::StopRC, this))
636 ("Stop a calibration or ratescan in progress");
637
638 AddEvent("SET_MIN_THRESHOLD", "I:1")
639 (bind(&StateMachineRateControl::SetMinThreshold, this, placeholders::_1))
640 ("Set a minimum threshold at which th rate control starts calibrating");
641
642 AddEvent("SET_TARGET_RATE", "F:1")
643 (bind(&StateMachineRateControl::SetTargetRate, this, placeholders::_1))
644 ("Set a target trigger rate for the calibration");
645
646 AddEvent("PRINT")
647 (bind(&StateMachineRateControl::Print, this))
648 ("Print current status");
649
650 AddEvent("SET_VERBOSE", "B")
651 (bind(&StateMachineRateControl::SetVerbosity, this, placeholders::_1))
652 ("set verbosity state"
653 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
654
655 }
656
657 int EvalOptions(Configuration &conf)
658 {
659 fVerbose = !conf.Get<bool>("quiet");
660
661 fThresholdReference = 300;
662 fThresholdMin = 300;
663 fTargetRate = 75;
664
665 return -1;
666 }
667};
668
669// ------------------------------------------------------------------------
670
671#include "Main.h"
672
673template<class T>
674int RunShell(Configuration &conf)
675{
676 return Main::execute<T, StateMachineRateControl>(conf);
677}
678
679void SetupConfiguration(Configuration &conf)
680{
681 po::options_description control("Rate control options");
682 control.add_options()
683 ("quiet,q", po_bool(), "Disable printing more informations during rate control.")
684 //("max-wait", var<uint16_t>(150), "The maximum number of seconds to wait to get the anticipated resolution for a point.")
685 // ("resolution", var<double>(0.05) , "The minimum resolution required for a single data point.")
686 ;
687
688 conf.AddOptions(control);
689}
690
691/*
692 Extract usage clause(s) [if any] for SYNOPSIS.
693 Translators: "Usage" and "or" here are patterns (regular expressions) which
694 are used to match the usage synopsis in program output. An example from cp
695 (GNU coreutils) which contains both strings:
696 Usage: cp [OPTION]... [-T] SOURCE DEST
697 or: cp [OPTION]... SOURCE... DIRECTORY
698 or: cp [OPTION]... -t DIRECTORY SOURCE...
699 */
700void PrintUsage()
701{
702 cout <<
703 "The ratecontrol program is a keep the rate reasonable low.\n"
704 "\n"
705 "Usage: ratecontrol [-c type] [OPTIONS]\n"
706 " or: ratecontrol [OPTIONS]\n";
707 cout << endl;
708}
709
710void PrintHelp()
711{
712 Main::PrintHelp<StateMachineRateControl>();
713
714 /* Additional help text which is printed after the configuration
715 options goes here */
716
717 /*
718 cout << "bla bla bla" << endl << endl;
719 cout << endl;
720 cout << "Environment:" << endl;
721 cout << "environment" << endl;
722 cout << endl;
723 cout << "Examples:" << endl;
724 cout << "test exam" << endl;
725 cout << endl;
726 cout << "Files:" << endl;
727 cout << "files" << endl;
728 cout << endl;
729 */
730}
731
732int main(int argc, const char* argv[])
733{
734 Configuration conf(argv[0]);
735 conf.SetPrintUsage(PrintUsage);
736 Main::SetupConfiguration(conf);
737 SetupConfiguration(conf);
738
739 if (!conf.DoParse(argc, argv, PrintHelp))
740 return 127;
741
742 if (!conf.Has("console"))
743 return RunShell<LocalStream>(conf);
744
745 if (conf.Get<int>("console")==0)
746 return RunShell<LocalShell>(conf);
747 else
748 return RunShell<LocalConsole>(conf);
749
750 return 0;
751}
Note: See TracBrowser for help on using the repository browser.