GpsNmeaSource::GpsNmeaSource(AbstractRoutingEngine *re, map<string, string> config)
:AbstractSource(re,config), mUuid("33d86462-1708-4f78-a001-99ea8d55422b")
{
location =new Location(re, mUuid);
VehicleProperty::registerProperty(GPSTIME,[](){ return new BasicPropertyType<double>(GPSTIME,0); });
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);
///test:
if(config.find("test") != 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()<<"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()<<"alt: "<<location.altitude().toString()<<endl;
DebugOut()<<"lat: "<<location.latitude().toString()<<endl;
g_assert(location.altitude().toString() == "545.4");
g_assert(location.latitude().toString() == "48.1173");
}
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);
}
device = new SerialPort(dev);
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); //Pass ownership of the GIOChannel to the watch.
}
re->setSupported(supported(), this);
}
void threadLoop(gpointer data)
{
GAsyncQueue *privCommandQueue = g_async_queue_ref(((OBD2Source*)data)->commandQueue);
GAsyncQueue *privResponseQueue = g_async_queue_ref(((OBD2Source*)data)->responseQueue);
GAsyncQueue *privSingleShotQueue = g_async_queue_ref(((OBD2Source*)data)->singleShotQueue);
GAsyncQueue *privSubscriptionAddQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionAddQueue);
GAsyncQueue *privSubscriptionRemoveQueue = g_async_queue_ref(((OBD2Source*)data)->subscriptionRemoveQueue);
GAsyncQueue *privStatusQueue = g_async_queue_ref(((OBD2Source*)data)->statusQueue);
obdLib *obd = new obdLib();
OBD2Source *source = (OBD2Source*)data;
obd->setCommsCallback([](const char* mssg, void* data) { DebugOut(6)<<mssg<<endl; },NULL);
obd->setDebugCallback([](const char* mssg, void* data, obdLib::DebugLevel debugLevel) { DebugOut(debugLevel)<<mssg<<endl; },NULL);
std::list<ObdPid*> reqList;
std::list<ObdPid*> repeatReqList;
ObdPid::ByteArray replyVector;
std::string reply;
std::string port;
std::string baud;
bool connected=false;
int emptycount = 0;
int timeoutCount = 0;
while (source->m_threadLive)
{
//gpointer query = g_async_queue_pop(privCommandQueue);
gpointer query = g_async_queue_try_pop(privSingleShotQueue);
if (query != nullptr)
{
//printf("Got request!\n");
ObdPid *req = (ObdPid*)query;
DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got single shot request: " << req->pid.substr(0,req->pid.length()-1) << ":" << req->property <<endl;
repeatReqList.push_back(req);
}
query = g_async_queue_try_pop(privSubscriptionAddQueue);
if (query != nullptr)
{
ObdPid *req = (ObdPid*)query;
//DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got subscription request for "<<req->req<<endl;
reqList.push_back(req);
}
query = g_async_queue_try_pop(privCommandQueue);
if (query != nullptr)
{
//ObdPid *req = (ObdPid*)query;
CommandRequest *req = (CommandRequest*)query;
//commandMap[req->req] = req->arg;
//printf("Command: %s\n",req->req.c_str());
DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Command:" << req->req << endl;
if (req->req == "connect" )
{
if (source->m_isBluetooth)
{
BluetoothDevice bt;
std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress);
if(tempPort != "")
{
DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl;
port = tempPort;
}
}
else
{
port = req->arglist[0];
baud = req->arglist[1];
}
connected = connect(obd,port,baud);
if(connected)
{
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "connected";
g_async_queue_push(privStatusQueue,statusreq);
}
else
{
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "disconnected";
g_async_queue_push(privStatusQueue,statusreq);
}
}
else if (req->req == "connectifnot")
{
if (!connected)
{
if (source->m_isBluetooth)
{
BluetoothDevice bt;
std::string tempPort = bt.getDeviceForAddress(source->m_btDeviceAddress, source->m_btAdapterAddress);
if(tempPort != "")
{
DebugOut(3)<<"Using bluetooth device \""<<source->m_btDeviceAddress<<"\" bound to: "<<tempPort<<endl;
port = tempPort;
}
else
{
DebugOut(DebugOut::Error)<<"Error creating bluetooth device"<<endl;
continue;
}
}
connected = connect(obd,port,baud);
if(connected)
{
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "connected";
g_async_queue_push(privStatusQueue,statusreq);
}
else
{
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "disconnected";
g_async_queue_push(privStatusQueue,statusreq);
}
}
}
else if (req->req == "setportandbaud")
{
port = req->arglist[0];
baud = req->arglist[1];
}
else if (req->req == "disconnect")
{
DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Using queued disconnect" << (ulong)req << endl;
obd->closePort();
BluetoothDevice bt;
bt.disconnect(source->m_btDeviceAddress, source->m_btAdapterAddress);
connected = false;
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "disconnected";
g_async_queue_push(privStatusQueue,statusreq);
}
delete req;
}
query = g_async_queue_try_pop(privSubscriptionRemoveQueue);
if (query != nullptr)
{
DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Got unsubscription request"<<endl;
ObdPid *req = (ObdPid*)query;
for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
{
if ((*i)->property == req->property)
{
reqList.erase(i);
delete (*i);
i--;
if (reqList.size() == 0)
{
break;
}
}
}
//reqList.push_back(req->req);
delete req;
}
if (reqList.size() > 0 && !connected)
{
/*CommandRequest *req = new CommandRequest();
req->req = "connect";
req->arglist.push_back(port);
req->arglist.push_back(baud);
g_async_queue_push(privCommandQueue,req);
continue;*/
}
else if (reqList.size() == 0 && connected)
{
emptycount++;
if (emptycount < 1000)
{
usleep(10000);
continue;
}
emptycount = 0;
CommandRequest *req = new CommandRequest();
req->req = "disconnect";
g_async_queue_push(privCommandQueue,req);
continue;
}
if (!connected)
{
usleep(10000);
continue;
}
for (std::list<ObdPid*>::iterator i=reqList.begin();i!= reqList.end();i++)
{
repeatReqList.push_back(*i);
}
int badloop = 0;
for (std::list<ObdPid*>::iterator i=repeatReqList.begin();i!= repeatReqList.end();i++)
{
DebugOut(10) << __SMALLFILE__ << ":" << __LINE__ << "Requesting pid: " << (*i)->pid.substr(0,(*i)->pid.length()-1) << (*i)->property << endl;
if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
{
//Don't erase the pid, just skip over it.
int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
if (count > 10)
{
continue;
}
}
badloop++;
bool result = false;
if(beginsWith((*i)->pid,"AT") || beginsWith((*i)->pid, "ST"))
{
result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector);
}
else result = obd->sendObdRequestString((*i)->pid.c_str(),(*i)->pid.length(),&replyVector,5,3);
if (!result)
{
//This only happens during a error with the com port. Close it and re-open it later.
DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Unable to send request:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
if (obd->lastError() == obdLib::NODATA)
{
DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::NODATA for pid" << (*i)->pid.substr(0,(*i)->pid.length()-1) << " expected property: " << (*i)->property << endl;
if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
{
//pid value i not yet in the list.
int count = (*source->m_blacklistPidCountMap.find((*i)->pid)).second;
if (count > 10)
{
}
source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,count));
}
else
{
source->m_blacklistPidCountMap.insert(pair<std::string,int>((*i)->pid,1));
}
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "error:nodata";
statusreq->property = (*i)->property;
g_async_queue_push(privStatusQueue,statusreq);
continue;
}
else if (obd->lastError() == obdLib::TIMEOUT)
{
timeoutCount++;
if (timeoutCount < 2)
{
DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBDLib::TIMEOUT for pid" << (*i)->pid << endl;
StatusMessage *statusreq = new StatusMessage();
statusreq->statusStr = "error:timeout";
g_async_queue_push(privStatusQueue,statusreq);
continue;
}
}
else
{
DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "OBD Other error:" << obd->lastError() << endl;
}
CommandRequest *req = new CommandRequest();
DebugOut() << __SMALLFILE__ << ":" << __LINE__ << "Queuing up a disconnect" << (ulong)req << endl;
req->req = "disconnect";
g_async_queue_push(privCommandQueue,req);
i = repeatReqList.end();
i--;
continue;
}
if (source->m_blacklistPidCountMap.find((*i)->pid) != source->m_blacklistPidCountMap.end())
{
//If we get the pid response, then we want to clear out the blacklist list.
source->m_blacklistPidCountMap.erase(source->m_blacklistPidCountMap.find((*i)->pid));
}
timeoutCount = 0;
//ObdPid *pid = ObdPid::pidFromReply(replyVector);
ObdPid *pid = obd2AmbInstance->createPidFromReply(replyVector);
if (!pid)
{
//Invalid reply
DebugOut() << "Invalid reply"<<endl;
continue;
}
else
{
DebugOut(11) << __SMALLFILE__ <<":"<< __LINE__ << "Reply recieved and queued for:" << (*i)->pid.substr(0,(*i)->pid.length()-1) << endl;
std::string repstr;
for (int i=0;i<replyVector.size();i++)
{
if (replyVector[i] != 13)
{
repstr += (char)replyVector[i];
}
//DebugOut(11) << replyVector[i];
}
DebugOut(11) << "Reply:" << repstr << endl;
}
g_async_queue_push(privResponseQueue,pid);
}
if (badloop == 0)
{
//We had zero non-blacklisted events. Pause for a moment here to keep from burning CPU.
//usleep(10000);
}
repeatReqList.clear();
}
if (connected)
{
obd->closePort();
}
}
OpenXCPlugin::OpenXCPlugin(AbstractRoutingEngine* re, map<string, string> config)
:AbstractSource(re, config)
{
re->setSupported(supported(), this);
/// populate the openxc to amb map:
openXC2AmbMap["steering_wheel_angle"] = VehicleProperty::SteeringWheelAngle;
//openXC2AmbMap["torque_at_transmission"] = VehicleProperty::Engine;
openXC2AmbMap["engine_speed"] = VehicleProperty::EngineSpeed;
openXC2AmbMap["vehicle_speed"] = VehicleProperty::VehicleSpeed;
openXC2AmbMap["accelerator_pedal_position"] = VehicleProperty::ThrottlePosition;
openXC2AmbMap["parking_brake_status"] = VehicleProperty::ParkingBrakeStatus;
openXC2AmbMap["brake_pedal_status"] = VehicleProperty::LightBrake;
openXC2AmbMap["transmission_gear_position"] = VehicleProperty::TransmissionGearPosition;
openXC2AmbMap["odometer"] = VehicleProperty::Odometer;
openXC2AmbMap["ignition_status"] = VehicleProperty::VehiclePowerMode;
openXC2AmbMap["fuel_level"] = VehicleProperty::FuelLevel;
openXC2AmbMap["fuel_consumed_since_restart"] = VehicleProperty::FuelConsumption;
openXC2AmbMap["headlamp_status"] = VehicleProperty::LightHead;
openXC2AmbMap["high_beam_status"] = VehicleProperty::LightHighBeam;
openXC2AmbMap["windshield_wiper_status"] = VehicleProperty::WindshieldWiper;
openXC2AmbMap["latitude"] = VehicleProperty::Latitude;
openXC2AmbMap["longitude"] = VehicleProperty::Longitude;
openXC2AmbMap["button_event"] = VehicleProperty::ButtonEvent;
bool test = false;
if(config.find("test") != config.end())
{
test = config["test"] == "true";
}
if(test)
{
testParseEngine();
}
std::string bluetoothAddy = config["device"];
std::string bluetoothAdapter = config["bluetoothAdapter"];
std::string serialDevice;
if(bluetoothAddy != "")
{
BluetoothDevice btDevice;
serialDevice = btDevice.getDeviceForAddress(bluetoothAddy, bluetoothAdapter);
}
device = new SerialPort(serialDevice);
if(!device->open())
{
throw std::runtime_error("unable to open serial device " + serialDevice);
}
GIOChannel *chan = g_io_channel_unix_new(device->fileDescriptor());
g_io_add_watch(chan, G_IO_IN,(GIOFunc)gioPollingFunc, this);
g_io_add_watch(chan,G_IO_HUP,(GIOFunc)gioPollingFunc,this);
g_io_add_watch(chan,G_IO_ERR,(GIOFunc)gioPollingFunc,this);
g_io_channel_unref(chan); //Pass ownership of the GIOChannel to the watch.
}