working USB TPMS sensor reading

3 files changed, 262 insertions, 51 deletions

diff --git a/plugins/tpms/CMakeLists.txt b/plugins/tpms/CMakeLists.txt
index f3f876d3..03ab069a 100644
--- a/plugins/tpms/CMakeLists.txt
+++ b/plugins/tpms/CMakeLists.txt
@@ -1,12 +1,14 @@
-include(CheckIncludeFiles)
 include_directories(${CMAKE_SOURCE_DIR}/lib ${include_dirs})
 
-check_include_files(libwebsockets.h HAVE_WEBSOCKETS)
-set(CMAKE_CXX_FLAGS "-g")
+pkg_check_modules(libusb REQUIRED libusb-1.0)
+set(link_libraries ${link_libraries} ${libusb_LIBRARIES})
+include_directories(${libusb_INCLUDE_DIRS})
+#check_include_files(libusb.h HAVE_USB)
+#set(CMAKE_CXX_FLAGS "-g -I/usr/include/libusb-1.0/ -lusb-1.0")
+
 set(tpmsplugin_headers tpmsplugin.h)
 set(tpmsplugin_sources tpmsplugin.cpp)
+
 add_library(tpmsplugin MODULE ${tpmsplugin_sources})
 set_target_properties(tpmsplugin PROPERTIES PREFIX "")
-target_link_libraries(tpmsplugin amb websockets  -L${CMAKE_CURRENT_BINARY_DIR}/lib ${link_libraries})
-
-install(TARGETS tpmsplugin LIBRARY DESTINATION lib/automotive-message-broker)
+target_link_libraries(tpmsplugin amb -L${CMAKE_CURRENT_BINARY_DIR}/lib ${link_libraries})
diff --git a/plugins/tpms/tpmsplugin.cpp b/plugins/tpms/tpmsplugin.cpp
index 7ec78b0f..2e11799f 100644
--- a/plugins/tpms/tpmsplugin.cpp
+++ b/plugins/tpms/tpmsplugin.cpp
@@ -1,5 +1,5 @@
 /*
-Copyright (C) 2012 Tim Trampedach
+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
@@ -21,87 +21,287 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 #include <iostream>
 #include <boost/assert.hpp>
 #include <glib.h>
+#include <libusb.h>
 
 using namespace std;
 
 #include "debugout.h"
 
+#define ENDPOINT_IN             0x81
+#define ENDPOINT_OUT            0x01
+
+#define DEVICE_VID 0x0000
+#define DEVICE_PID 0x0001
+
+#define MAX_SENSORS 4
+#define READ_INTERVAL 10
+//timeout for performing interrupt r/w operations in milliseconds
+#define INTR_TIMEOUT 1000
+
+#define PSI_MULTIPLIER 14.5038
+#define KPA_MULTIPLIER 100
+#define PRESSURE_SCALE 0.025
+ 
+#define FARENHEIT_MULTIPLIER 1.8
+
 
 static gboolean timeoutCallback(gpointer data)
 {
 	TpmsPlugin* src = (TpmsPlugin*)data;
 	
-	return true;
-}
+	int r = src->readValues();
 
-
-string TpmsPlugin::uuid()
-{
-  return "something TPMS";
+	return true;
 }
 
-
-TpmsPlugin::TpmsPlugin(AbstractRoutingEngine* re)
-:AbstractSource(re)
+TpmsPlugin::TpmsPlugin(AbstractRoutingEngine* re, map<string, string> config)
+:AbstractSource(re, config)
 {
 	re->setSupported(supported(), this);
 	debugOut("setting timeout");
 	g_timeout_add(1000, timeoutCallback, this );
-	
-    leftFrontPressure = rightFrontPressure = leftRearPressure = rightRearPressure = 0;
-    leftFrontTemperature = rightFrontTemperature = leftRearTemperature = rightRearTemperature = 0;
+
+    lfPressure = rfPressure = lrPressure = rrPressure = 0;
+    lfTemperature = rfTemperature = lrTemperature = rrTemperature = 0;
+
+    int r = 1;
+    
+    r = libusb_init(NULL);
+    if (r < 0) {
+      DebugOut() << "TPMS: Failed to initialize libusb" << endl;
+    }
+
+    r = findDevice();
+    if (r < 0) {
+      DebugOut() << "TPMS: Could not find/open device - run as root?" << endl;
+    }
+    
+    // need to detach device from kernel driver before claiming the interface
+    r = detachDevice();
+    if (r < 0) {
+      DebugOut() << "TPMS: USB device detach failed with code " << r << endl;
+    }
+
+    r = libusb_claim_interface(mDeviceHandle, 0);
+    if (r < 0) {
+      DebugOut() << "TPMS: usb_claim_interface error " << r << endl;
+    }
+    DebugOut() << "TPMS: USB interface initialized" << endl;	
 }
 
 
 
-extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine)
+extern "C" AbstractSource * create(AbstractRoutingEngine* routingengine, map<string, string> config)
 {
-	return new TpmsPlugin(routingengine);	
+	return new TpmsPlugin(routingengine, config);
+	
 }
 
+string TpmsPlugin::uuid()
+{
+	return "CHANGE THIS 6dd4268a-c605-4a06-9034-59c1e8344c8e";
+}
 
 
 void TpmsPlugin::getPropertyAsync(AsyncPropertyReply *reply)
 {
-	DebugOut()<<"TpmsPlugin: getPropertyAsync called for property: "<<reply->property<<endl;
-    /*	if(reply->property == VehicleProperty::VehicleSpeed)
-	{
-		VehicleProperty::VehicleSpeedType temp(velocity);
-		reply->value = &temp;
-		reply->completed(reply);
-	}
-	else if(reply->property == VehicleProperty::EngineSpeed)
-	{
-		VehicleProperty::EngineSpeedType temp(engineSpeed);
-		reply->value = &temp;
-		reply->completed(reply);
-        }*/
-}
+	DebugOut() << "TPMS: getPropertyAsync called for property: " << reply->property << endl;
+
+    if(reply->property == VehicleProperty::TirePressureLeftFront) {
+      VehicleProperty::TirePressureType temp(lfPressure);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TirePressureRightFront) {
+      VehicleProperty::TirePressureType temp(rfPressure);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TirePressureLeftRear) {
+      VehicleProperty::TirePressureType temp(lrPressure);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TirePressureRightRear) {
+      VehicleProperty::TirePressureType temp(rrPressure);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TireTemperatureLeftFront) {
+      VehicleProperty::EngineSpeedType temp(lfTemperature);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TireTemperatureRightFront) {
+      VehicleProperty::EngineSpeedType temp(rfTemperature);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TireTemperatureLeftRear) {
+      VehicleProperty::EngineSpeedType temp(lrTemperature);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
+    else if(reply->property == VehicleProperty::TireTemperatureRightRear) {
+      VehicleProperty::EngineSpeedType temp(rrTemperature);
+      reply->value = &temp;
+      reply->completed(reply);
+    }
 
+    else {
+      DebugOut() << "TPMS: no such getProperty type: " << reply->property << endl;
+      reply->value = nullptr;
+      reply->completed(reply);
+    }
+}
 
-void TpmsPlugin::setProperty(VehicleProperty::Property, AbstractPropertyType *)
+void TpmsPlugin::setProperty(VehicleProperty::Property , AbstractPropertyType *)
 {
 
 }
 
+void TpmsPlugin::subscribeToPropertyChanges(VehicleProperty::Property property)
+{
+	mRequests.push_back(property);
+}
 
 PropertyList TpmsPlugin::supported()
 {
 	PropertyList props;
-	props.push_back(VehicleProperty::TirePressureLeftFront);
+    props.push_back(VehicleProperty::TirePressureLeftFront);
 	props.push_back(VehicleProperty::TirePressureRightFront);
 	props.push_back(VehicleProperty::TirePressureLeftRear);
 	props.push_back(VehicleProperty::TirePressureRightRear);
+	props.push_back(VehicleProperty::TireTemperatureLeftFront);
+	props.push_back(VehicleProperty::TireTemperatureRightFront);
+	props.push_back(VehicleProperty::TireTemperatureLeftRear);
+	props.push_back(VehicleProperty::TireTemperatureRightRear);
 	
 	return props;
 }
 
-void TpmsPlugin::subscribeToPropertyChanges(VehicleProperty::Property property)
+void TpmsPlugin::unsubscribeToPropertyChanges(VehicleProperty::Property property)
 {
-	mRequests.push_back(property);
+	mRequests.remove(property);
 }
 
-void TpmsPlugin::unsubscribeToPropertyChanges(VehicleProperty::Property property)
+int TpmsPlugin::findDevice(void)
 {
-	mRequests.remove(property);
+  int deviceVid = DEVICE_VID;
+  int devicePid = DEVICE_PID;
+
+  DebugOut() << "TPMS: Trying to open USB device with VID: " << deviceVid << " PID: " << devicePid << endl;
+  mDeviceHandle = libusb_open_device_with_vid_pid(NULL, DEVICE_VID, DEVICE_PID);
+  return mDeviceHandle ? 0 : 1;
+}
+
+
+int TpmsPlugin::detachDevice(void)
+{
+  int r;
+  r = libusb_kernel_driver_active(mDeviceHandle, 0);
+  if (r == 1) {
+    DebugOut() << "TPMS: USB device seems to be kernel driven, trying to detach" << endl;
+    r = libusb_detach_kernel_driver(mDeviceHandle, 0);
+  }
+  return r;
+}
+
+
+int TpmsPlugin::exitClean(int deinit)
+{
+  if (deinit) {
+    libusb_release_interface(mDeviceHandle, 0);
+    libusb_attach_kernel_driver(mDeviceHandle, 0);
+    libusb_close(mDeviceHandle);
+  }
+  libusb_exit(NULL);
+}
+
+
+int TpmsPlugin::readValues()
+{
+  int snum;
+  unsigned char buf[4];
+
+  // Sensor 1 = Left Front
+  // Sensor 2 = Right Front
+  // Sensor 3 = Left Rear
+  // Sensor 4 = Right Rear
+
+  for (snum = 1; snum <= MAX_SENSORS; snum++) {
+    readUsbSensor(snum, buf);
+
+    // only do this if sensor is available
+    if (buf[3] != 0xff) {
+      string mode_string;
+
+      switch (snum) {
+      case 1:
+        lfPressure = (buf[0]-40) * PRESSURE_SCALE * PSI_MULTIPLIER;
+        lfTemperature = buf[1]-40;
+        DebugOut() << "TPMS debug: pressure = " << lfPressure << " temperature = " << lfTemperature << endl;
+        break;
+      case 2:
+        rfPressure = (buf[0]-40) * PRESSURE_SCALE * PSI_MULTIPLIER;
+        rfTemperature = buf[1]-40;
+        break;
+      case 3:
+        lrPressure = (buf[0]-40) * PRESSURE_SCALE * PSI_MULTIPLIER;
+        lrTemperature = buf[1]-40;
+        break;
+      case 4:
+        rrPressure = (buf[0]-40) * PRESSURE_SCALE * PSI_MULTIPLIER;
+        rrTemperature = buf[1]-40;
+        break;
+      }
+
+      // make sensor mode human-readable
+      switch (buf[3]) {
+        case 0x01: mode_string = "normal"; break;
+        case 0x02: mode_string = "pressure_alert"; break;
+        // more to add here...
+        default: mode_string = "unknown"; break;
+      }
+    }
+    else {
+      DebugOut() << "TPMS: Unable to read sensor " << sensorNumberToString(snum) << " (" << snum << ")" << endl;
+    }
+  }
+
+  VehicleProperty::TirePressureType lfPres(lfPressure);
+
+  routingEngine->updateProperty(VehicleProperty::TirePressureLeftFront, &lfPres);
+
+  return 0;
+}
+
+int TpmsPlugin::readUsbSensor(int sid, unsigned char *buf)
+{
+  int r, transferred;
+
+  buf[0] = 0x20 + sid;
+  r = libusb_interrupt_transfer(mDeviceHandle, ENDPOINT_OUT, buf, 1, &transferred, INTR_TIMEOUT);
+  if (r < 0) {
+    DebugOut() << "TPMS: USB write interrupt failed, code " << r << endl;
+  }
+
+  r = libusb_interrupt_transfer(mDeviceHandle, ENDPOINT_IN, buf, 4, &transferred, INTR_TIMEOUT);
+  if (r < 0) {
+    DebugOut() << "TPMS: USB read interrupt failed, code " << r << endl;
+  }
+
+  return r;
+}
+
+
+string TpmsPlugin::sensorNumberToString(int snid)
+{
+  switch (snid) {
+  case 1: return "left front"; break;
+  case 2: return "right front"; break;
+  case 3: return "left rear"; break;
+  case 4: return "right rear"; break;
+  default: return "unknown";
+  }
 }
diff --git a/plugins/tpms/tpmsplugin.h b/plugins/tpms/tpmsplugin.h
index 31080423..85bdfbeb 100644
--- a/plugins/tpms/tpmsplugin.h
+++ b/plugins/tpms/tpmsplugin.h
@@ -28,24 +28,33 @@ class TpmsPlugin: public AbstractSource
 {
 
 public:
-	TpmsPlugin(AbstractRoutingEngine* re);
-
-    string uuid();
-	PropertyList supported();
+	TpmsPlugin(AbstractRoutingEngine* re, map<string, string> config);
+	
+	string uuid();
+	void getPropertyAsync(AsyncPropertyReply *reply);
+	void setProperty(VehicleProperty::Property, AbstractPropertyType*);
 	void subscribeToPropertyChanges(VehicleProperty::Property property);
 	void unsubscribeToPropertyChanges(VehicleProperty::Property property);
+	PropertyList supported();
 	
-	void getPropertyAsync(AsyncPropertyReply *reply);
-    void setProperty(VehicleProperty::Property, AbstractPropertyType*);
-
 	void propertyChanged(VehicleProperty::Property property, AbstractPropertyType* value, string uuid) {}
 	void supportedChanged(PropertyList) {}
-	
+
+    int readValues();
 	
 private:
 	PropertyList mRequests;
-    float leftFrontPressure, rightFrontPressure, leftRearPressure, rightRearPressure;
-    float leftFrontTemperature, rightFrontTemperature, leftRearTemperature, rightRearTemperature;
+    float lfPressure, rfPressure, lrPressure, rrPressure;
+    float lfTemperature, rfTemperature, lrTemperature, rrTemperature;
+    struct libusb_device_handle *mDeviceHandle;
+
+    int findDevice();
+    int detachDevice();
+    int exitClean(int deinit);
+
+    int readUsbSensor(int sid, unsigned char *buf);
+
+    string sensorNumberToString(int snid);
 };
 
 #endif // TPMSPLUGIN_H