source: branches/fscctrl_safety_limits/src/gpsctrl.cc@ 18795

Last change on this file since 18795 was 17389, checked in by tbretz, 12 years ago
Updated Dim service description.
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 (!is_open())
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 separation (Diff. between WGS-84 earth ellipsoid and mean sea level)"
346 "|count:Number of satellites in use (not those in view)"
347 "|quality:GPS quality indicator (see Venus manual)")
348 {
349 }
350
351 void Update(const GPS::NEMA &nema)
352 {
353 fDim.Update(nema);
354 }
355};
356
357// ------------------------------------------------------------------------
358
359template <class T, class S>
360class StateMachineGPSControl : public StateMachineAsio<T>
361{
362private:
363 S fGPS;
364 Time fLastCommand;
365
366 bool CheckEventSize(size_t has, const char *name, size_t size)
367 {
368 if (has==size)
369 return true;
370
371 ostringstream msg;
372 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
373 T::Fatal(msg);
374 return false;
375 }
376
377 int Disconnect()
378 {
379 // Close all connections
380 fGPS.PostClose(false);
381
382 return T::GetCurrentState();
383 }
384
385 int Reconnect(const EventImp &evt)
386 {
387 // Close all connections to supress the warning in SetEndpoint
388 fGPS.PostClose(false);
389
390 // Now wait until all connection have been closed and
391 // all pending handlers have been processed
392 ba::io_service::poll();
393
394 if (evt.GetBool())
395 fGPS.SetEndpoint(evt.GetString());
396
397 // Now we can reopen the connection
398 fGPS.PostClose(true);
399
400 return T::GetCurrentState();
401 }
402
403 int SetVerbosity(const EventImp &evt)
404 {
405 if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
406 return T::kSM_FatalError;
407
408 fGPS.SetVerbose(evt.GetBool());
409
410 return T::GetCurrentState();
411 }
412
413 int Send(const string &cmd)
414 {
415 const string tx = cmd+"\r\n";
416 fGPS.PostMessage(tx, tx.size());
417 return T::GetCurrentState();
418 }
419
420 int SendCommand(const EventImp &evt)
421 {
422 return Send(evt.GetString());
423 }
424
425 int Execute()
426 {
427 return fGPS.GetState();
428 }
429
430
431public:
432 StateMachineGPSControl(ostream &out=cout) :
433 StateMachineAsio<T>(out, "GPS_CONTROL"), fGPS(*this, *this)
434 {
435 // State names
436 T::AddStateName(GPS::State::kDisconnected, "Disconnected",
437 "No connection to web-server could be established recently");
438
439 T::AddStateName(GPS::State::kConnected, "Connected",
440 "Connection established, but status still not known");
441
442 T::AddStateName(GPS::State::kDisabled, "Disabled",
443 "Veto is on, no trigger will be emitted");
444
445 T::AddStateName(GPS::State::kEnabled, "Enabled",
446 "System enabled, waiting for satellites");
447
448 T::AddStateName(GPS::State::kLocked, "Locked",
449 "One trigger per second will be send, but the one at the exact minute is vetoed");
450
451 // Commands
452 T::AddEvent("SEND_COMMAND", "C")
453 (bind(&StateMachineGPSControl::SendCommand, this, placeholders::_1))
454 ("Send command to GPS");
455
456 // Verbosity commands
457 T::AddEvent("SET_VERBOSE", "B")
458 (bind(&StateMachineGPSControl::SetVerbosity, this, placeholders::_1))
459 ("set verbosity state"
460 "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
461
462 T::AddEvent("ENABLE")
463 (bind(&StateMachineGPSControl::Send, this, "veto_60"))
464 ("Enable trigger signal once a second vetoed at every exact minute");
465
466 T::AddEvent("DISABLE")
467 (bind(&StateMachineGPSControl::Send, this, "veto_on"))
468 ("Diable trigger output");
469
470 // Conenction commands
471 T::AddEvent("DISCONNECT")
472 (bind(&StateMachineGPSControl::Disconnect, this))
473 ("disconnect from ethernet");
474
475 T::AddEvent("RECONNECT", "O")
476 (bind(&StateMachineGPSControl::Reconnect, this, placeholders::_1))
477 ("(Re)connect ethernet connection to GPS, a new address can be given"
478 "|[host][string]:new ethernet address in the form <host:port>");
479
480 }
481
482 int EvalOptions(Configuration &conf)
483 {
484 fGPS.SetVerbose(!conf.Get<bool>("quiet"));
485 fGPS.SetDebugTx(conf.Get<bool>("debug-tx"));
486 fGPS.SetEndpoint(conf.Get<string>("addr"));
487 fGPS.StartConnect();
488
489 return -1;
490 }
491};
492
493// ------------------------------------------------------------------------
494
495#include "Main.h"
496
497
498template<class T, class S, class R>
499int RunShell(Configuration &conf)
500{
501 return Main::execute<T, StateMachineGPSControl<S, R>>(conf);
502}
503
504void SetupConfiguration(Configuration &conf)
505{
506 po::options_description control("GPS control");
507 control.add_options()
508 ("no-dim,d", po_switch(), "Disable dim services")
509 ("addr,a", var<string>("gps:23"), "Network address of the lid controling Arduino including port")
510 ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.")
511 ("debug-tx", po_bool(), "Enable debugging of ethernet transmission.")
512 ;
513
514 conf.AddOptions(control);
515}
516
517/*
518 Extract usage clause(s) [if any] for SYNOPSIS.
519 Translators: "Usage" and "or" here are patterns (regular expressions) which
520 are used to match the usage synopsis in program output. An example from cp
521 (GNU coreutils) which contains both strings:
522 Usage: cp [OPTION]... [-T] SOURCE DEST
523 or: cp [OPTION]... SOURCE... DIRECTORY
524 or: cp [OPTION]... -t DIRECTORY SOURCE...
525 */
526void PrintUsage()
527{
528 cout <<
529 "The gpsctrl is an interface to the GPS hardware.\n"
530 "\n"
531 "The default is that the program is started without user intercation. "
532 "All actions are supposed to arrive as DimCommands. Using the -c "
533 "option, a local shell can be initialized. With h or help a short "
534 "help message about the usuage can be brought to the screen.\n"
535 "\n"
536 "Usage: gpsctrl [-c type] [OPTIONS]\n"
537 " or: gpsctrl [OPTIONS]\n";
538 cout << endl;
539}
540
541void PrintHelp()
542{
543// Main::PrintHelp<StateMachineFTM<StateMachine, ConnectionFTM>>();
544
545 /* Additional help text which is printed after the configuration
546 options goes here */
547
548 /*
549 cout << "bla bla bla" << endl << endl;
550 cout << endl;
551 cout << "Environment:" << endl;
552 cout << "environment" << endl;
553 cout << endl;
554 cout << "Examples:" << endl;
555 cout << "test exam" << endl;
556 cout << endl;
557 cout << "Files:" << endl;
558 cout << "files" << endl;
559 cout << endl;
560 */
561}
562
563int main(int argc, const char* argv[])
564{
565 Configuration conf(argv[0]);
566 conf.SetPrintUsage(PrintUsage);
567 Main::SetupConfiguration(conf);
568 SetupConfiguration(conf);
569
570 if (!conf.DoParse(argc, argv, PrintHelp))
571 return 127;
572
573 // No console access at all
574 if (!conf.Has("console"))
575 {
576 if (conf.Get<bool>("no-dim"))
577 return RunShell<LocalStream, StateMachine, ConnectionGPS>(conf);
578 else
579 return RunShell<LocalStream, StateMachineDim, ConnectionDimWeather>(conf);
580 }
581 // Cosole access w/ and w/o Dim
582 if (conf.Get<bool>("no-dim"))
583 {
584 if (conf.Get<int>("console")==0)
585 return RunShell<LocalShell, StateMachine, ConnectionGPS>(conf);
586 else
587 return RunShell<LocalConsole, StateMachine, ConnectionGPS>(conf);
588 }
589 else
590 {
591 if (conf.Get<int>("console")==0)
592 return RunShell<LocalShell, StateMachineDim, ConnectionDimWeather>(conf);
593 else
594 return RunShell<LocalConsole, StateMachineDim, ConnectionDimWeather>(conf);
595 }
596
597 return 0;
598}
Note: See TracBrowser for help on using the repository browser.