fixed up bluez5 support code
[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 #ifdef USE_BLUEZ5
430                 if(dev.find(":") != string::npos)
431                 {
432
433                         bt = new Bluetooth5();
434                         bt->getDeviceForAddress(dev, [this](int fd) {
435                                 DebugOut() << "fd: " << fd << endl;
436                                 device = new SerialPort(fd);
437                                 int baudrate=0;
438
439                                 if(baudrate!=0)
440                                 {
441                                         if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
442                                                 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
443                                 }
444
445                                 DebugOut()<<"read from device: "<<device->read()<<endl;
446
447                                 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
448                                 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
449                                 g_io_channel_set_close_on_unref(chan, true);
450                                 g_io_channel_unref(chan);
451                         });
452 #else
453                         bt = new BluetoothDevice();
454                         dev = bt->getDeviceForAddress(dev, btaddapter);
455
456                         device = new SerialPort(dev);
457
458                         if(baudrate!=0)
459                         {
460                                 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
461                                         DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
462                         }
463
464                         if(!device->open())
465                         {
466                                 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
467                                 perror("Error");
468                                 return;
469                         }
470
471                         DebugOut()<<"read from device: "<<device->read()<<endl;
472
473                         GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
474                         g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
475                         g_io_channel_set_close_on_unref(chan, true);
476                         g_io_channel_unref(chan);
477 #endif
478                 }
479                 else
480                 {
481                         device = new SerialPort(dev);
482
483                         if(baudrate!=0)
484                         {
485                                 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
486                                         DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
487                         }
488
489                         if(!device->open())
490                         {
491                                 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
492                                 perror("Error");
493                                 return;
494                         }
495
496                         DebugOut()<<"read from device: "<<device->read()<<endl;
497
498                         GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
499                         g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR), (GIOFunc)readCallback, this);
500                         g_io_channel_set_close_on_unref(chan, true);
501                         g_io_channel_unref(chan);
502                 }
503         }
504 }
505
506 GpsNmeaSource::~GpsNmeaSource()
507 {
508         if(device && device->isOpen())
509                 device->close();
510         if(bt)
511                 delete bt;
512 }
513
514 const string GpsNmeaSource::uuid()
515 {
516         return mUuid;
517 }
518
519
520 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
521 {
522         DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
523
524         std::list<AbstractPropertyType*> f = location->fix();
525
526         for(auto property : f)
527         {
528                 if(property->name == reply->property)
529                 {
530                         reply->success = true;
531                         reply->value = property;
532                         reply->completed(reply);
533                         return;
534                 }
535         }
536
537         reply->success = false;
538         reply->error = AsyncPropertyReply::InvalidOperation;
539         reply->completed(reply);
540 }
541
542 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
543 {
544
545 }
546
547 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
548 {
549
550 }
551
552 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
553 {
554         mRequests.push_back(property);
555 }
556
557 PropertyList GpsNmeaSource::supported()
558 {
559         return mSupported;
560 }
561
562 int GpsNmeaSource::supportedOperations()
563 {
564         return Get;
565 }
566
567 void GpsNmeaSource::canHasData()
568 {
569         std::string data = device->read();
570
571         tryParse(data);
572 }
573
574 void GpsNmeaSource::test()
575 {
576         Location location(nullptr, "");
577         location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
578
579         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
580
581         g_assert(location.latitude().toString() == "-23.86600833");
582         g_assert(location.gpsTime().toString() == "1050585131");
583
584         location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
585
586         DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
587         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
588         g_assert(location.altitude().toString() == "545.4");
589         g_assert(location.latitude().toString() == "48.1173");
590
591         location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
592         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
593         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
594
595         //Test incomplete message:
596         location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
597         DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
598         DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
599
600         std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
601
602         g_assert(checksum(testChecksuming));
603
604         std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
605         std::string multimessage2 = "57383,W,";
606         std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
607         std::string multimessage4 = "GPGSA,A,";
608         std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
609         std::string multimessage6 = ",7.53,1.00*";
610         std::string multimessage7 = "0E";
611
612         bool multimessageParse = false;
613
614         multimessageParse |= tryParse(multimessage1);
615         multimessageParse |= tryParse(multimessage2);
616         multimessageParse |= tryParse(multimessage3);
617         multimessageParse |= tryParse(multimessage4);
618         multimessageParse |= tryParse(multimessage5);
619         multimessageParse |= tryParse(multimessage6);
620         multimessageParse |= tryParse(multimessage7);
621
622         g_assert(multimessageParse);
623
624         //Test meaningingless message:
625         location.parse("GPRMC,,V,,,,,,,,,,N*53");
626
627         //test false message:
628
629         g_assert(!checksum("GPRMC,172758.296,V"));
630 }
631
632 bool GpsNmeaSource::tryParse(string data)
633 {
634         std::vector<std::string> lines;
635
636         boost::split(lines, data, boost::is_any_of("$"));
637
638         bool weFoundAMessage = false;
639
640         for(auto line : lines)
641         {
642                 if(checksum(line))
643                 {
644                         buffer = line;
645                 }
646                 else
647                 {
648                         buffer += line;
649                 }
650
651                 std::string::size_type pos = buffer.find('G');
652
653                 if(pos != std::string::npos && pos != 0)
654                 {
655                         ///Throw the incomplete stuff away.  if it doesn't begin with "G" it'll never be complete
656                         buffer = buffer.substr(pos);
657                 }
658
659                 if(checksum(buffer))
660                 {
661                         /// we have a complete message.  parse it!
662                         DebugOut(7)<<"Complete message: "<<buffer<<endl;
663                         location->parse(buffer);
664                         weFoundAMessage = true;
665                         buffer = "";
666                 }
667                 else
668                 {
669                         if(pos == 0 )
670                         {
671                                 uint cs = buffer.find('*');
672                                 if (cs != std::string::npos && cs != buffer.length()-1)
673                                 {
674                                         ///This means we have a false flag somewhere.
675                                         buffer = buffer.substr(cs+(buffer.length() - cs));
676                                 }
677                         }
678                 }
679
680                 DebugOut(7)<<"buffer: "<<buffer<<endl;
681         }
682
683         return weFoundAMessage;
684 }
685
686 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
687 {
688         removeOne(&mRequests,property);
689 }
690
691 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
692 {
693         mSupported.push_back(property);
694
695         std::list<Zone::Type> zones;
696
697         zones.push_back(zone);
698
699         PropertyInfo info(0, zones);
700
701         propertyInfoMap[property] = info;
702 }
703
704 bool GpsNmeaSource::checksum(std::string sentence)
705 {
706         if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
707         {
708                 return false;
709         }
710
711         int checksum = 0;
712
713         for(auto i : sentence)
714         {
715                 if(i == '*')
716                         break;
717                 if(i != '\n' || i != '\r')
718                         checksum ^= i;
719         }
720
721         std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
722
723         try
724         {
725                 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
726
727                 return sentenceCheck == checksum;
728         }
729         catch(...)
730
731         {
732                 return false;
733         }
734
735         return false;
736 }
737
738
739 int main(int argc, char** argv)
740 {
741         DebugOut::setDebugThreshhold(7);
742         GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());
743         plugin.test();
744
745         return 1;
746 }