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