--- /dev/null
+/*
+ Copyright (C) 2012 Intel Corporation
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+*/
+
+
+#include "obd2source.h"
+#include <iostream>
+#include <boost/assert.hpp>
+#include <boost/lexical_cast.hpp>
+#include <glib.h>
+#include <sstream>
+#include <json-glib/json-glib.h>
+#include <listplusplus.h>
+#include "debugout.h"
+#define __SMALLFILE__ std::string(__FILE__).substr(std::string(__FILE__).rfind("/")+1)
+AbstractRoutingEngine *m_re;
+
+bool sendElmCommand(obdLib *obd,std::string command)
+{
+ std::vector<unsigned char> replyVector;
+ std::string reply;
+ obd->sendObdRequestString(command.append("\r").c_str(),command.length()+1,&replyVector,500,3);
+ for (unsigned int i=0;i<replyVector.size();i++)
+ {
+ reply += replyVector[i];
+ }
+ if (reply.find("OK") == -1)
+ {
+ //No OK reply found
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+
+}
+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);
+ obdLib *obd = new obdLib();
+ obd->openPort("/dev/pts/7",115200);
+ std::vector<unsigned char> replyVector;
+ std::string reply;
+ obd->sendObdRequestString("ATZ\r",4,&replyVector,500,3);
+ for (unsigned int i=0;i<replyVector.size();i++)
+ {
+ reply += replyVector[i];
+ }
+ if (reply.find("ELM") == -1)
+ {
+ //No reply found
+ printf("Error!\n");
+ }
+ else
+ {
+ printf("Reply to reset: %s\n",reply.c_str());
+ }
+ if (!sendElmCommand(obd,"ATE0"))
+ {
+ printf("Error sending echo\n");
+ }
+ if (!sendElmCommand(obd,"ATH0"))
+ {
+ printf("Error sending headers off\n");
+ }
+ if (!sendElmCommand(obd,"ATL0"))
+ {
+ printf("Error turning linefeeds off\n");
+ }
+ std::list<std::string> reqList;
+ std::list<std::string> repeatReqList;
+ while (true)
+ {
+ //gpointer query = g_async_queue_pop(privCommandQueue);
+
+
+ gpointer query = g_async_queue_try_pop(privSingleShotQueue);
+ if (query != nullptr)
+ {
+ printf("Got request!\n");
+ ObdRequest *req = (ObdRequest*)query;
+ reqList.push_back(req->req);
+ delete req;
+ }
+ query = g_async_queue_try_pop(privSubscriptionAddQueue);
+ if (query != nullptr)
+ {
+ printf("Got request!\n");
+ ObdRequest *req = (ObdRequest*)query;
+ repeatReqList.push_back(req->req);
+ delete req;
+ }
+ for (std::list<std::string>::const_iterator i=reqList.cbegin();i!= reqList.cend();i++)
+ {
+ repeatReqList.push_back(*i);
+ }
+ for (std::list<std::string>::const_iterator i=repeatReqList.cbegin();i!= repeatReqList.cend();i++)
+ {
+ if (!obd->sendObdRequest((*i).c_str(),(*i).length(),&replyVector))
+ {
+ printf("Error sending obd2 request\n");
+ continue;
+ }
+ if (replyVector[0] == 0x41)
+ {
+ if (replyVector[1] == 0x0C)
+ {
+ double rpm = ((replyVector[2] << 8) + replyVector[3]) / 4.0;
+ ObdReply *rep = new ObdReply();
+ rep->req = "0C";
+ rep->reply = boost::lexical_cast<string>(rpm);
+ g_async_queue_push(privResponseQueue,rep);
+ //printf("RPM: %f\n",rpm);
+ }
+ else if (replyVector[1] == 0x0D)
+ {
+ int mph = replyVector[2];
+ ObdReply *rep = new ObdReply();
+ rep->req = "0D";
+ rep->reply = boost::lexical_cast<string>(mph);
+ g_async_queue_push(privResponseQueue,rep);
+ }
+ else
+ {
+ printf("Unknown response type: %i\n",replyVector[1]);
+ }
+ }
+
+ //printf("Reply: %i %i\n",replyVector[2],replyVector[3]);
+ }
+ usleep(10000);
+ }
+
+}
+static gboolean updateProperties(gpointer data)
+{
+ OBD2Source* src = (OBD2Source*)data;
+
+ //src->randomizeProperties();
+ gpointer retval = g_async_queue_try_pop(src->responseQueue);
+ if (retval != nullptr)
+ {
+ ObdReply *reply = (ObdReply*)retval;
+ if (reply->req == "0C")
+ {
+ src->engineSpeed(boost::lexical_cast<float>(reply->reply));
+ }
+ else if (reply->req == "0D")
+ {
+ src->vehicleSpeed(boost::lexical_cast<int>(reply->reply));
+ }
+ }
+ return true;
+}
+
+void OBD2Source::engineSpeed(double speed)
+{
+ VehicleProperty::VehicleSpeedType espeed(speed);
+ m_re->updateProperty(VehicleProperty::EngineSpeed,&espeed);
+}
+void OBD2Source::vehicleSpeed(int speed)
+{
+ VehicleProperty::EngineSpeedType vspeed(speed);
+ m_re->updateProperty(VehicleProperty::VehicleSpeed,&vspeed);
+}
+void OBD2Source::setSupported(PropertyList list)
+{
+ m_supportedProperties = list;
+ m_re->updateSupported(list,PropertyList());
+}
+
+OBD2Source::OBD2Source(AbstractRoutingEngine *re) : AbstractSource(re)
+{
+ g_timeout_add(250, updateProperties, this );
+ clientConnected = false;
+ m_re = re;
+
+ //Read JSON that will tell us what to do:
+ /*
+ GError* error = nullptr;
+ JsonParser* parser = json_parser_new();
+ if (!json_parser_load_from_file(parser,"obd2source.conf",&error))
+ {
+ g_error_free(error);
+ error = nullptr;
+ if (!json_parser_load_from_file(parser,"../../plugins/obd2sourceplugin/obd2source.conf",&error))
+ {
+ error = nullptr;
+ g_error_free(error);
+ if (!json_parser_load_from_file(parser,"/etc/ambd/obd2source.conf",&error))
+ {
+ g_error_free(error);
+ error = nullptr;
+ DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error loading JSON";
+ return;
+ }
+ }
+ }
+ JsonNode* node = json_parser_get_root(parser);
+ if(node == nullptr)
+ {
+ DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "Error getting root node of json";
+ //throw std::runtime_error("Unable to get JSON root object");
+ return;
+ }
+ JsonReader* reader = json_reader_new(node);
+ if(reader == nullptr)
+ {
+ DebugOut() << __SMALLFILE__ <<":"<< __LINE__ << "json_reader is null!";
+ //throw std::runtime_error("Unable to create JSON reader");
+ return;
+ }
+
+
+ list<string> data;
+ json_reader_read_member(reader,"sinks");
+ if (json_reader_is_array(reader))
+ {
+ for(int i=0; i < json_reader_count_elements(reader); i++)
+ {
+ json_reader_read_element(reader,i);
+ json_reader_read_member(reader,"ip");
+ string ip = json_reader_get_string_value(reader);
+ json_reader_end_member(reader);
+
+ json_reader_read_member(reader,"port");
+ int port = json_reader_get_int_value(reader);
+ json_reader_end_member(reader);
+ printf("Connecting to %s on port %i\n",ip.c_str(),port);
+
+ json_reader_end_element(reader);
+ }
+ }
+ json_reader_end_member(reader);
+ */
+ m_supportedProperties.push_back(VehicleProperty::EngineSpeed);
+ m_supportedProperties.push_back(VehicleProperty::VehicleSpeed);
+ re->setSupported(supported(), this);
+ /*if (openPort(std::string("/dev/pts/7"),115200))
+ {
+ printf("Error opening OBD2 port\n");
+ }*/
+ commandQueue = g_async_queue_new();
+ subscriptionAddQueue = g_async_queue_new();
+ subscriptionRemoveQueue = g_async_queue_new();
+ responseQueue = g_async_queue_new();
+ singleShotQueue = g_async_queue_new();
+ g_thread_new("mythread",(GThreadFunc)&threadLoop,this);
+ ObdRequest *requ = new ObdRequest();
+ requ->req = "010C\r";
+ g_async_queue_push(subscriptionAddQueue,requ);
+
+ requ = new ObdRequest();
+ requ->req = "010D\r";
+ g_async_queue_push(subscriptionAddQueue,requ);
+}
+
+PropertyList OBD2Source::supported()
+{
+ return m_supportedProperties;
+}
+extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine)
+{
+ return new OBD2Source(routingengine);
+
+}
+string OBD2Source::uuid()
+{
+ return "f77af740-f1f8-11e1-aff1-0800200c9a66";
+}
+void OBD2Source::subscribeToPropertyChanges(VehicleProperty::Property property)
+{
+ //printf("Subscribed to property: %s\n",property.c_str());
+ queuedRequests.push_back(property);
+ if (clientConnected)
+ {
+
+ }
+}
+
+
+void OBD2Source::unsubscribeToPropertyChanges(VehicleProperty::Property property)
+{
+ removeRequests.push_back(property);
+ if (clientConnected)
+ {
+
+ }
+}
+
+
+void OBD2Source::getPropertyAsync(AsyncPropertyReply *reply)
+{
+ /*if(reply->property == VehicleProperty::VehicleSpeed)
+ {
+ reply->value = velocity;
+ reply->completed(reply);
+ }
+ else if(reply->property == VehicleProperty::EngineSpeed)
+ {
+ reply->value = engineSpeed;
+ reply->completed(reply);
+ }
+ else if(reply->property == VehicleProperty::AccelerationX)
+ {
+ reply->value = accelerationX;
+ reply->completed(reply);
+ }
+ else if(reply->property == VehicleProperty::TransmissionShiftPosition)
+ {
+ reply->value = transmissionShiftPostion;
+ reply->completed(reply);
+ }
+ else if(reply->property == VehicleProperty::SteeringWheelAngle)
+ {
+ reply->value = steeringWheelAngle;
+ reply->completed(reply);
+ }*/
+}
+
+void OBD2Source::setProperty(VehicleProperty::Property , AbstractPropertyType * )
+{
+
+}
--- /dev/null
+/**************************************************************************\r
+* Copyright (C) 2010 by Michael Carpenter (malcom2073) *\r
+* mcarpenter@interforcesystems.com *\r
+* *\r
+* This file is a part of libobd *\r
+* *\r
+* libobd is free software: you can redistribute it and/or modify *\r
+* it under the terms of the GNU Lesser General Public License as *\r
+* published by the Free Software Foundation, either version 2 of *\r
+* the License, or (at your option) any later version. *\r
+* *\r
+* libobd is distributed in the hope that it will be useful, *\r
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *\r
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *\r
+* GNU General Public License for more details. *\r
+* *\r
+* You should have received a copy of the GNU General Public License *\r
+* along with libobd. If not, see <http://www.gnu.org/licenses/>. *\r
+***************************************************************************/\r
+\r
+#include "obdlib.h"\r
+#include <time.h>\r
+#include <iostream>\r
+#include <fstream>\r
+#include <string.h>\r
+#include <cstdarg>\r
+void (*debugCallback)(const char *,void*,obdLib::DebugLevel) = NULL;\r
+void (*commsCallback)(const char *,void*) = NULL;\r
+void *debugUserData = NULL;\r
+void *commsUserData = NULL;\r
+obdLib::obdLib()\r
+{\r
+ m_lastError = NONE;\r
+ debugCallback = 0;\r
+ commsCallback = 0;\r
+}\r
+\r
+int obdLib::openPort(const char *portName)\r
+{\r
+ return openPort(portName,-1);\r
+}\r
+void obdLib::setDebugCallback(void (*callbackptr)(const char*,void*,obdLib::DebugLevel),void *usrdata)\r
+{\r
+ //printf("Calling setDebugCallback: %i\n",debugCallback);\r
+ debugCallback = callbackptr;\r
+ debugUserData = usrdata;\r
+ //printf("Calling setDebugCallback: %i\n",debugCallback);\r
+}\r
+void obdLib::setCommsCallback(void (*callbackptr)(const char*,void*),void* usrdata)\r
+{\r
+ commsCallback = callbackptr;\r
+ commsUserData = usrdata;\r
+}\r
+void obdLib::debug(DebugLevel lvl,const char* msg,...)\r
+{\r
+ char fmsg[4096];\r
+ va_list vl;\r
+ va_start(vl,msg);\r
+ vsnprintf(fmsg,sizeof(fmsg),msg,vl);\r
+ if (debugCallback != 0)\r
+ {\r
+ debugCallback(fmsg,debugUserData,lvl);\r
+ }\r
+ va_end(vl);\r
+}\r
+void obdLib::commsDebug(const char *msg)\r
+{\r
+ if (commsCallback != 0)\r
+ {\r
+ commsCallback(msg,commsUserData);\r
+ }\r
+}\r
+\r
+int obdLib::openPort(const char *portName,int baudrate)\r
+{\r
+#ifdef WINVER\r
+ portHandle=CreateFileA(portName, GENERIC_READ|GENERIC_WRITE,0, NULL, OPEN_EXISTING, 0, NULL);\r
+ if (portHandle == INVALID_HANDLE_VALUE)\r
+ {\r
+ return -1;\r
+ }\r
+ COMMCONFIG Win_CommConfig;\r
+ COMMTIMEOUTS Win_CommTimeouts;\r
+ unsigned long confSize = sizeof(COMMCONFIG);\r
+ Win_CommConfig.dwSize = confSize;\r
+ GetCommConfig(portHandle, &Win_CommConfig, &confSize);\r
+ Win_CommConfig.dcb.Parity = 0;\r
+ Win_CommConfig.dcb.fRtsControl = RTS_CONTROL_DISABLE;\r
+ Win_CommConfig.dcb.fOutxCtsFlow = FALSE;\r
+ Win_CommConfig.dcb.fOutxDsrFlow = FALSE;\r
+ Win_CommConfig.dcb.fDtrControl = DTR_CONTROL_DISABLE;\r
+ Win_CommConfig.dcb.fDsrSensitivity = FALSE;\r
+ Win_CommConfig.dcb.fNull=FALSE;\r
+ Win_CommConfig.dcb.fTXContinueOnXoff = FALSE;\r
+ Win_CommConfig.dcb.fInX=FALSE;\r
+ Win_CommConfig.dcb.fOutX=FALSE;\r
+ Win_CommConfig.dcb.fBinary=TRUE;\r
+ Win_CommConfig.dcb.DCBlength = sizeof(DCB);\r
+ if (baudrate != -1)\r
+ {\r
+ Win_CommConfig.dcb.BaudRate = baudrate;\r
+ }\r
+ Win_CommConfig.dcb.ByteSize = 8;\r
+ Win_CommTimeouts.ReadIntervalTimeout = 50;\r
+ Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0;\r
+ Win_CommTimeouts.ReadTotalTimeoutConstant = 110;\r
+ Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0;\r
+ Win_CommTimeouts.WriteTotalTimeoutConstant = 110;\r
+ SetCommConfig(portHandle, &Win_CommConfig, sizeof(COMMCONFIG));\r
+ SetCommTimeouts(portHandle,&Win_CommTimeouts);\r
+ return 0;\r
+#else\r
+ //NEED TO USE BAUD RATE HERE!!!: baudrate\r
+ //printf("Attempting to open COM port\n");\r
+ debug(obdLib::DEBUG_VERBOSE,"Attempting to open com port %s",portName);\r
+ portHandle = open(portName,O_RDWR | O_NOCTTY | O_NDELAY);\r
+ if (portHandle < 0)\r
+ {\r
+ //printf("Error opening Com: %s\n",portName);\r
+ debug(obdLib::DEBUG_ERROR,"Error opening com port %s",portName);\r
+\r
+ return -1;\r
+ }\r
+ //printf("Com Port Opened %i\n",portHandle);\r
+ debug(obdLib::DEBUG_VERBOSE,"Com Port Opened %i",portHandle);\r
+ fcntl(portHandle, F_SETFL, 0); //Set it to blocking. This is required? Wtf?\r
+ //struct termios oldtio;\r
+ struct termios newtio;\r
+ //bzero(&newtio,sizeof(newtio));\r
+ tcgetattr(portHandle,&newtio);\r
+ long BAUD = B9600;\r
+ switch (baudrate)\r
+ {\r
+ case 38400:\r
+ BAUD = B38400;\r
+ break;\r
+ case 115200:\r
+ BAUD = B115200;\r
+ break;\r
+ case 19200:\r
+ BAUD = B19200;\r
+ break;\r
+ case 9600:\r
+ BAUD = B9600;\r
+ break;\r
+ case 4800:\r
+ BAUD = B4800;\r
+ break;\r
+ default:\r
+ BAUD = B38400;\r
+ break;\r
+ } //end of switch baud_rate\r
+ if (strspn("/dev/pts",portName) >= 8)\r
+ {\r
+ debug(obdLib::DEBUG_WARN,"PTS Detected... disabling baud rate selection on: %s",portName);\r
+ //printf("PTS detected... disabling baud rate selection: %s\n",portName);\r
+ baudrate = -1;\r
+ }\r
+ else\r
+ {\r
+ }\r
+\r
+ newtio.c_cflag |= (CLOCAL | CREAD);\r
+ newtio.c_lflag &= !(ICANON | ECHO | ECHOE | ISIG);\r
+ newtio.c_oflag &= !(OPOST);\r
+ newtio.c_cc[VMIN] = 0;\r
+ newtio.c_cc[VTIME] = 100;\r
+/*\r
+ newtio.c_cflag &= ~CSIZE; //Disable byte size\r
+ newtio.c_cflag &= ~PARENB; //Disable parity\r
+ newtio.c_cflag &= ~CSTOPB; //Disable stop bits\r
+ newtio.c_cflag |= (CLOCAL | CREAD | CS8); //Set local mode, reader, and 8N1.\r
+\r
+ newtio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); //Disable CANON, echo, and signals\r
+\r
+ newtio.c_oflag &= ~(OPOST); //Disable post processing\r
+*/\r
+ if (baudrate != -1)\r
+ {\r
+ if(cfsetispeed(&newtio, BAUD))\r
+ {\r
+ perror("cfsetispeed");\r
+ }\r
+\r
+ if(cfsetospeed(&newtio, BAUD))\r
+ {\r
+ perror("cfsetospeed");\r
+ }\r
+ debug(obdLib::DEBUG_VERBOSE,"Setting baud rate to %i on port %s\n",baudrate,portName);\r
+ }\r
+ tcsetattr(portHandle,TCSANOW,&newtio);\r
+ //newtio.c_cc[VMIN] = 0; //Minimum number of bytes to read\r
+ //newtio.c_cc[VTIME] = 100; //Read Timeout (10.0 seconds)\r
+\r
+\r
+ //tcflush(portHandle,TCIFLUSH);\r
+ return 0;\r
+#endif\r
+}\r
+\r
+void obdLib::setPortHandle(HANDLE hdnl)\r
+{\r
+ portHandle = hdnl;\r
+}\r
+\r
+int obdLib::closePort()\r
+{\r
+ #ifdef WINVER\r
+ CloseHandle(portHandle);\r
+ #else\r
+// tcsetattr(portHandle,TCSANOW,&oldtio);\r
+ close(portHandle);\r
+ #endif\r
+ return 0;\r
+}\r
+int obdLib::initPort()\r
+{\r
+ sendObdRequest("atz\r",4);\r
+ sendObdRequest("ati\r",4);\r
+ sendObdRequest("ate0\r",5);\r
+#ifdef WINVER\r
+ Sleep(3000);\r
+#else\r
+ usleep(3000000);\r
+ tcflush(portHandle,TCIFLUSH);\r
+#endif\r
+ sendObdRequest("atl0\r",5);\r
+ sendObdRequest("ath0\r",5);\r
+ sendObdRequest("010C\r",5);\r
+ return 1;\r
+}\r
+byte obdLib::byteArrayToByte(byte b1, byte b2)\r
+{\r
+ byte newB1 = 0;\r
+ byte newB2 = 0;\r
+ if ((b1 >= 48) && (b1 <= 57))\r
+ {\r
+ newB1 = ((b1 - 48) * 16);\r
+ }\r
+ else if ((b1 >=65) && (b1 <= 90))\r
+ {\r
+ newB1 = ((b1 - 55) * 16);\r
+ }\r
+ else\r
+ {\r
+ newB1 = ((b1 - 87) * 16);\r
+ }\r
+ if ((b2 >= 48) && (b2 <= 57))\r
+ {\r
+ newB2 = (b2 - 48);\r
+ }\r
+ else if ((b2 >= 65) && (b2 <= 90))\r
+ {\r
+ newB2 = (b2 - 55);\r
+ }\r
+ else\r
+ {\r
+ newB2 = (b2 - 87);\r
+ }\r
+ byte retVal = (newB1 + newB2);\r
+ return retVal;\r
+}\r
+\r
+bool obdLib::sendObdRequest(const char *req,int len)\r
+{\r
+ //Blind request\r
+ std::vector<byte> reply;\r
+ return sendObdRequestString(req,len,&reply,-1,-1);\r
+}\r
+bool obdLib::sendObdRequest(const char *req,int len,int sleeptime)\r
+{\r
+ std::vector<byte> reply;\r
+ return sendObdRequestString(req,len,&reply,sleeptime,-1);\r
+}\r
+\r
+bool obdLib::sendObdRequestString(const char *req,int length,std::vector<byte> *reply)\r
+{\r
+ return sendObdRequestString(req,length,reply,20,3);\r
+}\r
+bool obdLib::sendObdRequestString(const char *req,int length,std::vector<byte> *reply,int sleeptime)\r
+{\r
+ return sendObdRequestString(req,length,reply,sleeptime,3);\r
+}\r
+\r
+void obdLib::flush()\r
+{\r
+#ifdef WINVER\r
+\r
+#else\r
+ tcflush(portHandle,TCIFLUSH);\r
+#endif\r
+}\r
+std::string obdLib::monitorModeReadLine()\r
+{\r
+ std::string retval;\r
+ int len=0;\r
+ char *tmp = new char[1024];\r
+ bool breakit = false;\r
+ while (!breakit)\r
+ {\r
+#ifndef WINHACK\r
+ len = read(portHandle,tmp,1024);\r
+#else\r
+ if (!ReadFile(portHandle,(LPVOID)tmp,1024,(LPDWORD)&len,NULL))\r
+ {\r
+ delete[] tmp;\r
+ m_lastError = SERIALREADERROR;\r
+ debug(obdLib::DEBUG_ERROR,"Serial read error");\r
+ return "";\r
+ }\r
+\r
+#endif\r
+\r
+ if (len < 0)\r
+ {\r
+ printf("No Read\n");\r
+ perror("Error");\r
+ delete[] tmp;\r
+ m_lastError = SERIALREADERROR;\r
+ debug(obdLib::DEBUG_ERROR,"Serial read error");\r
+ return std::string();\r
+ }\r
+ else if (len == 0)\r
+ {\r
+ #ifdef WINVER\r
+ Sleep(10);\r
+ #else\r
+ usleep(10000);\r
+ #endif\r
+ }\r
+ else\r
+ {\r
+ for (int i=0;i<len;i++)\r
+ {\r
+ if (tmp[i] == '\n')\r
+ {\r
+ breakit = true;\r
+ }\r
+ else\r
+ {\r
+ retval.append(1,tmp[i]);\r
+ }\r
+ }\r
+ }\r
+ }\r
+ return retval;\r
+\r
+}\r
+bool obdLib::sendObdRequestString(const char *req,int length,std::vector<byte> *reply,int sleeptime, int timeout)\r
+{\r
+ reply->clear();\r
+ //std::vector<byte> tmpReply;\r
+ char *tmp = new char[1024];\r
+ char *totalReply = new char[1024];\r
+ int loc = 0;\r
+ int len = 0;\r
+ time_t seconds = time(NULL);\r
+ commsDebug(req);\r
+#ifdef WINVER\r
+ if (!::WriteFile(portHandle, (void*)req, (DWORD)length, (LPDWORD)&len, NULL)) {\r
+ //DWORD error = GetLastError();\r
+ //int i = 2;\r
+ //An error happened, I should probably handle this sometime.\r
+ delete[] totalReply;\r
+ delete[] tmp;\r
+ m_lastError = SERIALWRITEERROR;\r
+ debug(obdLib::DEBUG_ERROR,"Serial write error");\r
+ return false;\r
+ }\r
+#else\r
+ len = write(portHandle,req,length);\r
+#endif\r
+ if (len < 0)\r
+ {\r
+ printf("No Write\n");\r
+ //delete tmp;\r
+ //delete totalReply;\r
+ //m_lastError = SERIALWRITEERROR;\r
+ //return false;\r
+ }\r
+ if (sleeptime == -1)\r
+ {\r
+ //Not expecting a reply.\r
+ delete[] totalReply;\r
+ delete[] tmp;\r
+ return true;\r
+\r
+ }\r
+#ifdef WINVER\r
+ Sleep(sleeptime);\r
+#else\r
+ usleep(sleeptime * 1000);\r
+#endif\r
+ bool continueLoop = true;\r
+ while (continueLoop)\r
+ {\r
+#ifdef WINVER\r
+ if (!ReadFile(portHandle,(LPVOID)tmp,1024,(LPDWORD)&len,NULL))\r
+ {\r
+ delete[] tmp;\r
+ delete[] totalReply;\r
+ m_lastError = SERIALREADERROR;\r
+ debug(obdLib::DEBUG_ERROR,"Serial read error");\r
+ return false;\r
+ }\r
+#else\r
+ len = read(portHandle,tmp,1024);\r
+#endif\r
+\r
+ if (len < 0)\r
+ {\r
+ printf("No Read\n");\r
+ perror("Error");\r
+ delete[] tmp;\r
+ delete[] totalReply;\r
+ m_lastError = SERIALREADERROR;\r
+ debug(obdLib::DEBUG_ERROR,"Serial read error");\r
+ return false;\r
+ }\r
+ else if (len == 0)\r
+ {\r
+ #ifdef WINVER\r
+ Sleep(10);\r
+ #else\r
+ usleep(10000);\r
+ #endif\r
+ }\r
+ else\r
+ {\r
+ for (int i=0;i<len;i++)\r
+ {\r
+ if (/*(tmp[i] != 0x20) && (tmp[i] != '\r') && (tmp[i] != '\n') && */(tmp[i] != '>'))\r
+ {\r
+ //printf("Byte: %c %i ",tmp[i],tmp[i]);\r
+ totalReply[loc++] = tmp[i];\r
+ }\r
+ if (tmp[i] == '>')\r
+ {\r
+ /*printf("\n> returned. Current: %i, Len: %i\n",i,len);\r
+ printf("\n\nResponse: ");\r
+ for (int j=i;j<len;j++)\r
+ {\r
+ printf("%c",tmp[j]);\r
+ }\r
+ printf(" :End\n\n");\r
+ */\r
+ continueLoop = false;\r
+ }\r
+ }\r
+ }\r
+ if (timeout > 0)\r
+ {\r
+ if ((time(NULL) - seconds)-(sleeptime / 1000.0) > timeout)\r
+ {\r
+ //printf("Time:%i:%i:%i\n",time(NULL) - seconds,(time(NULL) - seconds) - (sleeptime/1000.0),time(NULL));\r
+ continueLoop = false;\r
+\r
+ m_lastError = TIMEOUT;\r
+ printf("Timeout, current reply state:");\r
+ for (int i=0;i<loc;i++)\r
+ {\r
+ printf("%c",totalReply[i]);\r
+ }\r
+\r
+ printf(":\n");\r
+ printf("Current reply length: %i\n",loc);\r
+ delete[] tmp;\r
+ delete[] totalReply;\r
+ debug(obdLib::DEBUG_ERROR,"Timeout");\r
+ return false;\r
+ }\r
+ }\r
+ if (timeout == -1)\r
+ {\r
+ //Not waiting for a reply.\r
+ delete[] tmp;\r
+ delete[] totalReply;\r
+ debug(obdLib::DEBUG_ERROR,"Timeout, not waiting for a reply");\r
+ return true;\r
+ }\r
+ }\r
+\r
+ //Perform checking on totalReply\r
+ for (int i=0;i<loc;i++)\r
+ {\r
+ reply->push_back(totalReply[i]);\r
+ }\r
+ //printf("Total Reply Size: %i\n",loc);\r
+ totalReply[loc] = '\0';\r
+ commsDebug(totalReply);\r
+ int errorlen = strspn("NODATA",totalReply);\r
+ if (errorlen == 6)\r
+ {\r
+ //Error\r
+ //printf("Error\n");\r
+ //Nodata here\r
+ m_lastError = NODATA;\r
+ delete[] tmp;\r
+ delete[] totalReply;\r
+ debug(obdLib::DEBUG_ERROR,"nodata");\r
+ return false;\r
+ }\r
+ delete[] totalReply;\r
+ delete[] tmp;\r
+ //tcflush(portHandle,TCIFLUSH);\r
+ //debug(obdLib::DEBUG_ERROR,"Hunkydory");\r
+ return true;\r
+}\r
+obdLib::ObdError obdLib::lastError()\r
+{\r
+ return m_lastError;\r
+}\r
+bool obdLib::sendObdRequest(const char *req,int length,std::vector<byte> *reply)\r
+{\r
+ return sendObdRequest(req,length,reply,100,5);\r
+}\r
+\r
+\r
+bool obdLib::sendObdRequest(const char *req,int length,std::vector<byte> *reply,int sleep, int timeout)\r
+{\r
+ reply->clear();\r
+ std::vector<byte> tmpReply;\r
+ if (!sendObdRequestString(req,length,&tmpReply,sleep,timeout))\r
+ {\r
+ debug(obdLib::DEBUG_ERROR,"sendObdRequestString returned false!!");\r
+ return false;\r
+ }\r
+ if (tmpReply.size() == 0)\r
+ {\r
+ debug(obdLib::DEBUG_ERROR,"sendObdRequestString returned true with a tmpReply size of 0!");\r
+ return false;\r
+ }\r
+ for (unsigned int i=0;i<tmpReply.size()-1;i++)\r
+ {\r
+ if ((tmpReply[i] != 0x20) && (tmpReply[i] != '\r') && (tmpReply[i] != '\n'))\r
+ {\r
+ reply->push_back(byteArrayToByte(tmpReply[i],tmpReply[i+1]));\r
+ i++;\r
+ }\r
+ }\r
+ return true;\r
+}\r
+\r
+\r
+extern "C" {\r
+ STDCALL void *obdLibNew()\r
+ {\r
+ return new obdLib();\r
+ }\r
+ STDCALL int obdLibOpenPort(void *ptr,const char *portname,int baudrate)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ return lib->openPort(portname,baudrate);\r
+ }\r
+ STDCALL int obdLibClosePort(void *ptr)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ return lib->closePort();\r
+ }\r
+ STDCALL int obdLibInitPort(void *ptr)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ return lib->initPort();\r
+ }\r
+ STDCALL void obdLibDelete(void *ptr)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ delete lib;\r
+ }\r
+\r
+ //bool sendObdRequestString(const char *req,int length,std::vector<byte> *reply,int sleeptime, int timeout);\r
+ //bool sendObdRequest(const char *req,int length,std::vector<byte> *reply,int sleep, int timeout);\r
+ //bool sendObdRequest(const char *req,int length,std::vector<byte> *reply,int sleep, int timeout);\r
+ STDCALL bool obdLibSendObdRequest(void *ptr,const char *req,int length, char *reply,unsigned int replylength,int sleeptime, int timeout)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ std::vector<byte> replyvect;\r
+ if (!lib->sendObdRequest(req,length,&replyvect,sleeptime,timeout))\r
+ {\r
+ return false;\r
+ }\r
+ if (replyvect.size() > replylength)\r
+ {\r
+ //Not enough room in the buffer\r
+ return false;\r
+ }\r
+ for (unsigned int i=0;i<replyvect.size();i++)\r
+ {\r
+ reply[i] = replyvect.at(i);\r
+ }\r
+ reply[replyvect.size()] = '.';\r
+ return true;\r
+ }\r
+ STDCALL void setEcho(void *ptr,bool on)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ if (on)\r
+ {\r
+ lib->sendObdRequest("ate1\r",5,250);\r
+ }\r
+ else\r
+ {\r
+ lib->sendObdRequest("ate0\r",5,250);\r
+ }\r
+\r
+ }\r
+ STDCALL bool obdLibSendObdRequestString(void *ptr,const char *req,int length, char *reply,unsigned int replylength,int *replylengthptr,int sleeptime, int timeout)\r
+ {\r
+ obdLib *lib = ((obdLib*)ptr);\r
+ std::vector<byte> replyvect;\r
+ if (!lib->sendObdRequestString(req,length,&replyvect,sleeptime,timeout))\r
+ {\r
+ reply[0] = -1;\r
+ reply[1] = 1;\r
+ reply[2] = lib->lastError();\r
+ return false;\r
+ }\r
+ //char *tmp = new char(1024);\r
+ //sprintf(tmp,"ObdRequest returned! Size: %i",replyvect.size());\r
+ //OutputDebugString(tmp);\r
+ //fprintf(stderr,"ObdRequest returned! Size %i\n",replyvect.size());\r
+ if (replyvect.size() > replylength)\r
+ {\r
+ //Not enough room in the buffer\r
+ reply[0] = -1;\r
+ reply[1] = 2;\r
+ return false;\r
+ }\r
+ for (unsigned int i=0;i<replyvect.size();i++)\r
+ {\r
+ reply[i] = replyvect.at(i);\r
+ }\r
+ //reply[replyvect.size()]='.';\r
+ *replylengthptr = replyvect.size();\r
+ if (replyvect.size() == 0)\r
+ {\r
+ reply[0] = lib->lastError();\r
+ }\r
+ else\r
+ {\r
+ //reply[0] = replyvect.size();\r
+ }\r
+ return true;\r
+ }\r
+}\r
projects / profile / ivi / automotive-message-broker.git / commitdiff