source: trunk/FACT++/src/smartfact.cc@ 13497

Last change on this file since 13497 was 13497, checked in by tbretz, 13 years ago
Added current calibration; some updates to the colors
File size: 21.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#include "Converter.h"
11#include "DimServiceInfoList.h"
12#include "PixelMap.h"
13
14#include "tools.h"
15#include "DimData.h"
16
17#include "LocalControl.h"
18
19#include "HeadersFAD.h"
20#include "HeadersBIAS.h"
21
22namespace ba = boost::asio;
23namespace bs = boost::system;
24namespace dummy = ba::placeholders;
25
26using namespace std;
27
28// ------------------------------------------------------------------------
29
30#include "DimDescriptionService.h"
31
32// ------------------------------------------------------------------------
33
34class StateMachineSmartFACT : public StateMachineDim, public DimInfoHandler
35{
36private:
37 enum states_t
38 {
39 kStateDimNetworkNA = 1,
40 kStateRunning,
41 };
42
43 PixelMap fPixelMap;
44
45 DimServiceInfoList fNetwork;
46
47 pair<Time, int> fStatusDim;
48 pair<Time, int> fStatusDriveControl;
49 pair<Time, int> fStatusMagicWeather;
50 pair<Time, int> fStatusFeedback;
51 pair<Time, int> fStatusBiasControl;
52 pair<Time, int> fStatusFadControl;
53
54 DimStampedInfo fDim;
55
56 DimStampedInfo fDimDriveControl;
57 DimStampedInfo fDimDriveControlPointing;
58 DimStampedInfo fDimDriveControlTracking;
59 DimStampedInfo fDimDriveControlSource;
60
61 DimStampedInfo fDimMagicWeather;
62 DimStampedInfo fDimMagicWeatherData;
63
64 DimStampedInfo fDimFeedback;
65 DimStampedInfo fDimFeedbackCalibration;
66
67 DimStampedInfo fDimBiasControl;
68 DimStampedInfo fDimBiasControlVoltage;
69 DimStampedInfo fDimBiasControlCurrent;
70
71 DimStampedInfo fDimFadControl;
72 DimStampedInfo *fDimFadControlEventData;
73
74 Time fLastUpdate;
75
76 enum weather_t { kTemp = 0, kDew, kHum, kPress, kWind, kGusts, kDir };
77 float fMagicWeatherData[7];
78
79 vector<float> fFeedbackCalibration;
80 vector<float> fBiasControlVoltageVec;
81
82 float fBiasControlVoltageMed;
83 float fBiasControlCurrentMed;
84 float fBiasControlCurrentMax;
85
86 float fDriveControlPointingZd;
87 string fDriveControlPointingAz;
88 float fDriveControlTrackingDev;
89 string fDriveControlSourceName;
90
91 // -------------------------------------------------------------------
92
93 pair<Time, int> GetNewState(DimStampedInfo &info) const
94 {
95 const bool disconnected = info.getSize()==0;
96
97 // Make sure getTimestamp is called _before_ getTimestampMillisecs
98 const int tsec = info.getTimestamp();
99 const int tms = info.getTimestampMillisecs();
100
101 return make_pair(Time(tsec, tms*1000),
102 disconnected ? -2 : info.getQuality());
103 }
104
105 bool UpdateState(DimInfo *curr, DimStampedInfo &service, pair<Time,int> &rc)
106 {
107 if (curr!=&service)
108 return false;
109
110 rc = GetNewState(service);
111 return true;
112 }
113
114 bool HandleService(DimInfo *curr, const DimInfo &service, void (StateMachineSmartFACT::*handle)(const DimData &))
115 {
116 if (curr!=&service)
117 return false;
118
119 (this->*handle)(DimData(curr));
120 return true;
121 }
122
123
124 bool CheckDataSize(const DimData &d, const char *name, size_t size)
125 {
126 if (d.data.size()==size)
127 return true;
128
129 ostringstream msg;
130 msg << name << " - Received service has " << d.data.size() << " bytes, but expected " << size << ".";
131 Warn(msg);
132 return false;
133 }
134
135
136 // -------------------------------------------------------------------
137
138 void HandleMagicWeatherData(const DimData &d)
139 {
140 if (!CheckDataSize(d, "MagicWeather:Data", 7*4+2))
141 return;
142
143 // FIXME: Check size (7*4+2)
144
145 //const uint16_t status = d.get<uint16_t>();
146 memcpy(fMagicWeatherData, d.ptr<float>(2), 7*sizeof(float));
147
148 ostringstream out;
149 out << uint64_t(d.time.UnixTime()*1000) << '\n';
150
151 for (int i=0; i<7; i++)
152 out << "#ffffff\t" << fMagicWeatherData[i] << '\n';
153
154 ofstream fout("www/magicweather.txt");
155 fout << out.str();
156 }
157
158 void HandleDrivePointing(const DimData &d)
159 {
160 if (!CheckDataSize(d, "DriveControl:Pointing", 7*4+2))
161 return;
162
163 fDriveControlPointingZd = d.get<double>();
164
165 const double az = d.get<double>(8);
166
167 static const char *dir[] =
168 {
169 "N", "NNE", "NE", "ENE",
170 "E", "ESE", "SE", "SSE",
171 "S", "SSW", "SW", "WSW",
172 "W", "WNW", "NW", "NNW"
173 };
174
175 const uint16_t i = uint16_t(floor(fmod(az+11.25, 360)/22));
176 fDriveControlPointingAz = dir[i];
177
178 ostringstream out;
179 out << uint64_t(d.time.UnixTime()*1000) << '\n';
180
181 out << setprecision(5);
182 out << fDriveControlPointingZd << '\n';
183 out << az << '\n';
184
185 ofstream fout("www/drive-pointing.txt");
186 fout << out.str();
187 }
188
189 void HandleDriveTracking(const DimData &d)
190 {
191 if (!CheckDataSize(d, "DriveControl:Tracking", 7*4+2))
192 return;
193
194 const double zd = d.get<double>(4*8) * M_PI / 180;
195 const double dzd = d.get<double>(6*8) * M_PI / 180;
196 const double daz = d.get<double>(7*8) * M_PI / 180;
197
198 // Correct:
199 // const double d = cos(del) - sin(zd+dzd)*sin(zd)*(1.-cos(daz));
200
201 // Simplified:
202 const double dev = cos(dzd) - sin(zd)*sin(zd)*(1.-cos(daz));
203 fDriveControlTrackingDev = acos(dev) * 180 / M_PI;
204 }
205
206 void HandleDriveSource(const DimData &d)
207 {
208 if (!CheckDataSize(d, "DriveControl:Source", 7*4+2))
209 return;
210
211 const double *ptr = d.ptr<double>();
212
213 const double ra = ptr[0]; // Ra[h]
214 const double dec = ptr[1]; // Dec[deg]
215 const double woff = ptr[4]; // Wobble offset [deg]
216 const double wang = ptr[5]; // Wobble angle [deg]
217
218 fDriveControlSourceName = d.ptr<char>(6*8);
219
220 ostringstream out;
221 out << uint64_t(d.time.UnixTime()*1000) << '\n';
222
223 out << fDriveControlSourceName << '\n';
224 out << setprecision(5);
225 out << "#ffffff\t" << ra << '\n';
226 out << "#ffffff\t" << dec << '\n';
227 out << setprecision(3);
228 out << "#ffffff\t" << woff << '\n';
229 out << "#ffffff\t" << wang << '\n';
230
231 ofstream fout("www/drive.txt");
232 fout << out.str();
233 }
234
235 void HandleFeedbackCalibration(const DimData &d)
236 {
237 if (!CheckDataSize(d, "Feedback:Calibration", 3*4*416))
238 {
239 fFeedbackCalibration.clear();
240 return;
241 }
242
243 const float *ptr = d.ptr<float>();
244 fFeedbackCalibration.assign(ptr+2*416, ptr+3*416);
245 }
246
247 void HandleBiasControlVoltage(const DimData &d)
248 {
249 if (!CheckDataSize(d, "BiasControl:Voltage", 1664))
250 {
251 fBiasControlVoltageVec.clear();
252 return;
253 }
254
255 fBiasControlVoltageVec.assign(d.ptr<float>(), d.ptr<float>()+320);
256
257 vector<float> v(fBiasControlVoltageVec);
258 sort(v.begin(), v.end());
259
260 fBiasControlVoltageMed = (v[159]+v[160])/2;
261
262 const char *ptr = d.ptr<char>();
263
264 ofstream fout("www/biascontrol-voltage.bin");
265 fout.write(ptr, 320*sizeof(float));
266 }
267
268 void HandleBiasControlCurrent(const DimData &d)
269 {
270 if (!CheckDataSize(d, "BiasControl:Current", 832))
271 return;
272
273 vector<float> v(320);
274 for (int i=0; i<320; i++)
275 v[i] = d.ptr<uint16_t>()[i];
276
277 if (fFeedbackCalibration.size()>0 && fBiasControlVoltageVec.size()>0)
278 for (int i=0; i<320; i++)
279 v[i] -= fBiasControlVoltageVec[i]/fFeedbackCalibration[i]*1e6;
280
281 vector<uint8_t> val(160, 0);
282 for (int i=0; i<160; i++)
283 {
284 const float I = max(v[i*2], v[i*2+1]);
285
286 float range = nearbyint(128*I/1000); // [0, 1000uA]
287 if (range>127)
288 range=127;
289 if (range<0)
290 range=0;
291 val[i] = (uint8_t)range;
292 }
293
294 sort(v.begin(), v.end());
295
296 // Exclude the three crazy channels
297 fBiasControlCurrentMed = (v[159]+v[160])/2 * 5000./4096;
298 fBiasControlCurrentMax = v[316] * 5000./4096;
299
300 const char *ptr = reinterpret_cast<char*>(val.data());
301
302 ofstream fout("www/biascontrol-current.bin");
303 fout.write(ptr, 160*sizeof(uint8_t));
304 }
305
306 uint8_t fEventCounter;
307
308 void HandleFadControlEventData(const DimData &d)
309 {
310 if (!CheckDataSize(d, "FadControl:EventData", 23040))
311 return;
312
313 if (fEventCounter++%30)
314 return;
315
316 //const float *avg = d.ptr<float>();
317 //const float *rms = d.ptr<float>(1440*sizeof(float));
318 const float *max = d.ptr<float>(1440*sizeof(float)*2);
319 //const float *pos = d.ptr<float>(1440*sizeof(float)*3);
320
321 vector<float> dat(160, 0);
322 for (int i=0; i<1440; i++)
323 {
324 const int idx = fPixelMap.index(i).hw()/9;
325 if (max[i]>dat[idx])
326 dat[idx]=max[i];
327 //dat[idx] += max[i];
328 }
329
330 vector<uint8_t> val(160, 0);
331 for (int i=0; i<160; i++)
332 {
333 float range = nearbyint(64*dat[i]/2000)+64; // [-2V; 2V]
334 if (range>127)
335 range=127;
336 if (range<0)
337 range=0;
338 val[i] = (uint8_t)range;
339 }
340
341 const char *ptr = reinterpret_cast<char*>(val.data());
342
343 ofstream fout("www/fadcontrol-eventdata.bin");
344 fout.write(ptr, 160*sizeof(int8_t));
345 }
346
347 // -------------------------------------------------------------------
348
349 void infoHandler()
350 {
351 DimInfo *curr = getInfo(); // get current DimInfo address
352 if (!curr)
353 return;
354
355 if (HandleService(curr, fDimMagicWeatherData, &StateMachineSmartFACT::HandleMagicWeatherData))
356 return;
357 if (HandleService(curr, fDimFeedbackCalibration, &StateMachineSmartFACT::HandleFeedbackCalibration))
358 return;
359 if (HandleService(curr, fDimBiasControlVoltage, &StateMachineSmartFACT::HandleBiasControlVoltage))
360 return;
361 if (HandleService(curr, fDimBiasControlCurrent, &StateMachineSmartFACT::HandleBiasControlCurrent))
362 return;
363 if (HandleService(curr, *fDimFadControlEventData, &StateMachineSmartFACT::HandleFadControlEventData))
364 return;
365
366 if (UpdateState(curr, fDimMagicWeather, fStatusMagicWeather))
367 return;
368 if (UpdateState(curr, fDimFeedback, fStatusFeedback))
369 return;
370 if (UpdateState(curr, fDimBiasControl, fStatusBiasControl))
371 return;
372 if (UpdateState(curr, fDimFadControl, fStatusFadControl))
373 return;
374
375 if (curr==&fDim)
376 {
377 fStatusDim = GetNewState(fDim);
378 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
379 return;
380 }
381 }
382
383 bool CheckEventSize(size_t has, const char *name, size_t size)
384 {
385 if (has==size)
386 return true;
387
388 ostringstream msg;
389 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
390 Fatal(msg);
391 return false;
392 }
393
394 void PrintState(const pair<Time,int> &state, const char *server)
395 {
396 const State rc = fNetwork.GetState(server, state.second);
397
398 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
399 Out() << kBold << server << ": ";
400 if (rc.index==-2)
401 {
402 Out() << kReset << "Offline" << endl;
403 return;
404 }
405 Out() << rc.name << "[" << rc.index << "]";
406 Out() << kReset << " - " << kBlue << rc.comment << endl;
407 }
408
409 int Print()
410 {
411 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
412 Out() << kBold << "DIM_DNS: ";
413 if (fStatusDim.second==0)
414 Out() << "Offline" << endl;
415 else
416 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
417
418 PrintState(fStatusMagicWeather, "MAGIC_WEATHER");
419 PrintState(fStatusDriveControl, "DRIVE_CONTROL");
420 PrintState(fStatusFeedback, "FEEDBACK");
421 PrintState(fStatusBiasControl, "BIAS_CONTROL");
422 PrintState(fStatusFadControl, "FAD_CONTROL");
423
424 return GetCurrentState();
425 }
426
427 int Execute()
428 {
429 // Dispatch (execute) at most one handler from the queue. In contrary
430 // to run_one(), it doesn't wait until a handler is available
431 // which can be dispatched, so poll_one() might return with 0
432 // handlers dispatched. The handlers are always dispatched/executed
433 // synchronously, i.e. within the call to poll_one()
434 //poll_one();
435
436 if (fStatusDim.second==0)
437 return kStateDimNetworkNA;
438
439 Time now;
440 if (now-fLastUpdate<boost::posix_time::seconds(1))
441 return kStateRunning;
442
443 fLastUpdate=now;
444
445 ostringstream out;
446 out << uint64_t(nearbyint(now.UnixTime()*1000)) << '\n';
447 out << setprecision(3);
448
449 // -------------- System status --------------
450 out << "n/a\n";
451
452 const static string kWhite = "#ffffff";
453 const static string kYellow = "#fffff0";
454 const static string kRed = "#fff8f0";
455 const static string kGreen = "#f0fff0";
456 const static string kBlue = "#f0f0ff";
457
458 // ------------------ Drive -----------------
459 if (fStatusDriveControl.second>=5) // Armed, Moving, Tracking
460 {
461 const State rc = fNetwork.GetState("DRIVE_CONTROL", fStatusDriveControl.second);
462 out << kWhite << "\t";
463 out << rc.name << '\t';
464 out << fDriveControlPointingZd << '\t';
465 out << fDriveControlPointingAz << '\t';
466 out << fDriveControlTrackingDev << '\t';
467 out << fDriveControlSourceName << '\n';
468 }
469 else
470 out << kWhite << "\t\t\t\t\t\n";
471
472 // --------------- MagicWeather -------------
473 if (fStatusMagicWeather.second==3)
474 {
475 const float diff = fMagicWeatherData[kTemp]-fMagicWeatherData[kDew];
476 string col1 = kRed;
477 if (diff>0.3)
478 col1 = kYellow;
479 if (diff>0.7)
480 col1 = kGreen;
481
482 const float wind = fMagicWeatherData[kGusts];
483 string col2 = kGreen;
484 if (wind>35)
485 col2 = kYellow;
486 if (wind>50)
487 col2 = kRed;
488
489 out << col1 << "\t";
490 out << fMagicWeatherData[kTemp] << '\t';
491 out << fMagicWeatherData[kDew] << '\n';
492 out << col2 << "\t";
493 out << fMagicWeatherData[kGusts] << '\n';
494 }
495 else
496 out << kWhite << "\t\t\n\n";
497
498 // --------------- BiasControl -------------
499 if (fStatusBiasControl.second==5 || // Ramping
500 fStatusBiasControl.second==7 || // On
501 fStatusBiasControl.second==9) // Off
502 {
503 string col = fBiasControlVoltageMed>3?kGreen:kWhite;
504 if (fBiasControlCurrentMax>280)
505 col = kYellow;
506 if (fBiasControlCurrentMax>350)
507 col = kRed;
508
509 if (fFeedbackCalibration.size()==0)
510 col = kBlue;
511
512 out << col << "\t";
513 out << fBiasControlCurrentMed << '\t';
514 out << fBiasControlCurrentMax << '\t';
515 out << fBiasControlVoltageMed << '\n';
516 }
517 else
518 out << kWhite << "\t\t\t\n";
519
520
521 // ------------------------------------------
522 ofstream fout("www/fact.txt");
523 fout << out.str();
524
525 return kStateRunning;
526 }
527
528public:
529 StateMachineSmartFACT(ostream &out=cout) : StateMachineDim(out, "SMART_FACT"),
530 fStatusDim (make_pair(Time(), -2)),
531 fStatusDriveControl(make_pair(Time(), -2)),
532 fStatusMagicWeather(make_pair(Time(), -2)),
533 fStatusFeedback (make_pair(Time(), -2)),
534 fStatusBiasControl (make_pair(Time(), -2)),
535 fStatusFadControl (make_pair(Time(), -2)),
536 //---
537 fDim ("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
538 //---
539 fDimDriveControl ("DRIVE_CONTROL/STATE", (void*)NULL, 0, this),
540 fDimDriveControlPointing("DRIVE_CONTROL/POINTING_POSITION", (void*)NULL, 0, this),
541 fDimDriveControlTracking("DRIVE_CONTROL/TRACKING_POSITION", (void*)NULL, 0, this),
542 fDimDriveControlSource ("DRIVE_CONTROL/SOURCE_POSITION", (void*)NULL, 0, this),
543 //---
544 fDimMagicWeather ("MAGIC_WEATHER/STATE", (void*)NULL, 0, this),
545 fDimMagicWeatherData ("MAGIC_WEATHER/DATA", (void*)NULL, 0, this),
546 //---
547 fDimFeedback ("FEEDBACK/STATE", (void*)NULL, 0, this),
548 fDimFeedbackCalibration ("FEEDBACK/CALIBRATION", (void*)NULL, 0, this),
549 //---
550 fDimBiasControl ("BIAS_CONTROL/STATE", (void*)NULL, 0, this),
551 fDimBiasControlVoltage ("BIAS_CONTROL/VOLTAGE", (void*)NULL, 0, this),
552 fDimBiasControlCurrent ("BIAS_CONTROL/CURRENT", (void*)NULL, 0, this),
553 //---
554 fDimFadControl ("FAD_CONTROL/STATE", (void*)NULL, 0, this),
555 fDimFadControlEventData(0),
556 //---
557 fEventCounter(0)
558 {
559 // State names
560 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
561 "The Dim DNS is not reachable.");
562
563 AddStateName(kStateRunning, "Running", "");
564
565 // Verbosity commands
566// AddEvent("SET_VERBOSE", "B:1")
567// (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
568// ("set verbosity state"
569// "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
570
571 AddEvent("PRINT")
572 (bind(&StateMachineSmartFACT::Print, this))
573 ("");
574 }
575 ~StateMachineSmartFACT()
576 {
577 delete fDimFadControlEventData;
578 }
579 int EvalOptions(Configuration &conf)
580 {
581 if (!fPixelMap.Read(conf.Get<string>("pixel-map-file")))
582 {
583 Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
584 return 1;
585 }
586
587 // Pixel map is needed to deal with this service
588 fDimFadControlEventData=new DimStampedInfo("FAD_CONTROL/EVENT_DATA", (void*)NULL, 0, this);
589
590 return -1;
591 }
592};
593
594// ------------------------------------------------------------------------
595
596#include "Main.h"
597
598template<class T>
599int RunShell(Configuration &conf)
600{
601 return Main::execute<T, StateMachineSmartFACT>(conf);
602}
603
604void SetupConfiguration(Configuration &conf)
605{
606 po::options_description control("Smart FACT");
607 control.add_options()
608 ("pixel-map-file", var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
609 ;
610
611 conf.AddOptions(control);
612}
613
614/*
615 Extract usage clause(s) [if any] for SYNOPSIS.
616 Translators: "Usage" and "or" here are patterns (regular expressions) which
617 are used to match the usage synopsis in program output. An example from cp
618 (GNU coreutils) which contains both strings:
619 Usage: cp [OPTION]... [-T] SOURCE DEST
620 or: cp [OPTION]... SOURCE... DIRECTORY
621 or: cp [OPTION]... -t DIRECTORY SOURCE...
622 */
623void PrintUsage()
624{
625 cout <<
626 "SmartFACT is a tool writing the files needed for the SmartFACT web interface.\n"
627 "\n"
628 "The default is that the program is started without user intercation. "
629 "All actions are supposed to arrive as DimCommands. Using the -c "
630 "option, a local shell can be initialized. With h or help a short "
631 "help message about the usuage can be brought to the screen.\n"
632 "\n"
633 "Usage: smartfact [-c type] [OPTIONS]\n"
634 " or: smartfact [OPTIONS]\n";
635 cout << endl;
636}
637
638void PrintHelp()
639{
640 Main::PrintHelp<StateMachineSmartFACT>();
641
642 /* Additional help text which is printed after the configuration
643 options goes here */
644
645 /*
646 cout << "bla bla bla" << endl << endl;
647 cout << endl;
648 cout << "Environment:" << endl;
649 cout << "environment" << endl;
650 cout << endl;
651 cout << "Examples:" << endl;
652 cout << "test exam" << endl;
653 cout << endl;
654 cout << "Files:" << endl;
655 cout << "files" << endl;
656 cout << endl;
657 */
658}
659
660int main(int argc, const char* argv[])
661{
662 Configuration conf(argv[0]);
663 conf.SetPrintUsage(PrintUsage);
664 Main::SetupConfiguration(conf);
665 SetupConfiguration(conf);
666
667 if (!conf.DoParse(argc, argv, PrintHelp))
668 return -1;
669
670 //try
671 {
672 // No console access at all
673 if (!conf.Has("console"))
674 {
675// if (conf.Get<bool>("no-dim"))
676// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
677// else
678 return RunShell<LocalStream>(conf);
679 }
680 // Cosole access w/ and w/o Dim
681/* if (conf.Get<bool>("no-dim"))
682 {
683 if (conf.Get<int>("console")==0)
684 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
685 else
686 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
687 }
688 else
689*/ {
690 if (conf.Get<int>("console")==0)
691 return RunShell<LocalShell>(conf);
692 else
693 return RunShell<LocalConsole>(conf);
694 }
695 }
696 /*catch (std::exception& e)
697 {
698 cerr << "Exception: " << e.what() << endl;
699 return -1;
700 }*/
701
702 return 0;
703}
Note: See TracBrowser for help on using the repository browser.