2 Copyright (C) 2012 Intel Corporation
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.
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.
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
20 #include "timestamp.h"
21 #include "serialport.hpp"
22 #include "bluetooth.hpp"
23 #include "bluetooth5.h"
24 #include <listplusplus.h>
27 #include <boost/assert.hpp>
28 #include <boost/algorithm/string.hpp>
35 #include "abstractpropertytype.h"
37 #define GPSTIME "GpsTime"
40 inline T2 lexical_cast(const std::string &in) {
51 Location(AbstractRoutingEngine* re, std::string uuid);
53 void parse(std::string gprmc);
55 VehicleProperty::LatitudeType latitude()
60 VehicleProperty::LongitudeType longitude()
65 VehicleProperty::AltitudeType altitude()
70 VehicleProperty::DirectionType direction()
75 VehicleProperty::VehicleSpeedType speed()
80 BasicPropertyType<double> gpsTime()
85 std::list<AbstractPropertyType*> fix()
87 std::list<AbstractPropertyType*> l;
89 l.push_back(&mLatitude);
90 l.push_back(&mLongitude);
91 l.push_back(&mAltitude);
92 l.push_back(&mDirection);
94 l.push_back(&mGpsTime);
101 void parseGprmc(string gprmc);
102 void parseGpgga(string gpgga);
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);
111 double degsToDecimal(double degs);
115 VehicleProperty::LatitudeType mLatitude;
116 VehicleProperty::LongitudeType mLongitude;
117 VehicleProperty::AltitudeType mAltitude;
118 VehicleProperty::DirectionType mDirection;
119 VehicleProperty::VehicleSpeedType mSpeed;
120 BasicPropertyType<double> mGpsTime;
126 AbstractRoutingEngine* routingEngine;
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)
136 void Location::parse(string nmea)
138 if(boost::algorithm::starts_with(nmea,"GPRMC"))
142 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
148 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
152 void Location::parseGprmc(string gprmc)
154 DebugOut(7)<<"parsing gprmc message"<<endl;
156 std::vector<std::string> tokens;
157 boost::split(tokens, gprmc, boost::is_any_of(","));
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())
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));
176 parseLatitude(tokens[3], "", tokens[4]);
177 parseLongitude(tokens[5], "", tokens[6]);
178 parseSpeed(tokens[7]);
179 parseDirection(tokens[8]);
183 void Location::parseGpgga(string gpgga)
186 std::vector<std::string> tokens;
187 boost::split(tokens, gpgga, boost::is_any_of(","));
189 if(tokens.size() != 15)
191 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
195 parseLatitude(tokens[2],"",tokens[3]);
196 parseLongitude(tokens[4],"",tokens[5]);
201 else isActive = false;
203 parseAltitude(tokens[9]);
206 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
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;
218 time_t time = mktime(&t);
220 BasicPropertyType<double> temp(GPSTIME,(double)time);
226 routingEngine->updateProperty(&mGpsTime, mUuid);
231 DebugOut(5)<<"Failed to parse time "<<endl;
235 void Location::parseLatitude(string d, string m, string ns)
242 double degs = boost::lexical_cast<double>(d + m);
243 double dec = degsToDecimal(degs);
248 VehicleProperty::LatitudeType temp(dec);
250 if(mLatitude != temp)
255 routingEngine->updateProperty(&mLatitude, mUuid);
260 DebugOut(5)<<"Failed to parse latitude"<<endl;
264 void Location::parseLongitude(string d, string m, string ew)
268 if(d.empty()) return;
270 double degs = boost::lexical_cast<double>(d + m);
271 double dec = degsToDecimal(degs);
276 VehicleProperty::LongitudeType temp(dec);
278 if(mLongitude != temp)
283 routingEngine->updateProperty(&mLongitude, mUuid);
288 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
292 void Location::parseSpeed(string spd)
296 double s = boost::lexical_cast<double>(spd);
300 VehicleProperty::VehicleSpeedType temp(s);
306 routingEngine->updateProperty(&mSpeed, mUuid);
311 DebugOut(5)<<"failed to parse speed"<<endl;
315 void Location::parseDirection(string dir)
318 uint16_t d = boost::lexical_cast<double>(dir);
320 VehicleProperty::DirectionType temp(d);
321 if(mDirection != temp)
325 routingEngine->updateProperty(&mDirection, mUuid);
330 DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
334 void Location::parseAltitude(string alt)
338 if(alt.empty()) return;
340 double a = boost::lexical_cast<double>(alt);
342 VehicleProperty::AltitudeType temp(a);
343 if(mAltitude != temp)
348 routingEngine->updateProperty(&mAltitude, mUuid);
351 mAltitude = VehicleProperty::AltitudeType(a);
355 DebugOut(5)<<"failed to parse altitude"<<endl;
359 double Location::degsToDecimal(double degs)
362 double min = 100.0 * modf(degs / 100.0, °);
363 return deg + (min / 60.0);
366 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
368 // DebugOut(5) << "Polling..." << condition << endl;
370 if(condition & G_IO_ERR)
372 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
375 if (condition & G_IO_HUP)
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;
383 GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
390 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
392 return new GpsNmeaSource(routingengine, config);
396 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
397 :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr), bt(nullptr)
400 location =new Location(re, mUuid);
402 VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
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);
413 if(config.find("test") != config.end())
418 std::string btaddapter = config["bluetoothAdapter"];
420 if(config.find("baudrate")!= config.end())
422 baudrate = boost::lexical_cast<int>( config["baudrate"] );
425 if(config.find("device")!= config.end())
427 std::string dev = config["device"];
429 if(dev.find(":") != string::npos)
432 bt = new Bluetooth5();
433 bt->getDeviceForAddress(dev, [this](int fd) {
434 DebugOut() << "fd: " << fd << endl;
435 device = new SerialPort(fd);
440 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
441 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
444 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
452 bt = new BluetoothDevice();
453 dev = bt->getDeviceForAddress(dev, btaddapter);
455 device = new SerialPort(dev);
459 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
460 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
465 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
470 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
480 device = new SerialPort(dev);
484 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
485 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
490 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
495 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
505 GpsNmeaSource::~GpsNmeaSource()
507 if(device && device->isOpen())
513 const string GpsNmeaSource::uuid()
519 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
521 DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
523 std::list<AbstractPropertyType*> f = location->fix();
525 for(auto property : f)
527 if(property->name == reply->property)
529 reply->success = true;
530 reply->value = property;
531 reply->completed(reply);
536 reply->success = false;
537 reply->error = AsyncPropertyReply::InvalidOperation;
538 reply->completed(reply);
541 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
546 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
551 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
553 mRequests.push_back(property);
556 PropertyList GpsNmeaSource::supported()
561 int GpsNmeaSource::supportedOperations()
566 void GpsNmeaSource::canHasData()
568 std::string data = device->read();
573 void GpsNmeaSource::test()
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");
578 DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
580 g_assert(location.latitude().toString() == "-23.86600833");
581 g_assert(location.gpsTime().toString() == "1050585131");
583 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
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");
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;
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;
599 std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
601 g_assert(checksum(testChecksuming));
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";
611 bool multimessageParse = false;
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);
621 g_assert(multimessageParse);
623 //Test meaningingless message:
624 location.parse("GPRMC,,V,,,,,,,,,,N*53");
626 //test false message:
628 g_assert(!checksum("GPRMC,172758.296,V"));
631 bool GpsNmeaSource::tryParse(string data)
633 std::vector<std::string> lines;
635 boost::split(lines, data, boost::is_any_of("$"));
637 bool weFoundAMessage = false;
639 for(auto line : lines)
650 std::string::size_type pos = buffer.find('G');
652 if(pos != std::string::npos && pos != 0)
654 ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
655 buffer = buffer.substr(pos);
660 /// we have a complete message. parse it!
661 DebugOut(7)<<"Complete message: "<<buffer<<endl;
662 location->parse(buffer);
663 weFoundAMessage = true;
670 uint cs = buffer.find('*');
671 if (cs != std::string::npos && cs != buffer.length()-1)
673 ///This means we have a false flag somewhere.
674 buffer = buffer.substr(cs+(buffer.length() - cs));
679 DebugOut(7)<<"buffer: "<<buffer<<endl;
682 return weFoundAMessage;
685 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
687 removeOne(&mRequests,property);
690 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
692 mSupported.push_back(property);
694 std::list<Zone::Type> zones;
696 zones.push_back(zone);
698 PropertyInfo info(0, zones);
700 propertyInfoMap[property] = info;
703 bool GpsNmeaSource::checksum(std::string sentence)
705 if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
712 for(auto i : sentence)
716 if(i != '\n' || i != '\r')
720 std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
724 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
726 return sentenceCheck == checksum;
738 int main(int argc, char** argv)
740 DebugOut::setDebugThreshhold(7);
741 GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());