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

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