initial

CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.5)
+project(usb_bridge)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
+
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+find_package(LibUSB REQUIRED)
+
+
+add_executable(libusb_demo src/libusb_demo.cpp)
+ament_target_dependencies(
+  libusb_demo
+  rclcpp
+  std_msgs
+  example_interfaces
+  LIBUSB
+)
+
+
+install(TARGETS
+  libusb_demo
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

cmake/FindLibUSB.cmake

@@ -0,0 +1,55 @@
+if(NOT LIBUSB_FOUND)
+  pkg_check_modules (LIBUSB_PKG libusb-1.0 libusb)
+  find_path(LIBUSB_INCLUDE_DIR NAMES libusb.h
+    PATHS
+    ${LIBUSB_PKG_INCLUDE_DIRS}
+    /usr/include/libusb-1.0
+    /usr/include
+    /usr/local/include
+  )
+
+#standard library name for libusb-1.0
+set(libusb1_library_names usb-1.0)
+
+#libusb-1.0 compatible library on freebsd
+if((CMAKE_SYSTEM_NAME STREQUAL "FreeBSD") OR (CMAKE_SYSTEM_NAME STREQUAL "kFreeBSD") OR (CMAKE_SYSTEM_NAME STREQUAL "GNU"))
+    list(APPEND libusb1_library_names usb)
+endif()
+
+  find_library(LIBUSB_LIBRARIES
+    NAMES ${libusb1_library_names}
+    PATHS
+    ${LIBUSB_PKG_LIBRARY_DIRS}
+    /usr/lib
+    /usr/local/lib
+  )
+
+include(CheckFunctionExists)
+if(LIBUSB_INCLUDE_DIRS)
+    set(CMAKE_REQUIRED_INCLUDES ${LIBUSB_INCLUDE_DIRS})
+endif()
+if(LIBUSB_LIBRARIES)
+    set(CMAKE_REQUIRED_LIBRARIES ${LIBUSB_LIBRARIES})
+endif()
+
+CHECK_FUNCTION_EXISTS("libusb_handle_events_timeout_completed" HAVE_LIBUSB_HANDLE_EVENTS_TIMEOUT_COMPLETED)
+if(HAVE_LIBUSB_HANDLE_EVENTS_TIMEOUT_COMPLETED)
+    add_definitions(-DHAVE_LIBUSB_HANDLE_EVENTS_TIMEOUT_COMPLETED=1)
+endif(HAVE_LIBUSB_HANDLE_EVENTS_TIMEOUT_COMPLETED)
+
+CHECK_FUNCTION_EXISTS("libusb_error_name" HAVE_LIBUSB_ERROR_NAME)
+if(HAVE_LIBUSB_ERROR_NAME)
+    add_definitions(-DHAVE_LIBUSB_ERROR_NAME=1)
+endif(HAVE_LIBUSB_ERROR_NAME)
+
+if(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES)
+  set(LIBUSB_FOUND TRUE CACHE INTERNAL "libusb-1.0 found")
+  message(STATUS "Found libusb-1.0: ${LIBUSB_INCLUDE_DIR}, ${LIBUSB_LIBRARIES}")
+else(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES)
+  set(LIBUSB_FOUND FALSE CACHE INTERNAL "libusb-1.0 found")
+  message(STATUS "libusb-1.0 not found.")
+endif(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES)
+
+mark_as_advanced(LIBUSB_INCLUDE_DIR LIBUSB_LIBRARIES)
+
+endif(NOT LIBUSB_FOUND)

package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>usb_bridge</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="brettrd@brettrd.com">brettrd</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>libusb-1.0</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/libusb_demo.cpp

@@ -0,0 +1,216 @@
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <libusb-1.0/libusb.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "example_interfaces/msg/u_int8.hpp"
+#include "example_interfaces/msg/u_int32_multi_array.hpp"
+
+
+
+using namespace std::chrono_literals;
+
+
+#define CTRL_IN			(LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_OTHER | LIBUSB_ENDPOINT_IN)
+#define CTRL_OUT		(LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_OTHER | LIBUSB_ENDPOINT_OUT)
+
+
+
+#define CSR_BASE 0x0L
+#define CSR_PPM_INPUT_BASE (CSR_BASE + 0x3000L)
+#define CSR_PPM_INPUT_CHANNELS 2
+#define CSR_PPM_OUTPUT_BASE (CSR_BASE + 0x3800L)
+#define CSR_PPM_OUTPUT_CHANNELS 2
+#define CSR_PWM_INPUT_BASE (CSR_BASE + 0x7800L)
+#define CSR_TRIV_REG_BASE (CSR_BASE + 0x8000L)
+
+
+
+class WishbonePublisher : public rclcpp::Node
+{
+  public:
+    WishbonePublisher()
+    : Node("minimal_publisher"), count_(0)
+    {
+
+      uint16_t idVendor=0x1209;
+      uint16_t idProduct=0x5bf0;
+
+
+
+      triv_reg_pub = this->create_publisher<example_interfaces::msg::UInt8>("triv_reg", 10);
+      ppm_in_pub = this->create_publisher<example_interfaces::msg::UInt32MultiArray>("ppm_in", 10);
+
+
+
+      if (0 <= libusb_init(NULL))
+      {
+        devh = libusb_open_device_with_vid_pid(NULL, idVendor, idProduct);
+        if(devh != NULL)
+        {
+          if(0 <= libusb_claim_interface(devh, 0))
+          {
+            RCLCPP_INFO(this->get_logger(), "claimed interface");
+
+
+            this->triv_reg_timer = this->create_wall_timer(500ms, std::bind(&WishbonePublisher::triv_reg_callback, this));
+            this->ppm_in_timer = this->create_wall_timer(200ms, std::bind(&WishbonePublisher::ppm_in_callback, this));
+
+          }
+          else
+          {
+            RCLCPP_ERROR(this->get_logger(), "could not claim interface");
+            libusb_close(devh);
+            devh = NULL;
+          }
+        }
+        else
+        {
+          RCLCPP_ERROR(this->get_logger(), "No device");
+        }
+      }
+      else
+      {
+        RCLCPP_ERROR(this->get_logger(), "libusb did not init");
+      }
+    }
+
+    ~WishbonePublisher()
+    {
+      if(devh)
+      {
+        libusb_release_interface(devh, 0);
+        libusb_close(devh);
+      }
+    }
+
+
+
+  private:
+
+
+    int wishbone_poke(uint32_t address, uint32_t* data)
+    {
+      int r;
+      uint8_t bRequest = 0x00;
+      uint16_t addr_h, addr_l;
+      unsigned char buf[4];
+      uint16_t len = 4;
+      addr_h = address >> 16;
+      addr_l = address & 0xFFFF;
+
+      for(int i=0; i<4; i++)
+      {
+        buf[i] = (*data >> (8*i)) & 0xFF;
+      }
+
+      r = libusb_control_transfer(devh, CTRL_OUT, bRequest, addr_l, addr_h, buf, len, 0);
+      if (r < 0) {
+        RCLCPP_ERROR(this->get_logger(), "poke error: %s", libusb_error_name(r));
+      }
+      else if ((unsigned int) r < len) {
+        RCLCPP_ERROR(this->get_logger(), "poke error: short write (%d)", r);
+      }
+      return r;
+    }
+
+    int wishbone_peek(uint32_t address, uint32_t* data)
+    {
+      int r;
+      uint8_t bRequest = 0x00;
+      uint16_t addr_h, addr_l;
+      unsigned char buf[4];
+      uint16_t len = 4;
+      addr_h = address >> 16;
+      addr_l = address & 0xFFFF;
+
+      r = libusb_control_transfer(devh, CTRL_IN, bRequest, addr_l, addr_h, buf, len, 0);
+      if (r < 0) {
+        RCLCPP_ERROR(this->get_logger(), "peek error: %s", libusb_error_name(r));
+      }
+      else if ((unsigned int) r < len) {
+        RCLCPP_ERROR(this->get_logger(), "peek error: short read (%d)", r);
+      }
+      else
+      {
+        *data = 0;
+        for(int i=0; i<4; i++)
+        {
+          *data += ((uint32_t)(buf[i])) << (8*i);
+        }
+      }
+
+      return r;
+    }
+
+
+
+
+    void read_ppm(uint32_t* data, int count)
+    {
+      if(count > CSR_PPM_INPUT_CHANNELS) count = CSR_PPM_INPUT_CHANNELS;
+      for(int i=0; i<count; i++)
+      {
+        wishbone_peek(CSR_PPM_INPUT_BASE + (4*i), &(data[i]));
+      }
+    }
+
+    void write_ppm(uint32_t* data, int count)
+    {
+      if(count > CSR_PPM_OUTPUT_CHANNELS) count = CSR_PPM_OUTPUT_CHANNELS;
+      for(int i=0; i<count; i++)
+      {
+        wishbone_poke(CSR_PPM_OUTPUT_BASE + (4*i), &(data[i]));
+      }
+    }
+
+    void read_pwm(uint32_t* data)
+    {
+        wishbone_peek(CSR_PWM_INPUT_BASE, data);
+    }
+
+    void read_triv_reg(uint32_t* data)
+    {
+        wishbone_peek(CSR_TRIV_REG_BASE, data);
+    }
+
+
+    void triv_reg_callback()
+    {
+      uint32_t data = 0;
+      auto message = example_interfaces::msg::UInt8();
+      read_triv_reg(&data);
+      message.data = data & 0xff;
+      triv_reg_pub->publish(message);
+    }
+
+
+    void ppm_in_callback()
+    {
+      auto message = example_interfaces::msg::UInt32MultiArray();
+      message.data.resize(CSR_PPM_INPUT_CHANNELS);
+      read_ppm(message.data.data(), CSR_PPM_INPUT_CHANNELS);
+      ppm_in_pub->publish(message);
+    }
+
+
+
+    struct libusb_device_handle *devh = NULL;
+    rclcpp::TimerBase::SharedPtr triv_reg_timer;
+    rclcpp::TimerBase::SharedPtr ppm_in_timer;
+    rclcpp::Publisher<example_interfaces::msg::UInt8>::SharedPtr triv_reg_pub;
+    rclcpp::Publisher<example_interfaces::msg::UInt32MultiArray>::SharedPtr ppm_in_pub;
+
+    size_t count_;
+  };
+
+  int main(int argc, char * argv[])
+  {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<WishbonePublisher>());
+    rclcpp::shutdown();
+    return 0;
+  }