APA102 cpp implementation (#108)

* Use CPP for APA102

* Add color correction

* Utilise gpiod

* Remove ::

* Fix lights after tests

* Fix default pin number

* Add set brightness service

* Invert LED_SBC_SEL pin

* Change SPI mode

* Change clock polarity

* Reduce number of ROS lgger instances

* Celan up code, update to libgpiod 2.0

* Update panther_lights/include/panther_lights/apa102.hpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/lights_driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/lights_driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/include/panther_lights/apa102.hpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review changes

* Update panther_lights/src/apa102.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review cleanup

* Add build dependencies

* FIx launchfile

* Update panther_lights/CMakeLists.txt

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/CMakeLists.txt

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/apa102.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Update panther_lights/src/driver_node.cpp

Co-authored-by: Dawid Kmak <[email protected]>

* Review changes

* Fix inlude order

* Fix references and APA102 constructor

* Add build dependencies for libgpiod v2

* Bring back libgpiod v1

---------

Co-authored-by: Dawid Kmak <[email protected]>

panther_lights/CMakeLists.txt

@@ -1,15 +1,40 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.16.3)
 project(panther_lights)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 find_package(catkin REQUIRED COMPONENTS
+  image_transport
   panther_msgs
+  roscpp
   rospy
   sensor_msgs
   std_msgs
   std_srvs
 )
 
-catkin_package()
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS panther_msgs roscpp
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(driver_node
+  src/main.cpp
+  src/driver_node.cpp
+  src/apa102.cpp
+)
+
+add_dependencies(driver_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(driver_node
+  gpiodcxx
+  ${catkin_LIBRARIES}
+)
 
 install(DIRECTORY
   config

panther_lights/README.md

@@ -33,7 +33,7 @@ This node is responsible for processing animations and publishing frames to be d
 - `~test` [*bool*, default: **false**]: enables testing mode, enabling extra functionalities.
 - `~user_animations` [*list*, default: **None**]: optional list of animations defined by the user.
 
-### driver_node.py
+### driver_node
 
 This node is responsible for displaying frames on the Husarion Panther robot LED panels.
 

panther_lights/include/panther_lights/apa102.hpp

@@ -0,0 +1,37 @@
+#ifndef PANTHER_LIGHTS_APA102_HPP_
+#define PANTHER_LIGHTS_APA102_HPP_
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace panther_lights
+{
+
+class APA102
+{
+public:
+  APA102(
+    const std::string & device, const std::uint32_t speed = 800000, const bool cs_high = false);
+  ~APA102();
+
+  void set_global_brightness(const std::uint8_t brightness);
+  void set_global_brightness(const double brightness);
+  void set_panel(const std::vector<std::uint8_t> & frame) const;
+
+private:
+  const int fd_;
+  const std::string device_;
+  const std::uint8_t bits_ = 8;
+  const std::uint32_t speed_;
+  std::uint16_t global_brightness_;
+
+  // color correction constants
+  const std::uint16_t corr_red_ = 255;
+  const std::uint16_t corr_green_ = 200;
+  const std::uint16_t corr_blue_ = 62;
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_APA102_HPP_

panther_lights/include/panther_lights/driver_node.hpp

@@ -0,0 +1,56 @@
+#ifndef PANTHER_LIGHTS_DRIVER_NODE_HPP_
+#define PANTHER_LIGHTS_DRIVER_NODE_HPP_
+
+#include <fstream>
+#include <memory>
+#include <vector>
+
+#include <gpiod.hpp>
+#include <ros/ros.h>
+
+#include <image_transport/image_transport.h>
+
+#include <panther_msgs/SetLEDBrightness.h>
+
+#include <panther_lights/apa102.hpp>
+
+namespace panther_lights
+{
+
+class DriverNode
+{
+public:
+  DriverNode(
+    const std::shared_ptr<ros::NodeHandle> & ph, std::shared_ptr<ros::NodeHandle> & nh,
+    const std::shared_ptr<image_transport::ImageTransport> & it);
+  ~DriverNode();
+
+private:
+  int num_led_;
+  double frame_timeout_;
+  bool panels_initialised_ = false;
+  gpiod::line power_pin_;
+  std::string node_name_;
+
+  APA102 front_panel_;
+  APA102 rear_panel_;
+
+  ros::Time front_panel_ts_;
+  ros::Time rear_panel_ts_;
+  std::shared_ptr<ros::NodeHandle> ph_;
+  std::shared_ptr<ros::NodeHandle> nh_;
+  std::shared_ptr<image_transport::ImageTransport> it_;
+  ros::ServiceServer set_brightness_server_;
+  image_transport::Subscriber rear_light_sub_;
+  image_transport::Subscriber front_light_sub_;
+
+  void frame_cb(
+    const sensor_msgs::Image::ConstPtr & msg, const APA102 & panel, const ros::Time & last_time,
+    const std::string & panel_name);
+  bool set_brightness_cb(
+    panther_msgs::SetLEDBrightness::Request & req, panther_msgs::SetLEDBrightness::Response & res);
+};
+
+}  // namespace panther_lights
+
+#endif  // PANTHER_LIGHTS_DRIVER_NODE_HPP_

panther_lights/launch/lights.launch

@@ -3,7 +3,7 @@
   <arg name="test" default="false" />
   <arg name="user_animations_file" default="" />
 
-  <node pkg="panther_lights" type="driver_node.py" name="lights_driver_node"
+  <node pkg="panther_lights" type="driver_node" name="lights_driver_node"
     required="true" output="screen" />
 
   <node pkg="panther_lights" type="controller_node.py" name="lights_controller_node"

panther_lights/package.xml

@@ -16,15 +16,16 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <depend>image_transport</depend>
+  <depend>libgpiod-dev</depend>
   <depend>panther_msgs</depend>
+  <depend>roscpp</depend>
   <depend>rospy</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
 
   <!-- Python dependencies -->
-  <depend>python-rpi.gpio-pip</depend>
-  <depend>python3-apa102-pi-pip</depend>
   <depend>python3-pil</depend>
 
 </package>

panther_lights/src/apa102.cpp

@@ -0,0 +1,99 @@
+#include <panther_lights/apa102.hpp>
+
+#include <fcntl.h>
+#include <linux/spi/spidev.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdint>
+#include <fstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace panther_lights
+{
+APA102::APA102(const std::string & device, const std::uint32_t speed, const bool cs_high)
+: device_(device), speed_(speed), fd_(open(device.c_str(), O_WRONLY))
+{
+  if (fd_ < 0) {
+    throw std::ios_base::failure(std::string("Failed to open ") + device_);
+  }
+
+  static std::uint8_t mode = cs_high ? SPI_MODE_3 : SPI_MODE_3 | SPI_CS_HIGH;
+  if (ioctl(fd_, SPI_IOC_WR_MODE32, &mode) == -1) {
+    close(fd_);
+    throw std::ios_base::failure(std::string("Failed to set mode for ") + device_);
+  }
+
+  if (ioctl(fd_, SPI_IOC_WR_BITS_PER_WORD, &bits_) == -1) {
+    close(fd_);
+    throw std::ios_base::failure(std::string("Can't set bits per word for ") + device_);
+  }
+
+  if (ioctl(fd_, SPI_IOC_WR_MAX_SPEED_HZ, &speed_) == -1) {
+    close(fd_);
+    throw std::ios_base::failure(std::string("Can't set speed for ") + device_);
+  }
+}
+
+APA102::~APA102() { close(fd_); }
+
+void APA102::set_global_brightness(const double brightness)
+{
+  std::uint8_t val = brightness > 0.0f ? ceil(brightness * 31.0f) : 0;
+  set_global_brightness(val);
+}
+
+void APA102::set_global_brightness(const std::uint8_t brightness)
+{
+  // clamp values to be at max 31
+  global_brightness_ = std::uint16_t(brightness) & 0x1F;
+}
+
+void APA102::set_panel(const std::vector<std::uint8_t> & frame) const
+{
+  if (frame.size() % 4 != 0) {
+    throw std::runtime_error("Incorrect number of bytes to transfer to LEDs");
+  }
+  // init buffer with start and end frames
+  std::size_t buffer_size = (4 * sizeof(std::uint8_t)) + frame.size() + (4 * sizeof(std::uint8_t));
+  std::uint8_t * buffer = new std::uint8_t[buffer_size];
+
+  // init start and end frames
+  for (std::size_t i = 0; i < 4; i++) {
+    buffer[i] = 0x00;
+    buffer[buffer_size - i - 1] = 0xFF;
+  }
+
+  // copy frame from vector to sending buffer
+  for (std::size_t i = 0; i < frame.size() / 4; i++) {
+    // padding
+    std::size_t pad = i * 4;
+    // header with brightness
+    std::uint8_t brightness = (std::uint16_t(frame[pad + 3]) * global_brightness_) / 255;
+    buffer[4 + pad] = 0xE0 | brightness;
+    // convert rgb to bgr with collor correction
+    buffer[4 + pad + 1] = std::uint8_t((std::uint16_t(frame[pad + 2]) * corr_blue_) / 255);
+    buffer[4 + pad + 2] = std::uint8_t((std::uint16_t(frame[pad + 1]) * corr_green_) / 255);
+    buffer[4 + pad + 3] = std::uint8_t((std::uint16_t(frame[pad + 0]) * corr_red_) / 255);
+  }
+
+  struct spi_ioc_transfer tr = {
+    .tx_buf = (unsigned long long)buffer,
+    .rx_buf = 0,
+    .len = (unsigned int)buffer_size,
+    .speed_hz = speed_,
+    .delay_usecs = 0,
+    .bits_per_word = 8,
+  };
+
+  int ret = ioctl(fd_, SPI_IOC_MESSAGE(1), &tr);
+  delete[] buffer;
+
+  if (ret < 1) {
+    throw std::ios_base::failure(std::string("Failed to send data over SPI ") + device_);
+  }
+}
+}  // namespace panther_lights