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/regex.hpp>
  27 #include <boost/algorithm/string.hpp>
  28 #include <time.h>
  29
  30
  31 using namespace std;
  32
  33 #include "debugout.h"
  34 #include "abstractpropertytype.h"
  35
  36 #define GPSTIME "GpsTime"
  37
  38 std::string gprmcRegEx = "[\\$]?GPRMC,([0-1][0-9]|2[0-3])([0-5][0-9])([0-5][0-9])," /** time hh mm ss **/
  39                 "([AV])," /** Status A= Active, V= Void **/
  40                 "([0-8][0-9]|90)([0-5][0-9]\\.[0-9]{4})," /** latitude **/
  41                 "([NS])," /** lat North or South **/
  42                 "(180|0[0-9]{2}|1[0-7][0-9])([0-5][0-9]\\.[0-9]{4})," /** longitude **/
  43                 "([EW])," /** lon E or W **/
  44                 "([0-9]{3}\\.[0-9])," /** Speed in knots **/
  45                 "(3[0-5][0-9]|[0-2][0-9]{2}\\.[0-9])," /** Direction **/
  46                 "(3[0-1]|[1-2][0-9]|0[1-9])(0[1-9]|1[0-2])([0-9]{2}),"
  47                 "(180|[1][0-7][0-9]|[0][0-9]{2}\\.[0-9])," /** Magnetic variation **/
  48                 "([EW])" /** Magnetic Direction **/
  49                 "\\*([0-9,A-F]{2})";
  50
  51 class Location
  52 {
  53 public:
  54         Location(AbstractRoutingEngine* re, std::string uuid);
  55
  56         void parse(std::string gprmc);
  57
  58         VehicleProperty::LatitudeType latitude()
  59         {
  60                 return mLatitude;
  61         }
  62
  63         VehicleProperty::LongitudeType longitude()
  64         {
  65                 return mLongitude;
  66         }
  67
  68         VehicleProperty::AltitudeType altitude()
  69         {
  70                 return mAltitude;
  71         }
  72
  73         VehicleProperty::DirectionType direction()
  74         {
  75                 return mDirection;
  76         }
  77
  78         VehicleProperty::VehicleSpeedType speed()
  79         {
  80                 return mSpeed;
  81         }
  82
  83         BasicPropertyType<double> gpsTime()
  84         {
  85                 return mGpsTime;
  86         }
  87
  88 private: ///methods:
  89
  90         void parseGprmc(string gprmc);
  91         void parseGpgga(string gpgga);
  92
  93         void parseTime(std::string h, std::string m, std::string s);
  94         void parseLatitude(std::string d, std::string m, std::string ns);
  95         void parseLongitude(std::string d, string m, string ew);
  96         void parseSpeed(std::string spd);
  97         void parseDirection(std::string dir);
  98         void parseAltitude(std::string alt);
  99
 100         double degsToDecimal(double degs);
 101
 102 private:
 103
 104         VehicleProperty::LatitudeType mLatitude;
 105         VehicleProperty::LongitudeType mLongitude;
 106         VehicleProperty::AltitudeType mAltitude;
 107         VehicleProperty::DirectionType  mDirection;
 108         VehicleProperty::VehicleSpeedType mSpeed;
 109         BasicPropertyType<double> mGpsTime;
 110
 111         boost::regex regularExpression;
 112         bool isActive;
 113
 114         std::string mUuid;
 115
 116         AbstractRoutingEngine* routingEngine;
 117
 118 };
 119
 120 Location::Location(AbstractRoutingEngine* re, std::string uuid)
 121         :mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
 122 {
 123
 124 }
 125
 126 void Location::parse(string nmea)
 127 {
 128         if(boost::algorithm::starts_with(nmea,"GPRMC"))
 129         {
 130                 parseGprmc(nmea);
 131         }
 132         else if(boost::algorithm::starts_with(nmea,"GPGGA"))
 133         {
 134                 parseGpgga(nmea);
 135         }
 136 }
 137
 138 void Location::parseGprmc(string gprmc)
 139 {
 140         regularExpression.assign(gprmcRegEx);
 141
 142         boost::smatch tokens;
 143
 144         if (boost::regex_match (gprmc, tokens, regularExpression) )
 145         {
 146                 parseTime(tokens[1],tokens[2],tokens[3]);
 147
 148                 if(tokens[4] == "A")
 149                 {
 150                         isActive = true;
 151                 }
 152
 153                 parseLatitude(tokens[5], tokens[6], tokens[7]);
 154                 parseLongitude(tokens[8], tokens[9], tokens[10]);
 155                 parseSpeed(tokens[11]);
 156                 parseDirection(tokens[12]);
 157         }
 158 }
 159
 160 void Location::parseGpgga(string gpgga)
 161 {
 162
 163         std::vector<std::string> tokens;
 164         boost::split(tokens, gpgga, boost::is_any_of(","));
 165
 166         if(tokens.size() != 15)
 167         {
 168                 DebugOut()<<"Invalid GPGGA message: "<<gpgga<<endl;
 169                 return;
 170         }
 171
 172         parseTime(tokens[1].substr(0,2),tokens[1].substr(2,2),tokens[1].substr(4,2));
 173         parseLatitude(tokens[2],"",tokens[3]);
 174         parseLongitude(tokens[4],"",tokens[5]);
 175         if(tokens[6] != "0")
 176         {
 177                 isActive = true;
 178         }
 179         else isActive = false;
 180
 181         parseAltitude(tokens[9]);
 182 }
 183
 184 void Location::parseTime(string h, string m, string s)
 185 {
 186         tm t;
 187         t.tm_hour = boost::lexical_cast<int>(h);
 188         t.tm_min = boost::lexical_cast<int>(m);
 189         t.tm_sec = boost::lexical_cast<int>(s);
 190
 191         time_t time = mktime(&t);
 192
 193         BasicPropertyType<double> temp(GPSTIME,(double)time);
 194
 195         if(mGpsTime != temp)
 196         {
 197                 mGpsTime = temp;
 198                 routingEngine->updateProperty(GPSTIME, &mGpsTime, mUuid);
 199         }
 200 }
 201
 202 void Location::parseLatitude(string d, string m, string ns)
 203 {
 204         double degs = boost::lexical_cast<double>(d + m);
 205         double dec = degsToDecimal(degs);
 206
 207         if(ns == "S")
 208                 dec *= -1;
 209
 210         VehicleProperty::LatitudeType temp(dec);
 211
 212         if(mLatitude != temp)
 213         {
 214                 mLatitude = temp;\
 215                 routingEngine->updateProperty(VehicleProperty::Latitude, &mLatitude, mUuid);
 216         }
 217 }
 218
 219 void Location::parseLongitude(string d, string m, string ew)
 220 {
 221         double degs = boost::lexical_cast<double>(d + m);
 222         double dec = degsToDecimal(degs);
 223
 224         if(ew == "W")
 225                 dec *= -1;
 226
 227         VehicleProperty::LongitudeType temp(dec);
 228
 229         if(mLongitude != temp)
 230         {
 231                 mLongitude = temp;\
 232                 routingEngine->updateProperty(VehicleProperty::Longitude, &mLongitude, mUuid);
 233         }
 234 }
 235
 236 void Location::parseSpeed(string spd)
 237 {
 238         double s = boost::lexical_cast<double>(spd);
 239
 240         ///to kph:
 241         s *= 1.852;
 242         VehicleProperty::VehicleSpeedType temp(s);
 243         if(mSpeed != temp)
 244         {
 245                 mSpeed = temp;
 246                 routingEngine->updateProperty(VehicleProperty::VehicleSpeed, &mSpeed, mUuid);
 247         }
 248 }
 249
 250 void Location::parseDirection(string dir)
 251 {
 252         uint16_t d = boost::lexical_cast<double>(dir);
 253
 254         VehicleProperty::DirectionType temp(d);
 255         if(mDirection != temp)
 256         {
 257                 mDirection = temp;
 258                 routingEngine->updateProperty(VehicleProperty::Direction, &mDirection, mUuid);
 259         }
 260 }
 261
 262 void Location::parseAltitude(string alt)
 263 {
 264         double a = boost::lexical_cast<double>(alt);
 265
 266         VehicleProperty::AltitudeType temp(a);
 267         if(mAltitude != temp)
 268         {
 269                 mAltitude = temp;
 270                 routingEngine->updateProperty(VehicleProperty::Altitude, &mAltitude, mUuid);
 271         }
 272
 273         mAltitude = VehicleProperty::AltitudeType(a);
 274 }
 275
 276 double Location::degsToDecimal(double degs)
 277 {
 278         double deg;
 279         double min = 100.0 * modf(degs / 100.0, &deg);
 280         return deg + (min / 60.0);
 281 }
 282
 283 bool readCallback(GIOChannel *source, GIOCondition condition, gpointer data)
 284 {
 285 //      DebugOut(5) << "Polling..." << condition << endl;
 286
 287         if(condition & G_IO_ERR)
 288         {
 289                 DebugOut(DebugOut::Error)<<"GpsNmeaSource polling error."<<endl;
 290         }
 291
 292         if (condition & G_IO_HUP)
 293         {
 294                 //Hang up. Returning false closes out the GIOChannel.
 295                 //printf("Callback on G_IO_HUP\n");
 296                 DebugOut(DebugOut::Warning)<<"socket hangup event..."<<endl;
 297                 return false;
 298         }
 299
 300         GpsNmeaSource* src = static_cast<GpsNmeaSource*>(data);
 301
 302         src->canHasData();
 303
 304         return true;
 305 }
 306
 307 extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
 308 {
 309         return new GpsNmeaSource(routingengine, config);
 310
 311 }
 312
 313 GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
 314         :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
 315 {
 316         location =new Location(re, mUuid);
 317
 318         VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
 319
 320         addPropertySupport(VehicleProperty::Latitude, Zone::None);
 321         addPropertySupport(VehicleProperty::Longitude, Zone::None);
 322         addPropertySupport(VehicleProperty::Altitude, Zone::None);
 323         addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
 324         addPropertySupport(VehicleProperty::Direction, Zone::None);
 325         addPropertySupport(GPSTIME, Zone::None);
 326
 327
 328         ///test:
 329
 330         if(config.find("test") != config.end())
 331         {
 332                 Location location(routingEngine, mUuid);
 333                 location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
 334
 335                 DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
 336
 337                 g_assert(location.latitude().toString() == "-23.86600833");
 338
 339                 location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
 340
 341                 DebugOut()<<"alt: "<<location.altitude().toString()<<endl;
 342                 DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
 343                 g_assert(location.altitude().toString() == "545.4");
 344                 g_assert(location.latitude().toString() == "48.1173");
 345         }
 346
 347         std::string btaddapter = config["bluetoothAdapter"];
 348
 349         if(config.find("device")!= config.end())
 350         {
 351                 std::string dev = config["device"];
 352                 if(dev.find(":") != string::npos)
 353                 {
 354                         BluetoothDevice bt;
 355                         dev = bt.getDeviceForAddress(dev, btaddapter);
 356                 }
 357
 358                 device = new SerialPort(dev);
 359
 360                 if(!device->open())
 361                 {
 362                         DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
 363                         perror("Error");
 364                         return;
 365                 }
 366
 367                 DebugOut()<<"read from device: "<<device->read()<<endl;
 368
 369                 GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
 370                 g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
 371                 g_io_channel_set_close_on_unref(chan, true);
 372                 g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
 373         }
 374
 375         re->setSupported(supported(), this);
 376 }
 377
 378 GpsNmeaSource::~GpsNmeaSource()
 379 {
 380         device->close();
 381 }
 382
 383 string GpsNmeaSource::uuid()
 384 {
 385         return mUuid;
 386 }
 387
 388
 389 void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
 390 {
 391         DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
 392
 393 }
 394
 395 void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
 396 {
 397
 398 }
 399
 400 AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
 401 {
 402
 403 }
 404
 405 void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
 406 {
 407         mRequests.push_back(property);
 408 }
 409
 410 PropertyList GpsNmeaSource::supported()
 411 {
 412         return mSupported;
 413 }
 414
 415 int GpsNmeaSource::supportedOperations()
 416 {
 417         return Get;
 418 }
 419
 420 void GpsNmeaSource::canHasData()
 421 {
 422         std::string data = device->read();
 423
 424         std::vector<std::string> lines;
 425
 426         boost::split(lines,data,boost::is_any_of("$"));
 427
 428         for(int i = 0; i < lines.size(); i++)
 429         {
 430                 location->parse(lines[i]);
 431         }
 432 }
 433
 434 void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
 435 {
 436         mRequests.remove(property);
 437 }
 438
 439 void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
 440 {
 441         mSupported.push_back(property);
 442
 443         std::list<Zone::Type> zones;
 444
 445         zones.push_back(zone);
 446
 447         PropertyInfo info(0, zones);
 448
 449         propertyInfoMap[property] = info;
 450 }