source: branches/FACT++_lidctrl_new_eth/src/gpsctrl.cc@ 19128

Last change on this file since 19128 was 18611, checked in by tbretz, 8 years ago
FITS column description was too long
File size: 17.6 KB
Line 
1#include "FACT.h"
2#include "Dim.h"
3#include "Event.h"
4#include "StateMachineDim.h"
5#include "StateMachineAsio.h"
6#include "Connection.h"
7#include "LocalControl.h"
8#include "Configuration.h"
9#include "Console.h"
10
11#include "tools.h"
12
13#include "HeadersGPS.h"
14
15namespace ba = boost::asio;
16namespace bs = boost::system;
17namespace dummy = ba::placeholders;
18
19using namespace std;
20
21class ConnectionGPS : public Connection
22{
23protected:
24 virtual void Update(const GPS::NEMA &)
25 {
26 }
27
28private:
29 bool fIsVerbose;
30
31 Time fLastReport;
32
33 int fState;
34
35 float ConvLngLat(const string &l) const
36 {
37 const double lf = stof(l);
38 const uint32_t li = stoi(l);
39
40 const double min = fmod(lf, 100);
41 const double deg = li/100;
42
43 return deg + min/60;
44 }
45
46 float ConvTm(const string &t) const
47 {
48 const double tf = stof(t);
49 const uint32_t ti = stoi(t);
50
51 const double h = ti/10000;
52 const double m = (ti/100)%100;
53 const double s = fmod(tf, 100);
54
55 return h/24 + m/1440 + s/86400;
56 }
57
58 bool ParseAnswer(const string &buffer)
59 {
60 if (buffer=="Invalid command, type help")
61 {
62 Error("Command was ignored by GPS.");
63 return false;
64 }
65
66 // answer to get_status or veto_[on|off|60]
67 if (buffer=="veto_60" || buffer=="veto 60 now on")
68 {
69 if (fState!=GPS::State::kLocked)
70 fState = GPS::State::kEnabled;
71 PostMessage(string("get_nema\r\n"), 10);
72 return true;
73 }
74 if (buffer=="veto_on" || buffer=="veto now on")
75 {
76 fState = GPS::State::kDisabled;
77 PostMessage(string("get_nema\r\n"), 10);
78 return true;
79 }
80 /*
81 if (buffer=="veto_off" || buffer=="veto now off")
82 {
83 fState = GPS::State::kVetoOff;
84 PostMessage(string("get_nema\r\n"), 10);
85 return true;
86 }*/
87
88 // answer to get_nema
89 if (buffer[0]=='$')
90 {
91 /*
92 1 = UTC of Position
93 2 = Latitude
94 3 = N or S
95 4 = Longitude
96 5 = E or W
97 6 = GPS quality indicator (0=invalid; 1=GPS fix;
98 2=Diff. GPS fix)
99 7 = Number of satellites in use [not those in view]
100 8 = Horizontal dilution of position
101 9 = Antenna altitude above/below mean sea level (geoid)
102 10 = Meters (Antenna height unit)
103 11 = Geoidal separation (Diff. between WGS-84 earth ellipsoid
104 and mean sea level. -=geoid is below WGS-84 ellipsoid)
105 12 = Meters (Units of geoidal separation)
106 13 = Age in seconds since last update from diff.
107 reference station
108 14 = Diff. reference station ID#
109 */
110
111 const vector<string> cs = Tools::Split(buffer, "$*");
112 if (cs.size()!=3)
113 throw runtime_error("syntax error");
114
115 // check checksum
116 uint8_t c = cs[1][0];
117 for (size_t i=1; i<cs[1].size(); i++)
118 c ^= cs[1][i];
119
120 stringstream ss;
121 ss << std::hex << cs[2];
122
123 unsigned int x;
124 ss >> x;
125
126 if (x!=c)
127 throw runtime_error("checksum error");
128
129 // interpret contents
130 const vector<string> dat = Tools::Split(cs[1], ",");
131 if (dat.size()!=15)
132 throw runtime_error("size mismatch");
133 if (dat[0]!="GPGGA")
134 throw runtime_error("type mismatch");
135 if (dat[5]!="W" && dat[5]!="E")
136 throw runtime_error("longitude type unknown");
137 if (dat[10]!="M")
138 throw runtime_error("height unit unknown");
139 if (dat[12]!="M")
140 throw runtime_error("hdop unit unknown");
141 if (!dat[13].empty())
142 throw runtime_error("unexpected data at position 13");
143 if (dat[14]!="0000")
144 throw runtime_error("unexpected data at position 14");
145
146 GPS::NEMA nema;
147 nema.time = ConvTm(dat[1]);
148 nema.lat = dat[3]=="N" ? ConvLngLat(dat[2]) : -ConvLngLat(dat[3]);
149 nema.lng = dat[5]=="W" ? ConvLngLat(dat[4]) : -ConvLngLat(dat[4]);
150 nema.qos = stoi(dat[6]);
151 nema.count = stoi(dat[7]);
152 nema.hdop = stof(dat[8]);
153 nema.height = stof(dat[9]);
154 nema.geosep = stof(dat[11]);
155
156 if (fabs(nema.time-fmod(Time().Mjd(), 1))*24*3600>5)
157 {
158 Error("Time mismatch: GPS time deviates from PC time by more than 5s");
159 return false;
160 }
161
162 if (fState>=GPS::State::kEnabled)
163 fState = nema.qos==1 ? GPS::State::kLocked : GPS::State::kEnabled;
164
165 Update(nema);
166
167 return true;
168 }
169
170 return false;
171 }
172
173 void HandleRead(const boost::system::error_code& err, size_t bytes_received)
174 {
175 // Do not schedule a new read if the connection failed.
176 if (bytes_received==0 || err)
177 {
178 if (err==ba::error::eof)
179 Warn("Connection closed by remote host.");
180
181 // 107: Transport endpoint is not connected (bs::error_code(107, bs::system_category))
182 // 125: Operation canceled
183 if (err && err!=ba::error::eof && // Connection closed by remote host
184 err!=ba::error::basic_errors::not_connected && // Connection closed by remote host
185 err!=ba::error::basic_errors::operation_aborted) // Connection closed by us
186 {
187 ostringstream str;
188 str << "Reading from " << URL() << ": " << err.message() << " (" << err << ")";// << endl;
189 Error(str);
190 }
191 PostClose(err!=ba::error::basic_errors::operation_aborted);
192 return;
193 }
194
195 istream is(&fBuffer);
196
197 string buffer;
198 if (!getline(is, buffer, '\n'))
199 {
200 Error("Received message does not contain \\n... closing connection.");
201 PostClose(false);
202 return;
203 }
204 buffer = buffer.substr(0, buffer.size()-1);
205
206 if (fIsVerbose)
207 Out() << buffer << endl;
208
209 try
210 {
211 if (!ParseAnswer(buffer))
212 {
213 Error("Received: "+buffer);
214 PostClose(false);
215 return;
216 }
217 }
218 catch (const exception &e)
219 {
220 Error("Parsing NEMA message failed ["+string(e.what())+"]");
221 Error("Received: "+buffer);
222 PostClose(false);
223 return;
224 }
225
226 fLastReport = Time();
227 StartReadReport();
228 }
229
230 boost::asio::streambuf fBuffer;
231
232 void StartReadReport()
233 {
234 async_read_until(*this, fBuffer, '\n',
235 boost::bind(&ConnectionGPS::HandleRead, this,
236 dummy::error, dummy::bytes_transferred));
237 }
238
239 boost::asio::deadline_timer fKeepAlive;
240
241 void HandleRequest(const bs::error_code &error)
242 {
243 // 125: Operation canceled (bs::error_code(125, bs::system_category))
244 if (error && error!=ba::error::basic_errors::operation_aborted)
245 {
246 ostringstream str;
247 str << "Write timeout of " << URL() << ": " << error.message() << " (" << error << ")";// << endl;
248 Error(str);
249
250 PostClose(false);
251 return;
252 }
253
254 if (!is_open())
255 {
256 // For example: Here we could schedule a new accept if we
257 // would not want to allow two connections at the same time.
258 PostClose(true);
259 return;
260 }
261
262 // Check whether the deadline has passed. We compare the deadline
263 // against the current time since a new asynchronous operation
264 // may have moved the deadline before this actor had a chance
265 // to run.
266 if (fKeepAlive.expires_at() > ba::deadline_timer::traits_type::now())
267 return;
268
269 PostMessage(string("get_status\r\n"), 12);
270 Request();
271 }
272
273
274private:
275 // This is called when a connection was established
276 void ConnectionEstablished()
277 {
278 fState = GPS::State::kConnected;
279
280 StartReadReport();
281 Request(true);
282 }
283
284public:
285 static const uint16_t kMaxAddr;
286
287public:
288 ConnectionGPS(ba::io_service& ioservice, MessageImp &imp) : Connection(ioservice, imp()),
289 fIsVerbose(true), fLastReport(Time::none), fKeepAlive(ioservice)
290 {
291 SetLogStream(&imp);
292 }
293
294 void SetVerbose(bool b)
295 {
296 fIsVerbose = b;
297 Connection::SetVerbose(b);
298 }
299
300 void Request(bool immediate=false)
301 {
302 double mjd = Time().Mjd();
303
304 if (!immediate)
305 mjd = (ceil(mjd*24*60+0.01)+0.5)/(24*60);
306
307 fKeepAlive.expires_at(Time(mjd));
308 fKeepAlive.async_wait(boost::bind(&ConnectionGPS::HandleRequest,
309 this, dummy::error));
310 }
311
312 int GetState() const
313 {
314 if (!IsConnected())
315 return GPS::State::kDisconnected;
316
317 if (fState!=GPS::State::kConnected && fLastReport+boost::posix_time::seconds(105) < Time())
318 return StateMachineImp::kSM_Error;
319
320 return fState;
321 }
322};
323
324const uint16_t ConnectionGPS::kMaxAddr = 0xfff;
325
326// ------------------------------------------------------------------------
327
328#include "DimDescriptionService.h"
329
330class ConnectionDimWeather : public ConnectionGPS
331{
332private:
333 DimDescribedService fDim;
334
335public:
336 ConnectionDimWeather(ba::io_service& ioservice, MessageImp &imp) :
337 ConnectionGPS(ioservice, imp),
338 fDim("GPS_CONTROL/NEMA", "F:1;F:1;F:1;F:1;F:1;F:1;S:1;S:1",
339 "NEMA message from the GPS module"
340 "|time[utc]:Time of day as fraction of day (UTC)"
341 "|lat[deg]:Latitude"
342 "|long[deg]:Longitude"
343 "|hdop:Horizontal delution of precision"
344 "|height[m]:Antenna altitude above mean sea level (geoid)"
345 "|geosep[m]:Geoidal sep.(Diff. between WGS-84 earth ellipsoid and mean sea lvl)"
346 "|count:Number of satellites in use (not those in view)"
347 "|quality:GPS quality indicator (see Venus manual)")
348 {
349 }
350 void Update(const GPS::NEMA &nema)
351 {
352 fDim.Update(nema);
353 }
354};
355
356// ------------------------------------------------------------------------
357
358template <class T, class S>
359class StateMachineGPSControl : public StateMachineAsio<T>
360{
361private:
362 S fGPS;
363 Time fLastCommand;
364
365 bool CheckEventSize(size_t has, const char *name, size_t size)
366 {
367 if (has==size)
368 return true;
369
370 ostringstream msg;
371 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
372 T::Fatal(msg);
373 return false;
374 }
375
376 int Disconnect()
377 {
378 // Close all connections
379 fGPS.PostClose(false);
380
381 return T::GetCurrentState();
382 }
383
384 int Reconnect(const EventImp &evt)
385 {
386 // Close all connections to supress the warning in SetEndpoint
387 fGPS.PostClose(false);
388
389 // Now wait until all connection have been closed and
390 // all pending handlers have been processed
391 ba::io_service::poll();
392
393 if (evt.GetBool())
394 fGPS.SetEndpoint(evt.GetString());
395
396 // Now we can reopen the connection
397 fGPS.PostClose(true);
398
399 return T::GetCurrentState();
400 }
401
402 int SetVerbosity(const EventImp &evt)
403 {
404 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
405 return T::kSM_FatalError;
406
407 fGPS.SetVerbose(evt.GetBool());
408
409 return T::GetCurrentState();
410 }
411
412 int Send(const string &cmd)
413 {
414 const string tx = cmd+"\r\n";
415 fGPS.PostMessage(tx, tx.size());
416 return T::GetCurrentState();
417 }
418
419 int SendCommand(const EventImp &evt)
420 {
421 return Send(evt.GetString());
422 }
423
424 int Execute()
425 {
426 return fGPS.GetState();
427 }
428
429
430public:
431 StateMachineGPSControl(ostream &out=cout) :
432 StateMachineAsio<T>(out, "GPS_CONTROL"), fGPS(*this, *this)
433 {
434 // State names
435 T::AddStateName(GPS::State::kDisconnected, "Disconnected",
436 "No connection to web-server could be established recently");
437
438 T::AddStateName(GPS::State::kConnected, "Connected",
439 "Connection established, but status still not known");
440
441 T::AddStateName(GPS::State::kDisabled, "Disabled",
442 "Veto is on, no trigger will be emitted");
443
444 T::AddStateName(GPS::State::kEnabled, "Enabled",
445 "System enabled, waiting for satellites");
446
447 T::AddStateName(GPS::State::kLocked, "Locked",
448 "One trigger per second will be send, but the one at the exact minute is vetoed");
449
450 // Commands
451 T::AddEvent("SEND_COMMAND", "C")
452 (bind(&StateMachineGPSControl::SendCommand, this, placeholders::_1))
453 ("Send command to GPS");
454
455 // Verbosity commands
456 T::AddEvent("SET_VERBOSE", "B")
457 (bind(&StateMachineGPSControl::SetVerbosity, this, placeholders::_1))
458 ("set verbosity state"
459 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
460
461 T::AddEvent("ENABLE")
462 (bind(&StateMachineGPSControl::Send, this, "veto_60"))
463 ("Enable trigger signal once a second vetoed at every exact minute");
464
465 T::AddEvent("DISABLE")
466 (bind(&StateMachineGPSControl::Send, this, "veto_on"))
467 ("Diable trigger output");
468
469 // Conenction commands
470 T::AddEvent("DISCONNECT")
471 (bind(&StateMachineGPSControl::Disconnect, this))
472 ("disconnect from ethernet");
473
474 T::AddEvent("RECONNECT", "O")
475 (bind(&StateMachineGPSControl::Reconnect, this, placeholders::_1))
476 ("(Re)connect ethernet connection to GPS, a new address can be given"
477 "|[host][string]:new ethernet address in the form <host:port>");
478
479 }
480
481 int EvalOptions(Configuration &conf)
482 {
483 fGPS.SetVerbose(!conf.Get<bool>("quiet"));
484 fGPS.SetDebugTx(conf.Get<bool>("debug-tx"));
485 fGPS.SetEndpoint(conf.Get<string>("addr"));
486 fGPS.StartConnect();
487
488 return -1;
489 }
490};
491
492// ------------------------------------------------------------------------
493
494#include "Main.h"
495
496
497template<class T, class S, class R>
498int RunShell(Configuration &conf)
499{
500 return Main::execute<T, StateMachineGPSControl<S, R>>(conf);
501}
502
503void SetupConfiguration(Configuration &conf)
504{
505 po::options_description control("GPS control");
506 control.add_options()
507 ("no-dim,d", po_switch(), "Disable dim services")
508 ("addr,a", var<string>("gps:23"), "Network address of the lid controling Arduino including port")
509 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
510 ("debug-tx", po_bool(), "Enable debugging of ethernet transmission.")
511 ;
512
513 conf.AddOptions(control);
514}
515
516/*
517 Extract usage clause(s) [if any] for SYNOPSIS.
518 Translators: "Usage" and "or" here are patterns (regular expressions) which
519 are used to match the usage synopsis in program output. An example from cp
520 (GNU coreutils) which contains both strings:
521 Usage: cp [OPTION]... [-T] SOURCE DEST
522 or: cp [OPTION]... SOURCE... DIRECTORY
523 or: cp [OPTION]... -t DIRECTORY SOURCE...
524 */
525void PrintUsage()
526{
527 cout <<
528 "The gpsctrl is an interface to the GPS hardware.\n"
529 "\n"
530 "The default is that the program is started without user intercation. "
531 "All actions are supposed to arrive as DimCommands. Using the -c "
532 "option, a local shell can be initialized. With h or help a short "
533 "help message about the usuage can be brought to the screen.\n"
534 "\n"
535 "Usage: gpsctrl [-c type] [OPTIONS]\n"
536 " or: gpsctrl [OPTIONS]\n";
537 cout << endl;
538}
539
540void PrintHelp()
541{
542// Main::PrintHelp<StateMachineFTM<StateMachine, ConnectionFTM>>();
543
544 /* Additional help text which is printed after the configuration
545 options goes here */
546
547 /*
548 cout << "bla bla bla" << endl << endl;
549 cout << endl;
550 cout << "Environment:" << endl;
551 cout << "environment" << endl;
552 cout << endl;
553 cout << "Examples:" << endl;
554 cout << "test exam" << endl;
555 cout << endl;
556 cout << "Files:" << endl;
557 cout << "files" << endl;
558 cout << endl;
559 */
560}
561
562int main(int argc, const char* argv[])
563{
564 Configuration conf(argv[0]);
565 conf.SetPrintUsage(PrintUsage);
566 Main::SetupConfiguration(conf);
567 SetupConfiguration(conf);
568
569 if (!conf.DoParse(argc, argv, PrintHelp))
570 return 127;
571
572 // No console access at all
573 if (!conf.Has("console"))
574 {
575 if (conf.Get<bool>("no-dim"))
576 return RunShell<LocalStream, StateMachine, ConnectionGPS>(conf);
577 else
578 return RunShell<LocalStream, StateMachineDim, ConnectionDimWeather>(conf);
579 }
580 // Cosole access w/ and w/o Dim
581 if (conf.Get<bool>("no-dim"))
582 {
583 if (conf.Get<int>("console")==0)
584 return RunShell<LocalShell, StateMachine, ConnectionGPS>(conf);
585 else
586 return RunShell<LocalConsole, StateMachine, ConnectionGPS>(conf);
587 }
588 else
589 {
590 if (conf.Get<int>("console")==0)
591 return RunShell<LocalShell, StateMachineDim, ConnectionDimWeather>(conf);
592 else
593 return RunShell<LocalConsole, StateMachineDim, ConnectionDimWeather>(conf);
594 }
595
596 return 0;
597}
Note: See TracBrowser for help on using the repository browser.