source: trunk/FACT++/src/feedback.cc@ 12105

Last change on this file since 12105 was 12067, checked in by tbretz, 13 years ago
Added or updated some text; set default control loop parameters in EvalOptions; added a gain (conversion constant)
File size: 22.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 "DimServiceInfoList.h"
12#include "PixelMap.h"
13
14#include "tools.h"
15
16#include "LocalControl.h"
17
18#include "HeadersFAD.h"
19#include "HeadersBIAS.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
31// ------------------------------------------------------------------------
32
33class StateMachineFeedback : public StateMachineDim, public DimInfoHandler
34{
35 /*
36 int Wrap(boost::function<void()> f)
37 {
38 f();
39 return T::GetCurrentState();
40 }
41
42 boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
43 {
44 return bind(&StateMachineMCP::Wrap, this, func);
45 }*/
46
47private:
48 enum states_t
49 {
50 kStateDimNetworkNA = 1,
51 kStateDisconnected,
52 kStateConnecting,
53 kStateConnected,
54 kStateRunning,
55 kStateControlling,
56 };
57
58 PixelMap fMap;
59
60 DimServiceInfoList fNetwork;
61
62 pair<Time, int> fStatusDim;
63 pair<Time, int> fStatusFAD;
64 pair<Time, int> fStatusBias;
65
66 DimStampedInfo fDim;
67 DimStampedInfo fFAD;
68 DimStampedInfo fBias;
69
70 DimStampedInfo *fBiasData;
71
72 DimDescribedService fDimReference;
73 DimDescribedService fDimDeviation;
74
75 vector<vector<float>> fData;
76
77 uint64_t fCursor;
78
79 Time fBiasLast;
80 Time fStartTime;
81
82 valarray<double> fPV[3]; // Process variable (intgerated/averaged amplitudes)
83 valarray<double> fSP; // Set point (target amplitudes)
84
85 double fKp; // Proportional constant
86 double fKi; // Integral constant
87 double fKd; // Derivative constant
88 double fT; // Time constant (cycle time)
89 double fGain; // Gain (conversion from a DRS voltage deviation into a BIAS voltage change at G-APD reference voltage)
90
91 double fT21;
92
93 bool fOutputEnabled;
94
95 pair<Time, int> GetNewState(DimStampedInfo &info) const
96 {
97 const bool disconnected = info.getSize()==0;
98
99 // Make sure getTimestamp is called _before_ getTimestampMillisecs
100 const int tsec = info.getTimestamp();
101 const int tms = info.getTimestampMillisecs();
102
103 return make_pair(Time(tsec, tms*1000),
104 disconnected ? -2 : info.getQuality());
105 }
106
107 void ResetData()
108 {
109 fData.clear();
110 fData.resize(500);
111 fCursor = 0;
112 fStartTime = Time();
113
114 fSP.resize(416);
115 fSP = valarray<double>(0., 416);
116
117 vector<float> vec(2*BIAS::kNumChannels);
118 fDimDeviation.Update(vec);
119
120 fPV[0].resize(0);
121 fPV[1].resize(0);
122 fPV[2].resize(0);
123
124 if (fKp==0 && fKi==0 && fKd==0)
125 Warn("Control loop parameters are all set to zero.");
126 }
127
128 void infoHandler()
129 {
130 DimInfo *curr = getInfo(); // get current DimInfo address
131 if (!curr)
132 return;
133
134 if (curr==&fBias)
135 {
136 fStatusBias = GetNewState(fBias);
137 return;
138 }
139
140 if (curr==&fFAD)
141 {
142 fStatusFAD = GetNewState(fFAD);
143 return;
144 }
145
146 if (curr==&fDim)
147 {
148 fStatusDim = GetNewState(fDim);
149 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
150 return;
151 }
152
153 if (curr==fBiasData)
154 {
155 if (curr->getSize()==0)
156 return;
157 if (curr->getSize()!=1440*sizeof(float))
158 return;
159
160 // -------- Check age of last stored event --------
161
162 // Must be called in this order
163 const int tsec = curr->getTimestamp();
164 const int tms = curr->getTimestampMillisecs();
165
166 const Time tm(tsec, tms*1000);
167
168 if (Time()-fBiasLast>boost::posix_time::seconds(30))
169 {
170 Warn("Last received event data older than 30s... resetting average calculation.");
171 ResetData();
172 }
173 fBiasLast = tm;
174
175 // -------- Store new event --------
176
177 fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
178 reinterpret_cast<float*>(curr->getData())+1440);
179
180
181 fCursor++;
182
183 if (fCursor<fData.size())
184 return;
185
186 // -------- Calculate statistics --------
187
188 valarray<double> med(1440);
189
190 for (int ch=0; ch<1440; ch++)
191 {
192 vector<float> arr(fData.size());
193 for (size_t i=0; i<fData.size(); i++)
194 arr[i] = fData[i][ch];
195
196 sort(arr.begin(), arr.end());
197
198 med[ch] = arr[arr.size()/2];
199 }
200
201 /*
202 vector<float> med(1440);
203 vector<float> rms(1440);
204 for (size_t i=0; i<fData.size(); i++)
205 {
206 if (fData[i].size()==0)
207 return;
208
209 for (int j=0; j<1440; j++)
210 {
211 med[j] += fData[i][j];
212 rms[j] += fData[i][j]*fData[i][j];
213 }
214 }
215 */
216
217 vector<double> avg(BIAS::kNumChannels);
218 vector<int> num(BIAS::kNumChannels);
219 for (int i=0; i<1440; i++)
220 {
221 const PixelMapEntry &ch = fMap.hw(i);
222
223 // FIXME: Add a consistency check if the median makes sense...
224 // FIXME: Add a consistency check to remove pixels with bright stars (median?)
225
226 avg[ch.hv()] += med[i];
227 num[ch.hv()]++;
228 }
229
230 for (int i=0; i<BIAS::kNumChannels; i++)
231 {
232 if (num[i])
233 avg[i] /= num[i];
234
235 }
236
237 // -------- Calculate correction --------
238
239 // http://bestune.50megs.com/typeABC.htm
240
241 // CO: Controller output
242 // PV: Process variable
243 // SP: Set point
244 // T: Sampling period (loop update period)
245 // e = SP - PV
246 //
247 // Kp : No units
248 // Ki : per seconds
249 // Kd : seconds
250
251 // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
252
253 if (fCursor%fData.size()==0)
254 {
255 // FIXME: Take out broken / dead boards.
256
257 const Time tm0 = Time();
258
259 const double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
260 const double T10 = fT21;
261 fT21 = T21;
262
263 fStartTime = tm0;
264
265 ostringstream out;
266 out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
267 Info(out);
268
269 if (fPV[0].size()==0)
270 {
271 fPV[0].resize(avg.size());
272 fPV[0] = valarray<double>(avg.data(), avg.size());
273 }
274 else
275 if (fPV[1].size()==0)
276 {
277 fPV[1].resize(avg.size());
278 fPV[1] = valarray<double>(avg.data(), avg.size());
279 }
280 else
281 if (fPV[2].size()==0)
282 {
283 fPV[2].resize(avg.size());
284 fPV[2] = valarray<double>(avg.data(), avg.size());
285 }
286 else
287 {
288 fPV[0] = fPV[1];
289 fPV[1] = fPV[2];
290
291 fPV[2].resize(avg.size());
292 fPV[2] = valarray<double>(avg.data(), avg.size());
293
294 if (T10<=0 || T21<=0)
295 return;
296
297 cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
298
299 // fKi[j] = response[j]*gain;
300 // Kp = 0;
301 // Kd = 0;
302
303 // -110 / -110 (-23 DAC / -0.51V)
304 // Reference voltage: -238 / -203
305 // -360 / -343 ( 23 DAC / 0.51V)
306
307 // 0.005 A/V
308 // 220 Amplitude / 1V
309
310 // Gain = 1V / 200 = 0.005
311
312 // => Kp = 0.01 * gain = 0.00005
313 // => Ki = 0.8 * gain/20s = 0.00025
314 // => Kd = 0.1 * gain/20s = 0.00003
315
316 //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
317 //valarray<double> correction =
318 // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
319 const valarray<double> correction = fGain/1000*
320 (
321 - (fKp+fKd/T21)*(fPV[2] - fPV[1])
322 + fKi*T21*(fSP-fPV[2])
323 + fKd/T10*(fPV[1]-fPV[0])
324 );
325
326 vector<float> vec(2*BIAS::kNumChannels);
327 for (int i=0; i<BIAS::kNumChannels; i++)
328 vec[i] = fPV[2][i]-fSP[i];
329
330 for (int i=0; i<BIAS::kNumChannels; i++)
331 vec[i+416] = correction[i];
332
333 fDimDeviation.Update(vec);
334
335 if (fOutputEnabled)
336 {
337 Info("Sending correction to feedback.");
338
339 dic_cmnd_service((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
340 (void*)(vec.data()+416), 416*sizeof(float));
341
342 /*
343 if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
344 (const void*)(vec.data()+416), 416*sizeof(float)))
345 {
346 Error("Sending correction to bias control failed... switching off.");
347 fOutputEnabled=false;
348 }
349 else
350 Info("Success!");
351 */
352 }
353 }
354
355 }
356
357 /*
358 vector<float> correction(416);
359 vector<float> response(416);
360 vector<float> dev(416);
361
362 double gain = 0;
363
364 for (int j=0; j<416; j++)
365 {
366 //avg[j] /= fData.size();
367 //rms[j] /= sqrt((rms[j]*fData.size() - avg[j]*avg[j]))/fData.size();
368
369 dev[j] = avg[j] - target[j];
370
371 // Determine correction from response maxtrix and change voltages
372 correction[j] = -dev[j]*response[j]*gain;
373
374 if (correction[j]==0)
375 continue;
376
377 // Limit voltage steps // Limit changes to 100 mV
378// if (fabs(correction[j]) > 0.1)
379// correction[j] = 0.1*fabs(correction[j])/correction[j];
380
381 // Add voltage change command if not too noisy
382// if (fabs(Average[i]) > 2*Sigma[i])
383// Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";
384
385 }
386 */
387 return;
388 }
389
390 }
391
392 bool CheckEventSize(size_t has, const char *name, size_t size)
393 {
394 if (has==size)
395 return true;
396
397 ostringstream msg;
398 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
399 Fatal(msg);
400 return false;
401 }
402
403 void PrintState(const pair<Time,int> &state, const char *server)
404 {
405 const State rc = fNetwork.GetState(server, state.second);
406
407 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
408 Out() << kBold << server << ": ";
409 Out() << rc.name << "[" << rc.index << "]";
410 Out() << kReset << " - " << kBlue << rc.comment << endl;
411 }
412
413 int Print()
414 {
415 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
416 Out() << kBold << "DIM_DNS: ";
417 if (fStatusDim.second==0)
418 Out() << "Offline" << endl;
419 else
420 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
421
422 PrintState(fStatusFAD, "FAD_CONTROL");
423 PrintState(fStatusBias, "BIAS_CONTROL");
424
425 return GetCurrentState();
426 }
427
428 int SetConstant(const EventImp &evt, int constant)
429 {
430 if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
431 return kSM_FatalError;
432
433 switch (constant)
434 {
435 case 0: fKi = evt.GetDouble(); break;
436 case 1: fKp = evt.GetDouble(); break;
437 case 2: fKd = evt.GetDouble(); break;
438 case 3: fT = evt.GetDouble(); break;
439 case 4: fGain = evt.GetDouble(); break;
440 default:
441 Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
442 return kSM_FatalError;
443 }
444
445 return GetCurrentState();
446 }
447
448 int EnableOutput(const EventImp &evt)
449 {
450 if (!CheckEventSize(evt.GetSize(), "EnableOutput", 1))
451 return kSM_FatalError;
452
453 fOutputEnabled = evt.GetBool();
454
455 return GetCurrentState();
456 }
457
458 int StartFeedback()
459 {
460 fBiasData = new DimStampedInfo("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this);
461
462 ResetData();
463
464 return GetCurrentState();
465 }
466
467 int StopFeedback()
468 {
469 delete fBiasData;
470 fBiasData = 0;
471
472 return GetCurrentState();
473 }
474
475 int StoreReference()
476 {
477 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
478 {
479 Warn("No values in memory. Take enough events first!");
480 return GetCurrentState();
481 }
482
483 // FIXME: Check age
484
485 if (!fPV[1].size() && !fPV[2].size())
486 fSP = fPV[0];
487
488 if (!fPV[2].size())
489 fSP = fPV[1];
490 else
491 fSP = fPV[2];
492
493 vector<float> vec(BIAS::kNumChannels);
494 for (int i=0; i<BIAS::kNumChannels; i++)
495 vec[i] = fSP[i];
496 fDimReference.Update(vec);
497
498 return GetCurrentState();
499 }
500
501
502 int Execute()
503 {
504 // Dispatch (execute) at most one handler from the queue. In contrary
505 // to run_one(), it doesn't wait until a handler is available
506 // which can be dispatched, so poll_one() might return with 0
507 // handlers dispatched. The handlers are always dispatched/executed
508 // synchronously, i.e. within the call to poll_one()
509 //poll_one();
510
511 if (fStatusDim.second==0)
512 return kStateDimNetworkNA;
513
514 if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnecting)
515 return kStateDisconnected;
516
517 if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnected)
518 return kStateConnecting;
519
520 return fBiasData ? kStateRunning : kStateConnected;
521 }
522
523public:
524 StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
525 fStatusDim(make_pair(Time(), -2)),
526 fStatusFAD(make_pair(Time(), -2)),
527 fStatusBias(make_pair(Time(), -2)),
528 fDim("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
529 fFAD("FAD_CONTROL/STATE", (void*)NULL, 0, this),
530 fBias("BIAS_CONTROL/STATE", (void*)NULL, 0, this),
531 fBiasData(0),
532 fDimReference("FEEDBACK/REFERENCE", "F:416", ""),
533 fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416", ""),
534 fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false)
535 {
536 // ba::io_service::work is a kind of keep_alive for the loop.
537 // It prevents the io_service to go to stopped state, which
538 // would prevent any consecutive calls to run()
539 // or poll() to do nothing. reset() could also revoke to the
540 // previous state but this might introduce some overhead of
541 // deletion and creation of threads and more.
542
543 // State names
544 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
545 ".");
546
547 AddStateName(kStateDisconnected, "Disconnected",
548 ".");
549
550 AddStateName(kStateConnecting, "Connecting",
551 ".");
552
553 AddStateName(kStateConnected, "Connected",
554 ".");
555
556 AddStateName(kStateRunning, "Running",
557 ".");
558
559 AddEvent("START")//, kStateIdle)
560 (bind(&StateMachineFeedback::StartFeedback, this))
561 ("Start control loop");
562
563 AddEvent("STOP")//, kStateIdle)
564 (bind(&StateMachineFeedback::StopFeedback, this))
565 ("Stop control loop");
566
567 AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
568 (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
569 ("Enable sending of correction values caluclated by the control loop to the biasctrl");
570
571 AddEvent("STORE_REFERENCE")//, kStateIdle)
572 (bind(&StateMachineFeedback::StoreReference, this))
573 ("Store the last (averaged) value as new reference (for debug purpose only)");
574
575 AddEvent("SET_Ki", "D:1")//, kStateIdle)
576 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
577 ("Set integral constant Ki");
578
579 AddEvent("SET_Kp", "D:1")//, kStateIdle)
580 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
581 ("Set proportional constant Kp");
582
583 AddEvent("SET_Kd", "D:1")//, kStateIdle)
584 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
585 ("Set derivative constant Kd");
586
587 AddEvent("SET_T", "D:1")//, kStateIdle)
588 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 3))
589 ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");
590
591 // Verbosity commands
592// AddEvent("SET_VERBOSE", "B:1")
593// (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
594// ("set verbosity state"
595// "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
596
597 AddEvent("PRINT")
598 (bind(&StateMachineFeedback::Print, this))
599 ("");
600 }
601
602 int EvalOptions(Configuration &conf)
603 {
604 if (!fMap.Read(conf.Get<string>("pixel-map-file")))
605 {
606 Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
607 return 1;
608 }
609
610 fGain = 5; // (BIAS)V / (DRS)V ( 1V / 0.22V )
611
612 fKp = 0;
613 fKd = 0;
614 fKi = 0.12;
615 fT = 1;
616
617 ostringstream msg;
618 msg << "Control loop parameters: ";
619 msg << "Kp=" << fKp << ", Kd=" << fKd << ", Ki=" << fKi << ", ";
620 if (fT>0)
621 msg << fT;
622 else
623 msg << "<auto>";
624 msg << ", Gain(BIAS/DRS)=" << fGain << "V/V";
625
626 Message(msg);
627
628 return -1;
629 }
630};
631
632// ------------------------------------------------------------------------
633
634#include "Main.h"
635
636template<class T>
637int RunShell(Configuration &conf)
638{
639 return Main::execute<T, StateMachineFeedback>(conf);
640}
641
642void SetupConfiguration(Configuration &conf)
643{
644 po::options_description control("Feedback options");
645 control.add_options()
646 ("pixel-map-file", var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
647 ;
648
649 conf.AddOptions(control);
650}
651
652/*
653 Extract usage clause(s) [if any] for SYNOPSIS.
654 Translators: "Usage" and "or" here are patterns (regular expressions) which
655 are used to match the usage synopsis in program output. An example from cp
656 (GNU coreutils) which contains both strings:
657 Usage: cp [OPTION]... [-T] SOURCE DEST
658 or: cp [OPTION]... SOURCE... DIRECTORY
659 or: cp [OPTION]... -t DIRECTORY SOURCE...
660 */
661void PrintUsage()
662{
663 cout <<
664 "The feedback control the BIAS voltages based on the calibration signal.\n"
665 "\n"
666 "The default is that the program is started without user intercation. "
667 "All actions are supposed to arrive as DimCommands. Using the -c "
668 "option, a local shell can be initialized. With h or help a short "
669 "help message about the usuage can be brought to the screen.\n"
670 "\n"
671 "Usage: feedback [-c type] [OPTIONS]\n"
672 " or: feedback [OPTIONS]\n";
673 cout << endl;
674}
675
676void PrintHelp()
677{
678 /* Additional help text which is printed after the configuration
679 options goes here */
680
681 /*
682 cout << "bla bla bla" << endl << endl;
683 cout << endl;
684 cout << "Environment:" << endl;
685 cout << "environment" << endl;
686 cout << endl;
687 cout << "Examples:" << endl;
688 cout << "test exam" << endl;
689 cout << endl;
690 cout << "Files:" << endl;
691 cout << "files" << endl;
692 cout << endl;
693 */
694}
695
696int main(int argc, const char* argv[])
697{
698 Configuration conf(argv[0]);
699 conf.SetPrintUsage(PrintUsage);
700 Main::SetupConfiguration(conf);
701 SetupConfiguration(conf);
702
703 if (!conf.DoParse(argc, argv, PrintHelp))
704 return -1;
705
706 //try
707 {
708 // No console access at all
709 if (!conf.Has("console"))
710 {
711// if (conf.Get<bool>("no-dim"))
712// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
713// else
714 return RunShell<LocalStream>(conf);
715 }
716 // Cosole access w/ and w/o Dim
717/* if (conf.Get<bool>("no-dim"))
718 {
719 if (conf.Get<int>("console")==0)
720 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
721 else
722 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
723 }
724 else
725*/ {
726 if (conf.Get<int>("console")==0)
727 return RunShell<LocalShell>(conf);
728 else
729 return RunShell<LocalConsole>(conf);
730 }
731 }
732 /*catch (std::exception& e)
733 {
734 cerr << "Exception: " << e.what() << endl;
735 return -1;
736 }*/
737
738 return 0;
739}
740
741const PixelMapEntry PixelMap::empty = { 0, 0, 0, 0, 0, 0 };
Note: See TracBrowser for help on using the repository browser.