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

Last change on this file since 12807 was 12805, checked in by tbretz, 13 years ago
Store calibration; implemented command to print a stored calibration; fixed a bug which caused the feedback to remain in state Calibrating even a STOP command was issued.
File size: 31.2 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 kStateTempCtrlIdle,
55 kStateFeedbackCtrlIdle,
56 kStateTempCtrlRunning,
57 kStateFeedbackCtrlRunning,
58 kStateCalibrating,
59 };
60
61 enum control_t
62 {
63 kIdle,
64 kTemp,
65 kFeedback
66 };
67
68 control_t fControlType;
69
70 PixelMap fMap;
71
72 DimServiceInfoList fNetwork;
73
74 pair<Time, int> fStatusDim;
75 pair<Time, int> fStatusFAD;
76 pair<Time, int> fStatusFSC;
77 pair<Time, int> fStatusBias;
78
79 DimStampedInfo fDim;
80 DimStampedInfo fFAD;
81 DimStampedInfo fFSC;
82 DimStampedInfo fBias;
83 DimStampedInfo fBiasA;
84
85 DimStampedInfo fBiasData;
86 DimStampedInfo fCameraTemp;
87
88 DimDescribedService fDimReference;
89 DimDescribedService fDimDeviation;
90 DimDescribedService fDimCalibration;
91
92 vector<int64_t> fCurrentsAvg;
93 vector<int64_t> fCurrentsRms;
94
95 vector<double> fCalibration;
96
97 vector<vector<float>> fData;
98
99 uint64_t fCursor;
100
101 Time fBiasLast;
102 Time fStartTime;
103
104 valarray<double> fPV[3]; // Process variable (intgerated/averaged amplitudes)
105 valarray<double> fSP; // Set point (target amplitudes)
106
107 double fKp; // Proportional constant
108 double fKi; // Integral constant
109 double fKd; // Derivative constant
110 double fT; // Time constant (cycle time)
111 double fGain; // Gain (conversion from a DRS voltage deviation into a BIAS voltage change at G-APD reference voltage)
112
113 double fT21;
114
115 double fBiasOffset;
116
117 bool fOutputEnabled;
118
119 pair<Time, int> GetNewState(DimStampedInfo &info) const
120 {
121 const bool disconnected = info.getSize()==0;
122
123 // Make sure getTimestamp is called _before_ getTimestampMillisecs
124 const int tsec = info.getTimestamp();
125 const int tms = info.getTimestampMillisecs();
126
127 return make_pair(Time(tsec, tms*1000),
128 disconnected ? -2 : info.getQuality());
129 }
130
131 void ResetData()
132 {
133 fData.clear();
134 fData.resize(500);
135 fCursor = 0;
136 fStartTime = Time();
137
138 fSP = valarray<double>(0., 416);
139
140 vector<float> vec(2*BIAS::kNumChannels);
141 fDimDeviation.Update(vec);
142
143 fPV[0].resize(0);
144 fPV[1].resize(0);
145 fPV[2].resize(0);
146
147 if (fKp==0 && fKi==0 && fKd==0)
148 Warn("Control loop parameters are all set to zero.");
149 }
150
151 void infoHandler()
152 {
153 DimInfo *curr = getInfo(); // get current DimInfo address
154 if (!curr)
155 return;
156
157 if (curr==&fBias)
158 {
159 fStatusBias = GetNewState(fBias);
160 return;
161 }
162
163 if (curr==&fFAD)
164 {
165 fStatusFAD = GetNewState(fFAD);
166 return;
167 }
168
169 if (curr==&fFSC)
170 {
171 fStatusFSC = GetNewState(fFSC);
172 return;
173 }
174
175 if (curr==&fDim)
176 {
177 fStatusDim = GetNewState(fDim);
178 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
179 return;
180 }
181
182 if (curr==&fCameraTemp)
183 {
184 if (curr->getSize()!=60*sizeof(float))
185 return;
186
187 const float *ptr = static_cast<float*>(curr->getData());
188
189 double avg = 0;
190 int num = 0;
191 for (int i=1; i<32; i++)
192 if (ptr[i]!=0)
193 {
194 avg += ptr[i];
195 num++;
196 }
197
198 if (num==0)
199 return;
200
201 avg /= num;
202
203
204 const float diff = (avg-25)*4./70 + fBiasOffset;
205
206 vector<float> vec(2*BIAS::kNumChannels);
207 for (int i=0; i<BIAS::kNumChannels; i++)
208 vec[i+416] = diff;
209
210 if (fControlType!=kTemp)
211 return;
212
213 fDimDeviation.Update(vec);
214
215 if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
216 {
217 Info("Sending correction to feedback.");
218
219 DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
220 (void*)&diff, sizeof(float));
221 }
222 }
223
224 if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
225 {
226 if (curr->getSize()!=416*sizeof(int16_t))
227 return;
228
229 if (fStatusBias.second==BIAS::kRamping)
230 return;
231
232 const int16_t *ptr = static_cast<int16_t*>(curr->getData());
233
234 for (int i=0; i<416; i++)
235 if (ptr[i]>0)
236 {
237 fCurrentsAvg[i] += ptr[i];
238 fCurrentsRms[i] += ptr[i]*ptr[i];
239 }
240
241 if (++fCursor<100)
242 {
243 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
244 return;
245 }
246
247 fCalibration.resize(416);
248 for (int i=0; i<416; i++)
249 {
250 fCalibration[i] = double(fCurrentsAvg[i])/fCursor;
251 fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-val[i]*val[i]);
252 }
253
254 fDimCalibration.Update(fCalibration);
255
256 fOutputEnabled = false;
257 fControlType = kIdle;
258
259 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
260 }
261
262 if (curr==&fBiasData && fControlType==kFeedback)
263 {
264 if (curr->getSize()!=1440*sizeof(float))
265 return;
266
267 // -------- Check age of last stored event --------
268
269 // Must be called in this order
270 const int tsec = curr->getTimestamp();
271 const int tms = curr->getTimestampMillisecs();
272
273 const Time tm(tsec, tms*1000);
274
275 if (Time()-fBiasLast>boost::posix_time::seconds(30))
276 {
277 Warn("Last received event data older than 30s... resetting average calculation.");
278 ResetData();
279 }
280 fBiasLast = tm;
281
282 // -------- Store new event --------
283
284 fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
285 reinterpret_cast<float*>(curr->getData())+1440);
286
287
288 fCursor++;
289
290 if (fCursor<fData.size())
291 return;
292
293 // -------- Calculate statistics --------
294
295 valarray<double> med(1440);
296
297 for (int ch=0; ch<1440; ch++)
298 {
299 vector<float> arr(fData.size());
300 for (size_t i=0; i<fData.size(); i++)
301 arr[i] = fData[i][ch];
302
303 sort(arr.begin(), arr.end());
304
305 med[ch] = arr[arr.size()/2];
306 }
307
308 /*
309 vector<float> med(1440);
310 vector<float> rms(1440);
311 for (size_t i=0; i<fData.size(); i++)
312 {
313 if (fData[i].size()==0)
314 return;
315
316 for (int j=0; j<1440; j++)
317 {
318 med[j] += fData[i][j];
319 rms[j] += fData[i][j]*fData[i][j];
320 }
321 }
322 */
323
324 vector<double> avg(BIAS::kNumChannels);
325 vector<int> num(BIAS::kNumChannels);
326 for (int i=0; i<1440; i++)
327 {
328 const PixelMapEntry &ch = fMap.hw(i);
329
330 // FIXME: Add a consistency check if the median makes sense...
331 // FIXME: Add a consistency check to remove pixels with bright stars (median?)
332
333 avg[ch.hv()] += med[i];
334 num[ch.hv()]++;
335 }
336
337 for (int i=0; i<BIAS::kNumChannels; i++)
338 {
339 if (num[i])
340 avg[i] /= num[i];
341
342 }
343
344 // -------- Calculate correction --------
345
346 // http://bestune.50megs.com/typeABC.htm
347
348 // CO: Controller output
349 // PV: Process variable
350 // SP: Set point
351 // T: Sampling period (loop update period)
352 // e = SP - PV
353 //
354 // Kp : No units
355 // Ki : per seconds
356 // Kd : seconds
357
358 // 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) ]
359
360 if (fCursor%fData.size()==0)
361 {
362 // FIXME: Take out broken / dead boards.
363
364 const Time tm0 = Time();
365
366 /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
367 const double T10 = fT21;
368 fT21 = T21;
369
370 fStartTime = tm0;
371
372 ostringstream out;
373 out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
374 Info(out);
375
376 if (fPV[0].size()==0)
377 {
378 fPV[0].resize(avg.size());
379 fPV[0] = valarray<double>(avg.data(), avg.size());
380 }
381 else
382 if (fPV[1].size()==0)
383 {
384 fPV[1].resize(avg.size());
385 fPV[1] = valarray<double>(avg.data(), avg.size());
386 }
387 else
388 if (fPV[2].size()==0)
389 {
390 fPV[2].resize(avg.size());
391 fPV[2] = valarray<double>(avg.data(), avg.size());
392 }
393 else
394 {
395 fPV[0] = fPV[1];
396 fPV[1] = fPV[2];
397
398 fPV[2].resize(avg.size());
399 fPV[2] = valarray<double>(avg.data(), avg.size());
400
401 if (T10<=0 || T21<=0)
402 return;
403
404 cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
405
406 // fKi[j] = response[j]*gain;
407 // Kp = 0;
408 // Kd = 0;
409
410 // => Kp = 0.01 * gain = 0.00005
411 // => Ki = 0.8 * gain/20s = 0.00025
412 // => Kd = 0.1 * gain/20s = 0.00003
413
414 fKp = 0;
415 fKd = 0;
416 fKi = 0.00003*20;
417 T21 = 1;
418
419 //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
420 //valarray<double> correction =
421 // - Kp * (PV[2] - PV[1])
422 // + dT * Ki * (SP - PV[2])
423 // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
424 //
425 // - (Kp+Kd/dT1) * (PV[2] - PV[1])
426 // + dT2 * Ki * (SP - PV[2])
427 // + Kd / dT1 * (PV[1] - PV[0]);
428 //
429 // - Kp * (PV[2] - PV[1])
430 // + Ki * (SP - PV[2])*dT
431 // - Kd * (PV[2] - PV[1])/dT
432 // + Kd * (PV[1] - PV[0])/dT;
433 //
434 //valarray<double> correction =
435 // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
436 const valarray<double> correction = fGain/1000*
437 (
438 - (fKp+fKd/T21)*(fPV[2] - fPV[1])
439 + fKi*T21*(fSP-fPV[2])
440 + fKd/T10*(fPV[1]-fPV[0])
441 );
442
443 /*
444 integral = 0
445 start:
446 integral += (fSP - fPV[2])*dt
447
448 output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt
449
450 wait(dt)
451
452 goto start
453 */
454
455 vector<float> vec(2*BIAS::kNumChannels);
456 for (int i=0; i<BIAS::kNumChannels; i++)
457 vec[i] = fPV[2][i]-fSP[i];
458
459 for (int i=0; i<BIAS::kNumChannels; i++)
460 vec[i+416] = avg[i]<5*2.5 ? 0 : correction[i];
461
462 fDimDeviation.Update(vec);
463
464 if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
465 {
466 Info("Sending correction to feedback.");
467
468 DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
469 (void*)(vec.data()+416), 416*sizeof(float));
470
471 /*
472 if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
473 (const void*)(vec.data()+416), 416*sizeof(float)))
474 {
475 Error("Sending correction to bias control failed... switching off.");
476 fOutputEnabled=false;
477 }
478 else
479 Info("Success!");
480 */
481 }
482 }
483
484 }
485 }
486
487 }
488
489 bool CheckEventSize(size_t has, const char *name, size_t size)
490 {
491 if (has==size)
492 return true;
493
494 ostringstream msg;
495 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
496 Fatal(msg);
497 return false;
498 }
499
500 void PrintState(const pair<Time,int> &state, const char *server)
501 {
502 const State rc = fNetwork.GetState(server, state.second);
503
504 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
505 Out() << kBold << server << ": ";
506 Out() << rc.name << "[" << rc.index << "]";
507 Out() << kReset << " - " << kBlue << rc.comment << endl;
508 }
509
510 int Print()
511 {
512 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
513 Out() << kBold << "DIM_DNS: ";
514 if (fStatusDim.second==0)
515 Out() << "Offline" << endl;
516 else
517 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
518
519 PrintState(fStatusFAD, "FAD_CONTROL");
520 PrintState(fStatusFSC, "FSC_CONTROL");
521 PrintState(fStatusBias, "BIAS_CONTROL");
522
523 return GetCurrentState();
524 }
525
526 int PrintCalibration()
527 {
528 if (fCalibration.size()==0)
529 {
530 Out() << "No calibration performed so far." << endl;
531 return GetCurrentStatus();
532 }
533
534 for (int k=0; k<13; k++)
535 for (int j=0; j<8; j++)
536 {
537 Out() << setw(2) << k << "|" << setw(2) << j*4 << "|";
538 for (int i=0; i<4; i++)
539 Out() << Tools::Format(" %6.1f+-4.1f", fCalibration[i], fCalibration[i+416]);
540 Out() << endl;
541 }
542 }
543
544 return GetCurrentState();
545 }
546
547 int SetConstant(const EventImp &evt, int constant)
548 {
549 if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
550 return kSM_FatalError;
551
552 switch (constant)
553 {
554 case 0: fKi = evt.GetDouble(); break;
555 case 1: fKp = evt.GetDouble(); break;
556 case 2: fKd = evt.GetDouble(); break;
557 case 3: fT = evt.GetDouble(); break;
558 case 4: fGain = evt.GetDouble(); break;
559 default:
560 Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
561 return kSM_FatalError;
562 }
563
564 return GetCurrentState();
565 }
566
567 int EnableOutput(const EventImp &evt)
568 {
569 if (!CheckEventSize(evt.GetSize(), "EnableOutput", 1))
570 return kSM_FatalError;
571
572 fOutputEnabled = evt.GetBool();
573
574 return GetCurrentState();
575 }
576
577 int StartFeedback()
578 {
579 ResetData();
580
581 fControlType = kFeedback;
582
583 return GetCurrentState();
584 }
585
586 int StartTempCtrl(const EventImp &evt)
587 {
588 if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
589 return kSM_FatalError;
590
591 ostringstream out;
592 out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
593 Message(out);
594
595 fBiasOffset = evt.GetFloat();
596 fControlType = kTemp;
597
598 return GetCurrentState();
599 }
600
601 int StopFeedback()
602 {
603 fControlType = kIdle;
604
605 return GetCurrentState();
606 }
607
608 int StoreReference()
609 {
610 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
611 {
612 Warn("No values in memory. Take enough events first!");
613 return GetCurrentState();
614 }
615
616 // FIXME: Check age
617
618 if (!fPV[1].size() && !fPV[2].size())
619 fSP = fPV[0];
620
621 if (!fPV[2].size())
622 fSP = fPV[1];
623 else
624 fSP = fPV[2];
625
626 vector<float> vec(BIAS::kNumChannels);
627 for (int i=0; i<BIAS::kNumChannels; i++)
628 vec[i] = fSP[i];
629 fDimReference.Update(vec);
630
631 return GetCurrentState();
632 }
633
634 int SetReference(const EventImp &evt)
635 {
636 if (!CheckEventSize(evt.GetSize(), "SetReference", 4))
637 return kSM_FatalError;
638
639 const float val = evt.GetFloat();
640 /*
641 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
642 {
643 Warn("No values in memory. Take enough events first!");
644 return GetCurrentState();
645 }*/
646
647 vector<float> vec(BIAS::kNumChannels);
648 for (int i=0; i<BIAS::kNumChannels; i++)
649 vec[i] = fSP[i] = val;
650 fDimReference.Update(vec);
651
652 return GetCurrentState();
653 }
654
655 int CalibrateCurrents()
656 {
657// if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
658// return kSM_FatalError;
659
660 ostringstream out;
661 out << "Starting temperature feedback with an offset of -2V";
662 Message(out);
663
664 fBiasOffset = -2;
665 fControlType = kTemp;
666 fCursor = 0;
667 fCurrentsAvg.assign(416, 0);
668 fCurrentsRms.assign(416, 0);
669 fCalibration.resize(0);
670 fStartTime = Time();
671 fOutputEnabled = true;
672
673 return kStateCalibrating;
674 }
675
676 int Execute()
677 {
678 // Dispatch (execute) at most one handler from the queue. In contrary
679 // to run_one(), it doesn't wait until a handler is available
680 // which can be dispatched, so poll_one() might return with 0
681 // handlers dispatched. The handlers are always dispatched/executed
682 // synchronously, i.e. within the call to poll_one()
683 //poll_one();
684
685 if (fStatusDim.second==0)
686 return kStateDimNetworkNA;
687
688 // All subsystems are not connected
689 if (fStatusFAD.second<FAD::kConnected &&
690 fStatusBias.second<BIAS::kConnecting &&
691 fStatusFSC.second<2)
692 return kStateDisconnected;
693
694 // At least one subsystem is not connected
695 if (fStatusFAD.second<FAD::kConnected ||
696 fStatusBias.second<BIAS::kConnected ||
697 fStatusFSC.second<2)
698 return kStateConnecting;
699
700/*
701 // All subsystems are connected
702 if (GetCurrentStatus()==kStateConfiguringStep1)
703 {
704 if (fCursor<1)
705 return kStateConfiguringStep1;
706
707 if (fCursor==1)
708 {
709 fStartTime = Time();
710 return kStateConfiguringStep2;
711 }
712 }
713 if (GetCurrentStatus()==kStateConfiguringStep2)
714 {
715 if (fCursor==1)
716 {
717 if ((Time()-fStartTime).total_microseconds()/1000000.<1.5)
718 return kStateConfiguringStep2;
719
720 Dim::SendCommand("BIAS_CONTROL/REQUEST_STATUS");
721 }
722 if (fCursor==2)
723 {
724
725 int n=0;
726 double avg = 0;
727 for (size_t i=0; i<fCurrents.size(); i++)
728 if (fCurrents[i]>=0)
729 {
730 avg += fCurrents[i];
731 n++;
732 }
733
734 cout << avg/n << endl;
735 }
736 return kStateConnected;
737 }
738*/
739 if (fControlType==kFeedback)
740 return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
741
742 if (fControlType==kTemp)
743 {
744 if (GetCurrentState()==kStateCalibrating && fCursor<100)
745 return GetCurrentState();
746
747 return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
748 }
749
750 return kStateConnected;
751 }
752
753public:
754 StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
755 fStatusDim(make_pair(Time(), -2)),
756 fStatusFAD(make_pair(Time(), -2)),
757 fStatusBias(make_pair(Time(), -2)),
758 fDim("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
759 fFAD("FAD_CONTROL/STATE", (void*)NULL, 0, this),
760 fFSC("FSC_CONTROL/STATE", (void*)NULL, 0, this),
761 fBias("BIAS_CONTROL/STATE", (void*)NULL, 0, this),
762 fBiasA("BIAS_CONTROL/CURRENT", (void*)NULL, 0, this),
763 fBiasData("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this),
764 fCameraTemp("FSC_CONTROL/TEMPERATURE", (void*)NULL, 0, this),
765 fDimReference("FEEDBACK/REFERENCE", "F:416", ""),
766 fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416", ""),
767 fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416", ""),
768 fSP(416),
769 fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false)
770 {
771 // ba::io_service::work is a kind of keep_alive for the loop.
772 // It prevents the io_service to go to stopped state, which
773 // would prevent any consecutive calls to run()
774 // or poll() to do nothing. reset() could also revoke to the
775 // previous state but this might introduce some overhead of
776 // deletion and creation of threads and more.
777
778// fSP.resize(416);
779
780 // State names
781 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
782 "The Dim DNS is not reachable.");
783
784 AddStateName(kStateDisconnected, "Disconnected",
785 "The Dim DNS is reachable, but the required subsystems are not available.");
786
787 AddStateName(kStateConnecting, "Connecting",
788 "Not all subsystems (FAD, FSC, Bias) are connected to their hardware.");
789
790 AddStateName(kStateConnected, "Connected",
791 "All needed subsystems are connected to their hardware, no action is performed.");
792
793 AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
794 "Feedback control activated, but voltage output disabled.");
795
796 AddStateName(kStateTempCtrlIdle, "FeedbackIdle",
797 "Temperature control activated, but voltage output disabled.");
798
799 AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl",
800 "Feedback control activated and voltage output enabled.");
801
802 AddStateName(kStateTempCtrlRunning, "TempControl",
803 "Temperature control activated and voltage output enabled.");
804
805 AddStateName(kStateCalibrating, "Calibrating",
806 "Calibrating current offsets.");
807
808 AddEvent("START_FEEDBACK_CONTROL", kStateConnected)
809 (bind(&StateMachineFeedback::StartFeedback, this))
810 ("Start the feedback control loop");
811
812 AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected)
813 (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
814 ("Start the temperature control loop"
815 "|offset[V]:Offset from the nominal temperature corrected value in Volts");
816
817 // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning
818 AddEvent("STOP")
819 (bind(&StateMachineFeedback::StopFeedback, this))
820 ("Stop any control loop");
821
822 AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
823 (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
824 ("Enable sending of correction values caluclated by the control loop to the biasctrl");
825
826 AddEvent("STORE_REFERENCE")//, kStateIdle)
827 (bind(&StateMachineFeedback::StoreReference, this))
828 ("Store the last (averaged) value as new reference (for debug purpose only)");
829
830 AddEvent("SET_REFERENCE", "F:1")//, kStateIdle)
831 (bind(&StateMachineFeedback::SetReference, this, placeholders::_1))
832 ("Set a new global reference value (for debug purpose only)");
833
834 AddEvent("SET_Ki", "D:1")//, kStateIdle)
835 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
836 ("Set integral constant Ki");
837
838 AddEvent("SET_Kp", "D:1")//, kStateIdle)
839 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
840 ("Set proportional constant Kp");
841
842 AddEvent("SET_Kd", "D:1")//, kStateIdle)
843 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
844 ("Set derivative constant Kd");
845
846 AddEvent("SET_T", "D:1")//, kStateIdle)
847 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 3))
848 ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");
849
850 AddEvent("CALIBRATE_CURRENTS", kStateConnected)//, kStateIdle)
851 (bind(&StateMachineFeedback::CalibrateCurrents, this))
852 ("");
853
854 // Verbosity commands
855// AddEvent("SET_VERBOSE", "B:1")
856// (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
857// ("set verbosity state"
858// "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
859
860 AddEvent("PRINT")
861 (bind(&StateMachineFeedback::Print, this))
862 ("");
863
864 AddEvent("PRINT_CALIBRATION")
865 (bind(&StateMachineFeedback::PrintCalibration, this))
866 ("");
867 }
868
869 int EvalOptions(Configuration &conf)
870 {
871 if (!fMap.Read(conf.Get<string>("pixel-map-file")))
872 {
873 Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
874 return 1;
875 }
876
877 // -110 / -110 (-23 DAC / -0.51V)
878 // Reference voltage: -238 / -203
879 // -360 / -343 ( 23 DAC / 0.51V)
880
881 // 0.005 A/V
882 // 220 Amplitude / 1V
883
884 // Gain = 1V / 200 = 0.005
885
886 fGain = 5; // (BIAS)V / (DRS)V ( 1V / 0.22V )
887
888 fKp = 0;
889 fKd = 0;
890 fKi = 0.12;
891 fT = 1;
892
893 ostringstream msg;
894 msg << "Control loop parameters: ";
895 msg << "Kp=" << fKp << ", Kd=" << fKd << ", Ki=" << fKi << ", ";
896 if (fT>0)
897 msg << fT;
898 else
899 msg << "<auto>";
900 msg << ", Gain(BIAS/DRS)=" << fGain << "V/V";
901
902 Message(msg);
903
904 return -1;
905 }
906};
907
908// ------------------------------------------------------------------------
909
910#include "Main.h"
911
912template<class T>
913int RunShell(Configuration &conf)
914{
915 return Main::execute<T, StateMachineFeedback>(conf);
916}
917
918void SetupConfiguration(Configuration &conf)
919{
920 po::options_description control("Feedback options");
921 control.add_options()
922 ("pixel-map-file", var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
923 ;
924
925 conf.AddOptions(control);
926}
927
928/*
929 Extract usage clause(s) [if any] for SYNOPSIS.
930 Translators: "Usage" and "or" here are patterns (regular expressions) which
931 are used to match the usage synopsis in program output. An example from cp
932 (GNU coreutils) which contains both strings:
933 Usage: cp [OPTION]... [-T] SOURCE DEST
934 or: cp [OPTION]... SOURCE... DIRECTORY
935 or: cp [OPTION]... -t DIRECTORY SOURCE...
936 */
937void PrintUsage()
938{
939 cout <<
940 "The feedback control the BIAS voltages based on the calibration signal.\n"
941 "\n"
942 "The default is that the program is started without user intercation. "
943 "All actions are supposed to arrive as DimCommands. Using the -c "
944 "option, a local shell can be initialized. With h or help a short "
945 "help message about the usuage can be brought to the screen.\n"
946 "\n"
947 "Usage: feedback [-c type] [OPTIONS]\n"
948 " or: feedback [OPTIONS]\n";
949 cout << endl;
950}
951
952void PrintHelp()
953{
954 Main::PrintHelp<StateMachineFeedback>();
955
956 /* Additional help text which is printed after the configuration
957 options goes here */
958
959 /*
960 cout << "bla bla bla" << endl << endl;
961 cout << endl;
962 cout << "Environment:" << endl;
963 cout << "environment" << endl;
964 cout << endl;
965 cout << "Examples:" << endl;
966 cout << "test exam" << endl;
967 cout << endl;
968 cout << "Files:" << endl;
969 cout << "files" << endl;
970 cout << endl;
971 */
972}
973
974int main(int argc, const char* argv[])
975{
976 Configuration conf(argv[0]);
977 conf.SetPrintUsage(PrintUsage);
978 Main::SetupConfiguration(conf);
979 SetupConfiguration(conf);
980
981 if (!conf.DoParse(argc, argv, PrintHelp))
982 return -1;
983
984 //try
985 {
986 // No console access at all
987 if (!conf.Has("console"))
988 {
989// if (conf.Get<bool>("no-dim"))
990// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
991// else
992 return RunShell<LocalStream>(conf);
993 }
994 // Cosole access w/ and w/o Dim
995/* if (conf.Get<bool>("no-dim"))
996 {
997 if (conf.Get<int>("console")==0)
998 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
999 else
1000 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
1001 }
1002 else
1003*/ {
1004 if (conf.Get<int>("console")==0)
1005 return RunShell<LocalShell>(conf);
1006 else
1007 return RunShell<LocalConsole>(conf);
1008 }
1009 }
1010 /*catch (std::exception& e)
1011 {
1012 cerr << "Exception: " << e.what() << endl;
1013 return -1;
1014 }*/
1015
1016 return 0;
1017}
Note: See TracBrowser for help on using the repository browser.