Merge pull request #22 from tripzero/master
[profile/ivi/automotive-message-broker.git] / plugins / gpsnmea / gpsnmea.cpp
1 /*
2 Copyright (C) 2012 Intel Corporation
3
4 This library is free software; you can redistribute it and/or
5 modify it under the terms of the GNU Lesser General Public
6 License as published by the Free Software Foundation; either
7 version 2.1 of the License, or (at your option) any later version.
8
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 Lesser General Public License for more details.
13
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 */
18
19 #include "gpsnmea.h"
20 #include "timestamp.h"
21 #include "serialport.hpp"
22 #include "bluetooth.hpp"
23 #include "bluetooth5.h"
24 #include <listplusplus.h>
25
26 #include <iostream>
27 #include <boost/assert.hpp>
28 #include <boost/algorithm/string.hpp>
29 #include <time.h>
30
31
32 using namespace std;
33
34 #include "debugout.h"
35 #include "abstractpropertytype.h"
36
37 #define GPSTIME "GpsTime"
38
39 template<typename T2>
40 inline T2 lexical_cast(const std::string &in) {
41         T2 out;
42         std::stringstream ss;
43         ss << std::hex << in;
44         ss >> out;
45         return out;
46 }
47
48 class Location
49 {
50 public:
51         Location(AbstractRoutingEngine* re, std::string uuid);
52
53         void parse(std::string gprmc);
54
55         VehicleProperty::LatitudeType latitude()
56         {
57                 return mLatitude;
58         }
59
60         VehicleProperty::LongitudeType longitude()
61         {
62                 return mLongitude;
63         }
64
65         VehicleProperty::AltitudeType altitude()
66         {
67                 return mAltitude;
68         }
69
70         VehicleProperty::DirectionType direction()
71         {
72                 return mDirection;
73         }
74
75         VehicleProperty::VehicleSpeedType speed()
76         {
77                 return mSpeed;
78         }
79
80         BasicPropertyType<double> gpsTime()
81         {
82                 return mGpsTime;
83         }
84
85         std::list<AbstractPropertyType*> fix()
86         {
87                 std::list<AbstractPropertyType*> l;
88
89                 l.push_back(&mLatitude);
90                 l.push_back(&mLongitude);
91                 l.push_back(&mAltitude);
92                 l.push_back(&mDirection);
93                 l.push_back(&mSpeed);
94                 l.push_back(&mGpsTime);
95
96                 return l;
97         }
98
99 private: ///methods:
100
101         void parseGprmc(string gprmc);
102         void parseGpgga(string gpgga);
103
104         void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
105         void parseLatitude(std::string d, std::string m, std::string ns);
106         void parseLongitude(std::string d, string m, string ew);
107         void parseSpeed(std::string spd);
108         void parseDirection(std::string dir);
109         void parseAltitude(std::string alt);
110
111         double degsToDecimal(double degs);
112
113 private:
114
115         VehicleProperty::LatitudeType mLatitude;
116         VehicleProperty::LongitudeType mLongitude;
117         VehicleProperty::AltitudeType mAltitude;
118         VehicleProperty::DirectionType  mDirection;
119         VehicleProperty::VehicleSpeedType mSpeed;
120         BasicPropertyType<double> mGpsTime;
121
122         bool isActive;
123
124         std::string mUuid;
125
126         AbstractRoutingEngine* routingEngine;
127
128 };
129
130 Location::Location(AbstractRoutingEngine* re, std::string uuid)
131         :mLatitude(0), mLongitude(0), mAltitude(0), mDirection(0), mSpeed(0), mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
132 {
133
134 }
135
136 void Location::parse(string nmea)
137 {
138         if(boost::algorithm::starts_with(nmea,"GPRMC"))
139         {
140                 parseGprmc(nmea);
141         }
142         else if(boost::algorithm::starts_with(nmea,"GPGGA"))
143         {
144                 parseGpgga(nmea);
145         }
146         else
147         {
148                 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
149         }
150 }
151
152 void Location::parseGprmc(string gprmc)
153 {
154         DebugOut(7)<<"parsing gprmc message"<<endl;
155
156         std::vector<std::string> tokens;
157         boost::split(tokens, gprmc, boost::is_any_of(","));
158
159         if(!tokens.size())
160         {
161                 return;
162         }
163
164         if(tokens[2] == "A")
165         {
166                 isActive = true;
167         }
168
169         if(tokens[1].empty() || tokens[9].empty() || tokens[3].empty() || tokens[4].empty() || tokens[5].empty() || tokens[6].empty() || tokens[7].empty() || tokens[8].empty())
170         {
171                 return;
172         }
173
174         parseTime(tokens[1].substr(0,2),tokens[1].substr(2,2),tokens[1].substr(4,2),tokens[9].substr(0,2),tokens[9].substr(2,2),tokens[9].substr(4,2));
175
176         parseLatitude(tokens[3], "", tokens[4]);
177         parseLongitude(tokens[5], "", tokens[6]);
178         parseSpeed(tokens[7]);
179         parseDirection(tokens[8]);
180
181 }
182
183 void Location::parseGpgga(string gpgga)
184 {
185
186         std::vector<std::string> tokens;
187         boost::split(tokens, gpgga, boost::is_any_of(","));
188
189         if(tokens.size() != 15)
190         {
191                 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
192                 return;
193         }
194
195         parseLatitude(tokens[2],"",tokens[3]);
196         parseLongitude(tokens[4],"",tokens[5]);
197         if(tokens[6] != "0")
198         {
199                 isActive = true;
200         }
201         else isActive = false;
202
203         parseAltitude(tokens[9]);
204 }
205
206 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
207 {
208         try
209         {
210                 tm t;
211                 t.tm_hour = boost::lexical_cast<int>(h);
212                 t.tm_min = boost::lexical_cast<int>(m);
213                 t.tm_sec = boost::lexical_cast<int>(s);
214                 t.tm_mday = boost::lexical_cast<int>(dd);
215                 t.tm_mon = boost::lexical_cast<int>(mm);
216                 t.tm_year = boost::lexical_cast<int>(yy) + 100;
217
218                 time_t time = mktime(&t);
219
220                 BasicPropertyType<double> temp(GPSTIME,(double)time);
221
222                 if(mGpsTime != temp)
223                 {
224                         mGpsTime = temp;
225                         if(routingEngine)
226                                 routingEngine->updateProperty(&mGpsTime, mUuid);
227                 }
228         }
229         catch(...)
230         {
231                 DebugOut(5)<<"Failed to parse time "<<endl;
232         }
233 }
234
235 void Location::parseLatitude(string d, string m, string ns)
236 {
237         try
238         {
239                 if(d.empty() )
240                         return;
241
242                 double degs = boost::lexical_cast<double>(d + m);
243                 double dec = degsToDecimal(degs);
244
245                 if(ns == "S")
246                         dec *= -1;
247
248                 VehicleProperty::LatitudeType temp(dec);
249
250                 if(mLatitude != temp)
251                 {
252                         mLatitude = temp;
253
254                         if(routingEngine)
255                                 routingEngine->updateProperty(&mLatitude, mUuid);
256                 }
257         }
258         catch(...)
259         {
260                 DebugOut(5)<<"Failed to parse latitude"<<endl;
261         }
262 }
263
264 void Location::parseLongitude(string d, string m, string ew)
265 {
266         try
267         {
268                 if(d.empty()) return;
269
270                 double degs = boost::lexical_cast<double>(d + m);
271                 double dec = degsToDecimal(degs);
272
273                 if(ew == "W")
274                         dec *= -1;
275
276                 VehicleProperty::LongitudeType temp(dec);
277
278                 if(mLongitude != temp)
279                 {
280                         mLongitude = temp;
281
282                         if(routingEngine)
283                                 routingEngine->updateProperty(&mLongitude, mUuid);
284                 }
285         }
286         catch(...)
287         {
288                 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
289         }
290 }
291
292 void Location::parseSpeed(string spd)
293 {
294         try
295         {
296                 double s = boost::lexical_cast<double>(spd);
297
298                 ///to kph:
299                 s *= 1.852;
300                 VehicleProperty::VehicleSpeedType temp(s);
301                 if(mSpeed != temp)
302                 {
303                         mSpeed = temp;
304
305                         if(routingEngine)
306                                 routingEngine->updateProperty(&mSpeed, mUuid);
307                 }
308         }
309         catch(...)
310         {
311                 DebugOut(5)<<"failed to parse speed"<<endl;
312         }
313 }
314
315 void Location::parseDirection(string dir)
316 {
317         try {
318                 uint16_t d = boost::lexical_cast<double>(dir);
319
320                 VehicleProperty::DirectionType temp(d);
321                 if(mDirection != temp)
322                 {
323                         mDirection = temp;
324                         if(routingEngine)
325                                 routingEngine->updateProperty(&mDirection, mUuid);
326                 }
327         }
328         catch(...)
329         {
330                 DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
331         }
332 }
333
334 void Location::parseAltitude(string alt)
335 {
336         try{
337
338                 if(alt.empty()) return;
339
340                 double a = boost::lexical_cast<double>(alt);
341
342                 VehicleProperty::AltitudeType temp(a);
343                 if(mAltitude != temp)
344                 {
345                         mAltitude = temp;
346
347                         if(routingEngine)
348                                 routingEngine->updateProperty(&mAltitude, mUuid);
349                 }
350
351                 mAltitude = VehicleProperty::AltitudeType(a);
352         }
353         catch(...)
354         {
355                 DebugOut(5)<<"failed to parse altitude"<<endl;
356         }
357 }
358
359 double Location::degsToDecimal(double degs)
360 {
361         double deg;
362         double min = 100.0 * modf(degs / 100.0, &deg);
363         return deg + (min / 60.0);
364 }
365
366 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
367 {
368 //      DebugOut(5) << "Polling..." << condition << endl;
369
370         if(condition & G_IO_ERR)
371         {
372                 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
373         }
374
375         if (condition & G_IO_HUP)
376         {
377                 //Hang up. Returning false closes out the GIOChannel.
378                 //printf("Callback on G_IO_HUP\n");
379                 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
380                 return false;
381         }
382
383         GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
384
385         src->canHasData();
386
387         return true;
388 }
389
390 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
391 {
392         return new GpsNmeaSource(routingengine, config);
393
394 }
395
396 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
397         :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr), bt(nullptr)
398 {
399         int baudrate = 0;
400         location =new Location(re, mUuid);
401
402         VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
403
404         addPropertySupport(VehicleProperty::Latitude, Zone::None);
405         addPropertySupport(VehicleProperty::Longitude, Zone::None);
406         addPropertySupport(VehicleProperty::Altitude, Zone::None);
407         addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
408         addPropertySupport(VehicleProperty::Direction, Zone::None);
409         addPropertySupport(GPSTIME, Zone::None);
410
411         ///test:
412
413         if(config.find("test") != config.end())
414         {
415                 test();
416         }
417
418         std::string btaddapter = config["bluetoothAdapter"];
419
420         if(config.find("baudrate")!= config.end())
421         {
422                 baudrate = boost::lexical_cast<int>( config["baudrate"] );
423         }
424
425         if(config.find("device")!= config.end())
426         {
427                 std::string dev = config["device"];
428
429                 if(dev.find(":") != string::npos)
430                 {
431 #ifdef USE_BLUEZ5
432                         bt = new Bluetooth5();
433                         bt->getDeviceForAddress(dev, [this](int fd) {
434                                 DebugOut() << "fd: " << fd << endl;
435                                 device = new SerialPort(fd);
436                                 int baudrate=0;
437
438                                 if(baudrate!=0)
439                                 {
440                                         if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
441                                                 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
442                                 }
443
444                                 DebugOut()<<"read from device: "<<device->read()<<endl;
445
446                                 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
447                                 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
448                                 g_io_channel_set_close_on_unref(chan, true);
449                                 g_io_channel_unref(chan);
450                         });
451 #else
452                         bt = new BluetoothDevice();
453                         dev = bt->getDeviceForAddress(dev, btaddapter);
454
455                         device = new SerialPort(dev);
456
457                         if(baudrate!=0)
458                         {
459                                 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
460                                         DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
461                         }
462
463                         if(!device->open())
464                         {
465                                 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
466                                 perror("Error");
467                                 return;
468                         }
469
470                         DebugOut()<<"read from device: "<<device->read()<<endl;
471
472                         GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
473                         g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
474                         g_io_channel_set_close_on_unref(chan, true);
475                         g_io_channel_unref(chan);
476 #endif
477                 }
478                 else
479                 {
480                         device = new SerialPort(dev);
481
482                         if(baudrate!=0)
483                         {
484                                 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
485                                         DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
486                         }
487
488                         if(!device->open())
489                         {
490                                 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
491                                 perror("Error");
492                                 return;
493                         }
494
495                         DebugOut()<<"read from device: "<<device->read()<<endl;
496
497                         GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
498                         g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR), (GIOFunc)readCallback, this);
499                         g_io_channel_set_close_on_unref(chan, true);
500                         g_io_channel_unref(chan);
501                 }
502         }
503 }
504
505 GpsNmeaSource::~GpsNmeaSource()
506 {
507         if(device && device->isOpen())
508                 device->close();
509         if(bt)
510                 delete bt;
511 }
512
513 const string GpsNmeaSource::uuid()
514 {
515         return mUuid;
516 }
517
518
519 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
520 {
521         DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
522
523         std::list<AbstractPropertyType*> f = location->fix();
524
525         for(auto property : f)
526         {
527                 if(property->name == reply->property)
528                 {
529                         reply->success = true;
530                         reply->value = property;
531                         reply->completed(reply);
532                         return;
533                 }
534         }
535
536         reply->success = false;
537         reply->error = AsyncPropertyReply::InvalidOperation;
538         reply->completed(reply);
539 }
540
541 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
542 {
543
544 }
545
546 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
547 {
548
549 }
550
551 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
552 {
553         mRequests.push_back(property);
554 }
555
556 PropertyList GpsNmeaSource::supported()
557 {
558         return mSupported;
559 }
560
561 int GpsNmeaSource::supportedOperations()
562 {
563         return Get;
564 }
565
566 void GpsNmeaSource::canHasData()
567 {
568         std::string data = device->read();
569
570         tryParse(data);
571 }
572
573 void GpsNmeaSource::test()
574 {
575         Location location(nullptr, "");
576         location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
577
578         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
579
580         g_assert(location.latitude().toString() == "-23.86600833");
581         g_assert(location.gpsTime().toString() == "1050585131");
582
583         location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
584
585         DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
586         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
587         g_assert(location.altitude().toString() == "545.4");
588         g_assert(location.latitude().toString() == "48.1173");
589
590         location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
591         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
592         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
593
594         //Test incomplete message:
595         location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
596         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
597         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
598
599         std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
600
601         g_assert(checksum(testChecksuming));
602
603         std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
604         std::string multimessage2 = "57383,W,";
605         std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
606         std::string multimessage4 = "GPGSA,A,";
607         std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
608         std::string multimessage6 = ",7.53,1.00*";
609         std::string multimessage7 = "0E";
610
611         bool multimessageParse = false;
612
613         multimessageParse |= tryParse(multimessage1);
614         multimessageParse |= tryParse(multimessage2);
615         multimessageParse |= tryParse(multimessage3);
616         multimessageParse |= tryParse(multimessage4);
617         multimessageParse |= tryParse(multimessage5);
618         multimessageParse |= tryParse(multimessage6);
619         multimessageParse |= tryParse(multimessage7);
620
621         g_assert(multimessageParse);
622
623         //Test meaningingless message:
624         location.parse("GPRMC,,V,,,,,,,,,,N*53");
625
626         //test false message:
627
628         g_assert(!checksum("GPRMC,172758.296,V"));
629 }
630
631 bool GpsNmeaSource::tryParse(string data)
632 {
633         std::vector<std::string> lines;
634
635         boost::split(lines, data, boost::is_any_of("$"));
636
637         bool weFoundAMessage = false;
638
639         for(auto line : lines)
640         {
641                 if(checksum(line))
642                 {
643                         buffer = line;
644                 }
645                 else
646                 {
647                         buffer += line;
648                 }
649
650                 std::string::size_type pos = buffer.find('G');
651
652                 if(pos != std::string::npos && pos != 0)
653                 {
654                         ///Throw the incomplete stuff away.  if it doesn't begin with "G" it'll never be complete
655                         buffer = buffer.substr(pos);
656                 }
657
658                 if(checksum(buffer))
659                 {
660                         /// we have a complete message.  parse it!
661                         DebugOut(7)<<"Complete message: "<<buffer<<endl;
662                         location->parse(buffer);
663                         weFoundAMessage = true;
664                         buffer = "";
665                 }
666                 else
667                 {
668                         if(pos == 0 )
669                         {
670                                 uint cs = buffer.find('*');
671                                 if (cs != std::string::npos && cs != buffer.length()-1)
672                                 {
673                                         ///This means we have a false flag somewhere.
674                                         buffer = buffer.substr(cs+(buffer.length() - cs));
675                                 }
676                         }
677                 }
678
679                 DebugOut(7)<<"buffer: "<<buffer<<endl;
680         }
681
682         return weFoundAMessage;
683 }
684
685 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
686 {
687         removeOne(&mRequests,property);
688 }
689
690 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
691 {
692         mSupported.push_back(property);
693
694         std::list<Zone::Type> zones;
695
696         zones.push_back(zone);
697
698         PropertyInfo info(0, zones);
699
700         propertyInfoMap[property] = info;
701 }
702
703 bool GpsNmeaSource::checksum(std::string sentence)
704 {
705         if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
706         {
707                 return false;
708         }
709
710         int checksum = 0;
711
712         for(auto i : sentence)
713         {
714                 if(i == '*')
715                         break;
716                 if(i != '\n' || i != '\r')
717                         checksum ^= i;
718         }
719
720         std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
721
722         try
723         {
724                 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
725
726                 return sentenceCheck == checksum;
727         }
728         catch(...)
729
730         {
731                 return false;
732         }
733
734         return false;
735 }
736
737
738 int main(int argc, char** argv)
739 {
740         DebugOut::setDebugThreshhold(7);
741         GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());
742         plugin.test();
743
744         return 1;
745 }