#include "timestamp.h"
#include "serialport.hpp"
#include "bluetooth.hpp"
+#include "bluetooth5.h"
+#include <listplusplus.h>
+#include <superptr.hpp>
+#include <ambplugin.h>
+
+#include <dbusplugin.h>
+#include <dbusexport.h>
#include <iostream>
#include <boost/assert.hpp>
#include "abstractpropertytype.h"
#define GPSTIME "GpsTime"
+#define GPSSPEED "GpsSpeed"
+#define GpsFix "GpsFix"
+#define GpsSatsUsed "GpsSatsUsed"
+#define GpsNmea "GpsNmea"
template<typename T2>
inline T2 lexical_cast(const std::string &in) {
return out;
}
+class GpsInfo: public DBusSink
+{
+public:
+ GpsInfo(VehicleProperty::Property, AbstractRoutingEngine* re, GDBusConnection* connection)
+ :DBusSink("GpsInfo", re, connection, map<string, string>())
+ {
+ wantPropertyVariant(GPSTIME, "GpsTime", VariantType::Read);
+ wantPropertyVariant(GPSSPEED, "Speed", VariantType::Read);
+ wantPropertyVariant(GpsFix, "Fix", VariantType::Read);
+ wantPropertyVariant(GpsSatsUsed, "SattelitesUsed", VariantType::Read);
+ wantPropertyVariant(GpsNmea, "RawNmea", VariantType::Read);
+ }
+};
+
class Location
{
public:
- Location(AbstractRoutingEngine* re, std::string uuid);
+ enum FixType {
+ NoFix = 1,
+ Fix2D = 2,
+ Fix3D = 3
+ };
+
+ Location(AmbPluginImpl *source, std::shared_ptr<AbstractPropertyType> lat,
+ std::shared_ptr<AbstractPropertyType> lon,
+ std::shared_ptr<AbstractPropertyType> alt,
+ std::shared_ptr<AbstractPropertyType> dir,
+ std::shared_ptr<AbstractPropertyType> spd,
+ std::shared_ptr<AbstractPropertyType> time,
+ std::shared_ptr<AbstractPropertyType> fix,
+ std::shared_ptr<AbstractPropertyType> satsUsed,
+ std::shared_ptr<AbstractPropertyType> vspd);
void parse(std::string gprmc);
- VehicleProperty::LatitudeType latitude()
+ AbstractPropertyType * latitude()
{
- return mLatitude;
+ return mLatitude.get();
}
- VehicleProperty::LongitudeType longitude()
+ AbstractPropertyType * longitude()
{
- return mLongitude;
+ return mLongitude.get();
}
- VehicleProperty::AltitudeType altitude()
+ AbstractPropertyType * altitude()
{
- return mAltitude;
+ return mAltitude.get();
}
- VehicleProperty::DirectionType direction()
+ AbstractPropertyType * direction()
{
- return mDirection;
+ return mDirection.get();
}
- VehicleProperty::VehicleSpeedType speed()
+ AbstractPropertyType * speed()
{
- return mSpeed;
+ return mSpeed.get();
}
- BasicPropertyType<double> gpsTime()
+ AbstractPropertyType * gpsTime()
{
- return mGpsTime;
+ return mGpsTime.get();
}
std::list<AbstractPropertyType*> fix()
{
std::list<AbstractPropertyType*> l;
- l.push_back(&mLatitude);
- l.push_back(&mLongitude);
- l.push_back(&mAltitude);
- l.push_back(&mDirection);
- l.push_back(&mSpeed);
- l.push_back(&mGpsTime);
+ l.push_back(mLatitude.get());
+ l.push_back(mLongitude.get());
+ l.push_back(mAltitude.get());
+ l.push_back(mDirection.get());
+ l.push_back(mSpeed.get());
+ l.push_back(mGpsTime.get());
return l;
}
void parseGprmc(string gprmc);
void parseGpgga(string gpgga);
+ void parseGpgsa(string gpgsa);
void parseTime(std::string h, std::string m, std::string s, string dd, string mm, string yy);
void parseLatitude(std::string d, std::string m, std::string ns);
private:
- VehicleProperty::LatitudeType mLatitude;
- VehicleProperty::LongitudeType mLongitude;
- VehicleProperty::AltitudeType mAltitude;
- VehicleProperty::DirectionType mDirection;
- VehicleProperty::VehicleSpeedType mSpeed;
- BasicPropertyType<double> mGpsTime;
+ std::shared_ptr<AbstractPropertyType> mLatitude;
+ std::shared_ptr<AbstractPropertyType> mLongitude;
+ std::shared_ptr<AbstractPropertyType> mAltitude;
+ std::shared_ptr<AbstractPropertyType> mDirection;
+ std::shared_ptr<AbstractPropertyType> mSpeed;
+ std::shared_ptr<AbstractPropertyType> mVehicleSpeed;
+ std::shared_ptr<AbstractPropertyType> mGpsTime;
+ std::shared_ptr<AbstractPropertyType> mFix;
+ std::shared_ptr<AbstractPropertyType> mSatelitesUsed;
bool isActive;
- std::string mUuid;
-
- AbstractRoutingEngine* routingEngine;
+ AmbPluginImpl * parent;
};
-Location::Location(AbstractRoutingEngine* re, std::string uuid)
- :mLatitude(0), mLongitude(0), mAltitude(0), mDirection(0), mSpeed(0), mGpsTime(GPSTIME,0), isActive(false), routingEngine(re), mUuid(uuid)
+Location::Location(AmbPluginImpl* source,
+ std::shared_ptr<AbstractPropertyType> lat,
+ std::shared_ptr<AbstractPropertyType> lon,
+ std::shared_ptr<AbstractPropertyType> alt,
+ std::shared_ptr<AbstractPropertyType> dir,
+ std::shared_ptr<AbstractPropertyType> spd,
+ std::shared_ptr<AbstractPropertyType> time,
+ std::shared_ptr<AbstractPropertyType> fix,
+ std::shared_ptr<AbstractPropertyType> satsUsed,
+ std::shared_ptr<AbstractPropertyType> vspd)
+ :parent(source), isActive(false)
{
-
+ mLatitude = lat;
+ mLongitude = lon;
+ mAltitude = alt;
+ mDirection = dir;
+ mSpeed = spd;
+ mGpsTime = time;
+ mFix = fix;
+ mSatelitesUsed = satsUsed;
+ mVehicleSpeed = vspd;
}
void Location::parse(string nmea)
{
- if(boost::algorithm::starts_with(nmea,"GPRMC"))
+ if(boost::algorithm::starts_with(nmea,"GPRMC") || boost::algorithm::starts_with(nmea,"GNRMC"))
{
parseGprmc(nmea);
}
{
parseGpgga(nmea);
}
+ else if(boost::algorithm::starts_with(nmea, "GPGSA"))
+ {
+ parseGpgsa(nmea);
+ }
else
{
- DebugOut(DebugOut::Warning)<<"unknown/unhandled message: "<<nmea<<endl;
+ DebugOut(7)<<"unknown/unhandled message: "<<nmea<<endl;
}
}
std::vector<std::string> tokens;
boost::split(tokens, gprmc, boost::is_any_of(","));
+ if(!tokens.size())
+ {
+ return;
+ }
+
if(tokens[2] == "A")
{
isActive = true;
}
+ 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())
+ {
+ return;
+ }
+
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));
parseLatitude(tokens[3], "", tokens[4]);
parseAltitude(tokens[9]);
}
+void Location::parseGpgsa(string gpgsa)
+{
+ std::vector<std::string> tokens;
+ boost::split(tokens, gpgsa, boost::is_any_of(","));
+
+ if(tokens.size() != 18)
+ {
+ DebugOut()<<"Invalid GPGSA message: "<<gpgsa<<endl;
+ return;
+ }
+
+ Location::FixType fix = (Location::FixType)boost::lexical_cast<int>(tokens[2]);
+
+ uint16_t numsats = 0;
+
+ for(int i=3; i<15; i++)
+ {
+ std::string sat = tokens[i];
+ if(!sat.empty())
+ {
+ numsats ++;
+ }
+ }
+
+ if(mFix->value<Location::FixType>() != fix)
+ parent->setValue(mFix, fix);
+
+ if(mSatelitesUsed->value<uint16_t>() != numsats)
+ parent->setValue(mSatelitesUsed, numsats);
+}
+
void Location::parseTime(string h, string m, string s, string dd, string mm, string yy)
{
try
t.tm_min = boost::lexical_cast<int>(m);
t.tm_sec = boost::lexical_cast<int>(s);
t.tm_mday = boost::lexical_cast<int>(dd);
- t.tm_mon = boost::lexical_cast<int>(mm);
+ t.tm_mon = boost::lexical_cast<int>(mm) - 1;
t.tm_year = boost::lexical_cast<int>(yy) + 100;
- time_t time = mktime(&t);
-
- BasicPropertyType<double> temp(GPSTIME,(double)time);
+ time_t time = timegm(&t);
- if(mGpsTime != temp)
+ if(mGpsTime->value<double>() != (double(time)))
{
- mGpsTime = temp;
- routingEngine->updateProperty(&mGpsTime, mUuid);
+ parent->setValue(mGpsTime, double(time));
}
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"Failed to parse time "<<endl;
+ DebugOut(5)<<"Failed to parse time "<<endl;
}
}
if(ns == "S")
dec *= -1;
- VehicleProperty::LatitudeType temp(dec);
-
- if(mLatitude != temp)
+ if(mLatitude->value<double>() != dec)
{
- mLatitude = temp;\
- routingEngine->updateProperty(&mLatitude, mUuid);
+ parent->setValue(mLatitude, dec);
}
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"Failed to parse latitude"<<endl;
+ DebugOut(5)<<"Failed to parse latitude"<<endl;
}
}
if(ew == "W")
dec *= -1;
- VehicleProperty::LongitudeType temp(dec);
-
- if(mLongitude != temp)
+ if(mLongitude->value<double>() != dec)
{
- mLongitude = temp;\
- routingEngine->updateProperty(&mLongitude, mUuid);
+ parent->setValue(mLongitude, dec);
}
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"failed to parse longitude"<<endl;
+ DebugOut(5)<<"failed to parse longitude: "<<d<<" "<<m<<" "<<ew<<endl;
}
}
///to kph:
s *= 1.852;
- VehicleProperty::VehicleSpeedType temp(s);
- if(mSpeed != temp)
+
+ uint16_t speed = static_cast<uint16_t>(s);
+
+ if(mSpeed->value<uint16_t>() != speed)
{
- mSpeed = temp;
- routingEngine->updateProperty(&mSpeed, mUuid);
+ parent->setValue(mSpeed, speed);
}
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"failed to parse speed"<<endl;
+ DebugOut(5)<<"failed to parse speed"<<endl;
}
}
try {
uint16_t d = boost::lexical_cast<double>(dir);
- VehicleProperty::DirectionType temp(d);
- if(mDirection != temp)
+ if(mDirection->value<uint16_t>() != d)
{
- mDirection = temp;
- routingEngine->updateProperty(&mDirection, mUuid);
+ parent->setValue(mDirection, d);
}
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"Failed to parse direction: "<<dir<<endl;
+ DebugOut(5) << "Failed to parse direction: " << dir << endl;
}
}
double a = boost::lexical_cast<double>(alt);
- VehicleProperty::AltitudeType temp(a);
- if(mAltitude != temp)
+ if(mAltitude->value<double>() != a)
{
- mAltitude = temp;
- routingEngine->updateProperty(&mAltitude, mUuid);
+ parent->setValue(mAltitude, a);
}
-
- mAltitude = VehicleProperty::AltitudeType(a);
}
catch(...)
{
- DebugOut(DebugOut::Warning)<<"failed to parse altitude"<<endl;
+ DebugOut(5)<<"failed to parse altitude"<<endl;
}
}
return true;
}
-extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
+extern "C" void create(AbstractRoutingEngine* routingengine, map<string, string> config)
{
- return new GpsNmeaSource(routingengine, config);
-
+ auto plugin = new AmbPlugin<GpsNmeaSource>(routingengine, config);
+ plugin->init();
}
-GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
- :AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
+GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config, AbstractSource &parent)
+ :AmbPluginImpl(re, config, parent), mUuid("33d86462-1708-4f78-a001-99ea8d55422b"), device(nullptr), bt(nullptr)
{
- location =new Location(re, mUuid);
+ int baudrate = 0;
- VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
+ auto lat = addPropertySupport<VehicleProperty::LatitudeType>(Zone::None);
+ auto lon = addPropertySupport<VehicleProperty::LongitudeType>(Zone::None);
+ auto alt = addPropertySupport<VehicleProperty::AltitudeType>(Zone::None);
+ auto spd = addPropertySupport(Zone::None, [](){ return new BasicPropertyType<uint16_t>(GPSSPEED, 0); });
+ auto vspd = addPropertySupport<VehicleProperty::VehicleSpeedType>(Zone::None);
+ auto dir = addPropertySupport<VehicleProperty::DirectionType>(Zone::None);
+ auto time = addPropertySupport(Zone::None, [](){ return new BasicPropertyType<double>(GPSTIME, 0); });
+ auto fix = addPropertySupport(Zone::None, []() { return new BasicPropertyType<Location::FixType>(GpsFix, Location::NoFix); });
+ auto satsUsed = addPropertySupport(Zone::None, []() { return new BasicPropertyType<uint16_t>(GpsSatsUsed, 0); });
+ rawNmea = addPropertySupport(Zone::None, []() { return new StringPropertyType(GpsNmea); });
- addPropertySupport(VehicleProperty::Latitude, Zone::None);
- addPropertySupport(VehicleProperty::Longitude, Zone::None);
- addPropertySupport(VehicleProperty::Altitude, Zone::None);
- addPropertySupport(VehicleProperty::VehicleSpeed, Zone::None);
- addPropertySupport(VehicleProperty::Direction, Zone::None);
- addPropertySupport(GPSTIME, Zone::None);
+ location = new Location(this, lat, lon, alt, dir, spd, time, fix, satsUsed, vspd);
+ std::string btaddapter = config["bluetoothAdapter"];
- ///test:
-
- if(config.find("test") != config.end())
+ if(config.find("baudrate")!= config.end())
{
- Location location(routingEngine, mUuid);
- location.parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
-
- DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
-
- g_assert(location.latitude().toString() == "-23.86600833");
- g_assert(location.gpsTime().toString() == "1050585131");
-
- location.parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
-
- DebugOut(0)<<"alt: "<<location.altitude().toString()<<endl;
- DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
- g_assert(location.altitude().toString() == "545.4");
- g_assert(location.latitude().toString() == "48.1173");
-
- location.parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
- DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
- DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
-
- //Test incomplete message:
- location.parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
- DebugOut(0)<<"lon: "<<location.longitude().toString()<<endl;
- DebugOut(0)<<"lat: "<<location.latitude().toString()<<endl;
+ baudrate = boost::lexical_cast<int>( config["baudrate"] );
}
- std::string btaddapter = config["bluetoothAdapter"];
-
if(config.find("device")!= config.end())
{
std::string dev = config["device"];
+
if(dev.find(":") != string::npos)
{
- BluetoothDevice bt;
- dev = bt.getDeviceForAddress(dev, btaddapter);
+#ifdef USE_BLUEZ5
+ bt = new Bluetooth5();
+ bt->getDeviceForAddress(dev, [this](int fd) {
+ DebugOut() << "fd: " << fd << endl;
+ device = new SerialPort(fd);
+ int baudrate=0;
+
+ if(baudrate!=0)
+ {
+ if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
+ DebugOut(DebugOut::Error)<<"Unsupported baudrate " << configuration["baudrate"] << endl;
+ }
+
+ DebugOut()<<"read from device: "<<device->read()<<endl;
+
+ GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
+ g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
+ g_io_channel_set_close_on_unref(chan, true);
+ g_io_channel_unref(chan);
+ });
+#else
+ bt = new BluetoothDevice();
+ dev = bt->getDeviceForAddress(dev, btaddapter);
+
+ device = new SerialPort(dev);
+
+ if(baudrate!=0)
+ {
+ if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
+ DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
+ }
+
+ if(!device->open())
+ {
+ DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
+ perror("Error");
+ return;
+ }
+
+ DebugOut()<<"read from device: "<<device->read()<<endl;
+
+ GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
+ g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
+ g_io_channel_set_close_on_unref(chan, true);
+ g_io_channel_unref(chan);
+#endif
}
-
- device = new SerialPort(dev);
-
- if(!device->open())
+ else
{
- DebugOut(DebugOut::Error)<<"Failed to open gps tty: "<<config["device"]<<endl;
- perror("Error");
- return;
+ device = new SerialPort(dev);
+
+ if(baudrate!=0)
+ {
+ if((static_cast<SerialPort*>(device))->setSpeed(baudrate))
+ DebugOut(DebugOut::Error)<<"Unsupported baudrate " << config["baudrate"] << endl;
+ }
+
+ if(!device->open())
+ {
+ DebugOut(DebugOut::Error) << "Failed to open gps tty: " << config["device"] << endl;
+ perror("Error");
+ return;
+ }
+
+ DebugOut()<<"read from device: "<<device->read()<<endl;
+
+ GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
+ g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR), (GIOFunc)readCallback, this);
+ g_io_channel_set_close_on_unref(chan, true);
+ g_io_channel_unref(chan);
}
-
- DebugOut()<<"read from device: "<<device->read()<<endl;
-
- GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
- g_io_add_watch(chan, GIOCondition(G_IO_IN | G_IO_HUP | G_IO_ERR),(GIOFunc)readCallback, this);
- g_io_channel_set_close_on_unref(chan, true);
- g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
}
-
- re->setSupported(supported(), this);
}
GpsNmeaSource::~GpsNmeaSource()
{
- device->close();
+ if(device && device->isOpen())
+ device->close();
+ if(bt)
+ delete bt;
}
-const string GpsNmeaSource::uuid()
+const string GpsNmeaSource::uuid() const
{
return mUuid;
}
-
-void GpsNmeaSource::getPropertyAsync(AsyncPropertyReply *reply)
+int GpsNmeaSource::supportedOperations() const
{
- DebugOut()<<"GpsNmeaSource: getPropertyAsync called for property: "<<reply->property<<endl;
+ return AbstractSource::Get;
+}
- std::list<AbstractPropertyType*> f = location->fix();
+void GpsNmeaSource::init()
+{
+ if(configuration.find("test") != configuration.end())
+ {
+ test();
+ }
- for(auto property : f)
+ routingEngine->subscribeToProperty(DBusConnected,[this](AbstractPropertyType* value)
{
- if(property->name == reply->property)
+ if(value->name == DBusConnected)
{
- reply->success = true;
- reply->value = property;
- reply->completed(reply);
- return;
+ if(value->value<bool>())
+ {
+ amb::Exporter::instance()->exportProperty<GpsInfo>(routingEngine);
+ }
}
- }
-
- reply->success = false;
- reply->error = AsyncPropertyReply::InvalidOperation;
- reply->completed(reply);
+ });
}
-void GpsNmeaSource::getRangePropertyAsync(AsyncRangePropertyReply *reply)
+void GpsNmeaSource::canHasData()
{
+ std::string data = device->read();
+ tryParse(data);
}
-AsyncPropertyReply *GpsNmeaSource::setProperty(AsyncSetPropertyRequest request )
+void GpsNmeaSource::test()
{
+ location->parse("GPRMC,061211,A,2351.9605,S,15112.5239,E,000.0,053.4,170303,009.9,E*6E");
-}
+ DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
+ DebugOut(0)<<"lat: "<<location->gpsTime()->toString()<<endl;
-void GpsNmeaSource::subscribeToPropertyChanges(VehicleProperty::Property property)
-{
- mRequests.push_back(property);
-}
+ g_assert(location->latitude()->toString() == "-23.86600833");
+ g_assert(location->gpsTime()->toString() == "1050585131");
-PropertyList GpsNmeaSource::supported()
-{
- return mSupported;
-}
+ location->parse("GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
-int GpsNmeaSource::supportedOperations()
-{
- return Get;
+ DebugOut(0)<<"alt: "<<location->altitude()->toString()<<endl;
+ DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
+ g_assert(location->altitude()->toString() == "545.4");
+ g_assert(location->latitude()->toString() == "48.1173");
+
+ location->parse("GPRMC,060136.00,A,3101.40475,N,12126.87095,E,0.760,,160114,,,A*74");
+ DebugOut(0)<<"lon: "<<location->longitude()->toString()<<endl;
+ DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
+
+ ///test gpgsa
+ location->parse("GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39");
+
+ //Test incomplete message:
+ location->parse("GPRMC,023633.00,V,,,,,,,180314,,,N*75");
+ DebugOut(0)<<"lon: "<<location->longitude()->toString()<<endl;
+ DebugOut(0)<<"lat: "<<location->latitude()->toString()<<endl;
+
+ std::string testChecksuming = "GPRMC,195617.00,V,,,,,,,310314,,,N*74";
+
+ g_assert(checksum(testChecksuming));
+
+ std::string multimessage1 = "GA,235320.00,4532.48633,N,12257.";
+ std::string multimessage2 = "57383,W,";
+ std::string multimessage3 = "1,03,7.53,51.6,M,-21.3,M,,*55";
+ std::string multimessage4 = "GPGSA,A,";
+ std::string multimessage5 = "2,27,23,19,,,,,,,,,,7.60";
+ std::string multimessage6 = ",7.53,1.00*";
+ std::string multimessage7 = "0E";
+
+ bool multimessageParse = false;
+
+ multimessageParse |= tryParse(multimessage1);
+ multimessageParse |= tryParse(multimessage2);
+ multimessageParse |= tryParse(multimessage3);
+ multimessageParse |= tryParse(multimessage4);
+ multimessageParse |= tryParse(multimessage5);
+ multimessageParse |= tryParse(multimessage6);
+ multimessageParse |= tryParse(multimessage7);
+
+ g_assert(multimessageParse);
+
+ //Test meaningingless message:
+ location->parse("GPRMC,,V,,,,,,,,,,N*53");
+
+ //test false message:
+
+ g_assert(!checksum("GPRMC,172758.296,V"));
}
-void GpsNmeaSource::canHasData()
+bool GpsNmeaSource::tryParse(string data)
{
- std::string data = device->read();
-
std::vector<std::string> lines;
- boost::split(lines,data,boost::is_any_of("$"));
+ boost::split(lines, data, boost::is_any_of("$"));
+
+ bool weFoundAMessage = false;
- for(int i = 0; i < lines.size(); i++)
+ for(auto line : lines)
{
- if(checksum(lines[i]))
+ if(checksum(line))
{
- buffer = lines[i];
+ buffer = line;
}
else
{
- buffer += lines[i];
+ buffer += line;
+ }
+
+ std::string::size_type pos = buffer.find('G');
+
+ if(pos != std::string::npos && pos != 0)
+ {
+ ///Throw the incomplete stuff away. if it doesn't begin with "G" it'll never be complete
+ buffer = buffer.substr(pos);
}
if(checksum(buffer))
/// we have a complete message. parse it!
DebugOut(7)<<"Complete message: "<<buffer<<endl;
location->parse(buffer);
+ boost::algorithm::erase_all(buffer, "\n");
+ boost::algorithm::erase_all(buffer, "\r");
+ setValue(rawNmea, buffer);
+ weFoundAMessage = true;
+ buffer = "";
+ }
+ else
+ {
+ if(pos == 0 )
+ {
+ std::string::size_type cs = buffer.find('*');
+ if (cs != std::string::npos && cs != buffer.length()-1)
+ {
+ buffer = buffer.substr(cs+(buffer.length() - cs));
+ }
+ }
}
DebugOut(7)<<"buffer: "<<buffer<<endl;
-
}
-}
-void GpsNmeaSource::unsubscribeToPropertyChanges(VehicleProperty::Property property)
-{
- mRequests.remove(property);
-}
-
-void GpsNmeaSource::addPropertySupport(VehicleProperty::Property property, Zone::Type zone)
-{
- mSupported.push_back(property);
-
- std::list<Zone::Type> zones;
-
- zones.push_back(zone);
-
- PropertyInfo info(0, zones);
-
- propertyInfoMap[property] = info;
+ return weFoundAMessage;
}
bool GpsNmeaSource::checksum(std::string sentence)
{
- if(sentence.empty() || sentence.length() < 4)
+ if(sentence.empty() || sentence.length() < 4 || sentence.find("*") == string::npos || sentence.find("*") >= sentence.length()-2)
{
return false;
}
checksum ^= i;
}
- std::string sentenceCheckStr = sentence.substr(sentence.length()-4,2);
+ std::string sentenceCheckStr = sentence.substr(sentence.find('*')+1,2);
try
{
{
return false;
}
+
+ return false;
}