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>
25 #include <superptr.hpp>
28 #include <boost/assert.hpp>
29 #include <boost/algorithm/string.hpp>
36 #include "abstractpropertytype.h"
38 #define GPSTIME "GpsTime"
39 #define GPSSPEED "GpsSpeed"
42 inline T2 lexical_cast(const std::string &in) {
53 Location(AbstractRoutingEngine* re, std::string uuid);
55 void parse(std::string gprmc);
57 VehicleProperty::LatitudeType* latitude()
59 return mLatitude.get();
62 VehicleProperty::LongitudeType* longitude()
64 return mLongitude.get();
67 VehicleProperty::AltitudeType* altitude()
69 return mAltitude.get();
72 VehicleProperty::DirectionType* direction()
74 return mDirection.get();
77 BasicPropertyType<uint16_t>* speed()
82 BasicPropertyType<double>* gpsTime()
84 return mGpsTime.get();
87 std::list<AbstractPropertyType*> fix()
89 std::list<AbstractPropertyType*> l;
91 l.push_back(mLatitude.get());
92 l.push_back(mLongitude.get());
93 l.push_back(mAltitude.get());
94 l.push_back(mDirection.get());
95 l.push_back(mSpeed.get());
96 l.push_back(mGpsTime.get());
103 void parseGprmc(string gprmc);
104 void parseGpgga(string gpgga);
106 void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
107 void parseLatitude(std::string d, std::string m, std::string ns);
108 void parseLongitude(std::string d, string m, string ew);
109 void parseSpeed(std::string spd);
110 void parseDirection(std::string dir);
111 void parseAltitude(std::string alt);
113 double degsToDecimal(double degs);
117 std::unique_ptr<VehicleProperty::LatitudeType> mLatitude;
118 std::unique_ptr<VehicleProperty::LongitudeType> mLongitude;
119 std::unique_ptr<VehicleProperty::AltitudeType> mAltitude;
120 std::unique_ptr<VehicleProperty::DirectionType> mDirection;
121 std::unique_ptr<BasicPropertyType<uint16_t>> mSpeed;
122 std::unique_ptr<BasicPropertyType<double>> mGpsTime;
128 AbstractRoutingEngine* routingEngine;
132 Location::Location(AbstractRoutingEngine* re, std::string uuid)
133 :isActive(false), routingEngine(re), mUuid(uuid)
135 mLatitude = amb::make_unique(new VehicleProperty::LatitudeType(0));
136 mLongitude = amb::make_unique(new VehicleProperty::LongitudeType(0));
137 mAltitude = amb::make_unique(new VehicleProperty::AltitudeType(0));
138 mDirection = amb::make_unique(new VehicleProperty::DirectionType(0));
139 mSpeed = amb::make_unique(new BasicPropertyType<uint16_t>(GPSSPEED, 0));
140 mGpsTime = amb::make_unique(new BasicPropertyType<double>(GPSTIME, 0));
143 void Location::parse(string nmea)
145 if(boost::algorithm::starts_with(nmea,"GPRMC"))
149 else if(boost::algorithm::starts_with(nmea,"GPGGA"))
155 DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
159 void Location::parseGprmc(string gprmc)
161 DebugOut(7)<<"parsing gprmc message"<<endl;
163 std::vector<std::string> tokens;
164 boost::split(tokens, gprmc, boost::is_any_of(","));
176 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())
181 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));
183 parseLatitude(tokens[3], "", tokens[4]);
184 parseLongitude(tokens[5], "", tokens[6]);
185 parseSpeed(tokens[7]);
186 parseDirection(tokens[8]);
190 void Location::parseGpgga(string gpgga)
193 std::vector<std::string> tokens;
194 boost::split(tokens, gpgga, boost::is_any_of(","));
196 if(tokens.size() != 15)
198 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
202 parseLatitude(tokens[2],"",tokens[3]);
203 parseLongitude(tokens[4],"",tokens[5]);
208 else isActive = false;
210 parseAltitude(tokens[9]);
213 void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
218 t.tm_hour = boost::lexical_cast<int>(h);
219 t.tm_min = boost::lexical_cast<int>(m);
220 t.tm_sec = boost::lexical_cast<int>(s);
221 t.tm_mday = boost::lexical_cast<int>(dd);
222 t.tm_mon = boost::lexical_cast<int>(mm);
223 t.tm_year = boost::lexical_cast<int>(yy) + 100;
225 time_t time = mktime(&t);
227 if(mGpsTime->basicValue() != (double(time)))
229 mGpsTime->setValue(double(time));
231 routingEngine->updateProperty(mGpsTime.get(), mUuid);
236 DebugOut(5)<<"Failed to parse time "<<endl;
240 void Location::parseLatitude(string d, string m, string ns)
247 double degs = boost::lexical_cast<double>(d + m);
248 double dec = degsToDecimal(degs);
253 if(mLatitude->basicValue() != dec)
255 mLatitude->setValue(dec);
258 routingEngine->updateProperty(mLatitude.get(), mUuid);
263 DebugOut(5)<<"Failed to parse latitude"<<endl;
267 void Location::parseLongitude(string d, string m, string ew)
271 if(d.empty()) return;
273 double degs = boost::lexical_cast<double>(d + m);
274 double dec = degsToDecimal(degs);
279 if(mLongitude->basicValue() != dec)
281 mLongitude->setValue(dec);
284 routingEngine->updateProperty(mLongitude.get(), mUuid);
289 DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
293 void Location::parseSpeed(string spd)
297 double s = boost::lexical_cast<double>(spd);
302 uint16_t speed = static_cast<uint16_t>(s);
304 if(mSpeed->basicValue() != speed)
306 mSpeed->setValue(speed);
309 routingEngine->updateProperty(mSpeed.get(), mUuid);
314 DebugOut(5)<<"failed to parse speed"<<endl;
318 void Location::parseDirection(string dir)
321 uint16_t d = boost::lexical_cast<double>(dir);
323 if(mDirection->basicValue() != d)
325 mDirection->setValue(d);
328 routingEngine->updateProperty(mDirection.get(), mUuid);
333 DebugOut(5)<<"Failed to parse direction: "<<dir<<endl;
337 void Location::parseAltitude(string alt)
341 if(alt.empty()) return;
343 double a = boost::lexical_cast<double>(alt);
345 if(mAltitude->basicValue() != a)
347 mAltitude->setValue(a);
350 routingEngine->updateProperty(mAltitude.get(), mUuid);
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); });
403 VehicleProperty::registerProperty(GPSSPEED,[](){ return new BasicPropertyType<uint16_t>(GPSSPEED, 0); });
405 addPropertySupport(VehicleProperty::Latitude, Zone::None);
406 addPropertySupport(VehicleProperty::Longitude, Zone::None);
407 addPropertySupport(VehicleProperty::Altitude, Zone::None);
408 addPropertySupport(GPSSPEED, Zone::None);
409 addPropertySupport(VehicleProperty::Direction, Zone::None);
410 addPropertySupport(GPSTIME, Zone::None);
414 if(config.find("test") != config.end())
419 std::string btaddapter = config["bluetoothAdapter"];
421 if(config.find("baudrate")!= config.end())
423 baudrate = boost::lexical_cast<int>( config["baudrate"] );
426 if(config.find("device")!= config.end())
428 std::string dev = config["device"];
430 if(dev.find(":") != string::npos)
433 bt = new Bluetooth5();
434 bt->getDeviceForAddress(dev, [this](int fd) {
435 DebugOut() << "fd: " << fd << endl;
436 device = new SerialPort(fd);
441 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
442 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
445 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
453 bt = new BluetoothDevice();
454 dev = bt->getDeviceForAddress(dev, btaddapter);
456 device = new SerialPort(dev);
460 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
461 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
466 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
471 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
481 device = new SerialPort(dev);
485 if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
486 DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
491 DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
496 DebugOut()<<"read from device: "<<device->read()<<endl;
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);
506 GpsNmeaSource::~GpsNmeaSource()
508 if(device && device->isOpen())
514 const string GpsNmeaSource::uuid()
520 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
522 DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
524 std::list<AbstractPropertyType*> f = location->fix();
526 for(auto property : f)
528 if(property->name == reply->property)
530 reply->success = true;
531 reply->value = property;
532 reply->completed(reply);
537 reply->success = false;
538 reply->error = AsyncPropertyReply::InvalidOperation;
539 reply->completed(reply);
542 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
547 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
552 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
554 mRequests.push_back(property);
557 PropertyList GpsNmeaSource::supported()
562 int GpsNmeaSource::supportedOperations()
567 void GpsNmeaSource::canHasData()
569 std::string data = device->read();
574 void GpsNmeaSource::test()
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");
579 DebugOut(0)<<"lat: "<<location.latitude()->toString()<<endl;
581 g_assert(location.latitude()->toString() == "-23.86600833");
582 g_assert(location.gpsTime()->toString() == "1050585131");
584 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
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");
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;
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;
600 std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
602 g_assert(checksum(testChecksuming));
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";
612 bool multimessageParse = false;
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);
622 g_assert(multimessageParse);
624 //Test meaningingless message:
625 location.parse("GPRMC,,V,,,,,,,,,,N*53");
627 //test false message:
629 g_assert(!checksum("GPRMC,172758.296,V"));
632 bool GpsNmeaSource::tryParse(string data)
634 std::vector<std::string> lines;
636 boost::split(lines, data, boost::is_any_of("$"));
638 bool weFoundAMessage = false;
640 for(auto line : lines)
651 std::string::size_type pos = buffer.find('G');
653 if(pos != std::string::npos && pos != 0)
655 ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
656 buffer = buffer.substr(pos);
661 /// we have a complete message. parse it!
662 DebugOut(7)<<"Complete message: "<<buffer<<endl;
663 location->parse(buffer);
664 weFoundAMessage = true;
671 uint cs = buffer.find('*');
672 if (cs != std::string::npos && cs != buffer.length()-1)
674 ///This means we have a false flag somewhere.
675 buffer = buffer.substr(cs+(buffer.length() - cs));
680 DebugOut(7)<<"buffer: "<<buffer<<endl;
683 return weFoundAMessage;
686 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
688 removeOne(&mRequests,property);
691 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
693 mSupported.push_back(property);
695 std::list<Zone::Type> zones;
697 zones.push_back(zone);
699 PropertyInfo info(0, zones);
701 propertyInfoMap[property] = info;
704 bool GpsNmeaSource::checksum(std::string sentence)
706 if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
713 for(auto i : sentence)
717 if(i != '\n' || i != '\r')
721 std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
725 int sentenceCheck = lexical_cast<int>(sentenceCheckStr);
727 return sentenceCheck == checksum;
739 int main(int argc, char** argv)
741 DebugOut::setDebugThreshhold(7);
742 GpsNmeaSource plugin(nullptr, std::map<std::string, std::string>());