C++ motor driver for ROS2 control for Robot-HAT v4 and Pi5

Hi, I am trying to write a C++ motor driver to control motor 1 and motor 2 on v4 robot host. I can successfully write and then read the battery voltage at A4 at register 0X130000 (ADC assigned to the battery level), and having some success to control one motor. Has anyone tried something similar or would want to collaborate? I am a cpp amateur.

Rationale

I am hoping to write C++ motor driver for Robot-HAT v4 attached to Raspberry pi 5 running Ubuntu 24.04 and ROS2 natively. Most things in ROS2 can be done with python, however ROS2 control requires C++ drivers etc hence my aim of replicating motor control in C++

Project aims

  1. GPIO control via libgpiod which is the recommended C/C++ library for GPIO control in Linux and raspberry pi 5 going forwards. GPIO pins required for control of motor direction

  2. PMW motor speed control via Linux kernel i2c device driver.

Issues so far:

I partly seem to have GPIO control using libgpiod library but having issues. Some people have reported issues with raspberry 5 and older versions of this library. The apt repositories installs version 1.6.x on Ubuntu but the library is up to version 2.x and apparently this works with the hardware changes between pi4 and pi5. I will install and build the v2 library and post back if I have success with this

As for PMW control, again I have this partially working and will post some code I’m using to test i2c control using C++ on my pi5

Some background information on GPIO control on pi 5:

https://pip.raspberrypi.com/categories/685-whitepapers-app-notes/documents/RP-006553-WP/A-history-of-GPIO-usage-on-Raspberry-Pi-devices-and-current-best-practices.pdf