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