Use C++ progams to control Fusion-HAT+ for ROS2 Control

General
1

Hi, I need some clarification of how I can control motor channels on the Fusion-HAT+ connected to a raspberry pi 5 running Ubuntu 24.04 LTS.

The fusion hat driver installed properly as did the fusion_hat python library and I can easily control the HAT using python. I can also directly interact with the PWM channels via syfs at DEVICE_PATH = “/sys/class/fusion_hat/fusion_hat/”

I was trying to write a C++ program to set PWM timer, period and duty cycle for the motors. The rationale being that ROS2 Control requires C++ programs to interact with hardware. I had no difficulty in writing a C++ program to read the digital IO pins on the HAT (pi 5 GPIO pins 22, 27) for Sparkfun DC motor with quadrature encoders.

When I tried using C++ to control motor 2 and 3 via PWM directly on the fusion-HAT+ i2c registers it fails to acquire the MCU at register 0x17…

Then I realised… that of course the fusion_hat.ko driver has already claimed the hardware! This is shown by UU at address 0x17 using i2cdetect -y 1 . Am I correct in saying that?

I can see that the python fusion_hat library controls PWM settings through the fusion_hat class in the syfs. I don’t want to uninstall the fusion_hat driver or python library just to write a C++ motor driver. It a great driver and python library and very easy to use. I don’t want to reinvent the wheel and I am sure someone can give me advice on how best to use the fusion_hat driver with C++.

One C++ option I can see is to open the "sys/class/fusion_hat” as a file and read/write values to it like the python library does. I am guessing this is not as fast as low level control of i2c reigsters directly in C++? Would it be fast enough though?

ROS2 Control aims to run hardware at 100Hz and above. I am not that experienced with C++ compared to python and a novice using ROS2 Control. I was hoping to build a robot using ROS2 and learn ROS2 Control using the Fuion-HAT+ as the main hardware controller on the Pi 5. It won’t be a serious robot to slower ROS2 Control speeds shouldn’t really matter but curious what update speeds I can expect using sysfs vs low-level control. Does anyone know?

Is there any another way to control / interact with the fusion_hat driver other than via fusion_hat class in sysfs?

I cannot see why the Fusion-HAT+ couldn’t be a great platform to use ROS2 Control!

Any and all suggestions are welcomed!

2

Please dont treat following as definitive. .. Just my own thoughts

Yes — the UU shown by i2cdetect means that the kernel driver has already claimed that I2C address, so direct access to the device registers from userspace will fail. I fell into a similar trap.

Using the sysfs interface from C++ is therefore the correct approach in my opinion, as the fusion_hat driver already exposes the PWM controls in that way. In general it’s best to interact with the hardware through the kernel driver rather than bypassing it.

I experimented with something similar some time ago, and the sysfs overhead is typically very small compared to the I2c transaction time. I would expect update rates in the few-hundred Hz range (circa 500 Hz ~ish) which should be more than enough for a typical ROS2 control loop of typically around 100 Hz.

The main difference i can see is that I used ROS2 under raspbian and you’re using it under ubuntu, but I don’t think that makes any difference here.

It’s a very interesting sounding project.

Hopefully someone else can verify or disagree with my thoughts above!

Good luck!

3

That’s great insight and advice! Thanks so much. Now I know where to focus my efforts. Will see how it goes. Thanks again!

4

So to progress I need some information. There are different values for clock speed, periods ((ARR) and for prescaler (PSC) used between example in the fusion hat documentation, in the Python library and in the C++ driver code.

I’m guessing some are examples for the theory and some are what is actually used by the fusion_hat class.

I need to know what the clock speed is for the MCU (as I presume that is used for the PWM timers etc) and what the defaults are for the fusion_hat class is in the sysfs. If anyone can give me the definite values for that I would appreciate it!

5

I’m not sure which examples you are referring to but I think this is what youre looking for?

If you check out the source code

Then it appears that the mcu clock is 72MHz

CLOCK = 72000000.0  # Clock frequency, 72MHz

And then in code comments default pwm is 50hz

freq (int, optional): PWM frequency, default is 50Hz

And code

def __init__(self, channels:list, *args, freq: int=50, addr: int=I2C_ADDRESS, auto_write: bool=False, **kwargs) -> None:   super().__init__(*args, **kwargs)

Sunfounder will know better than I though

6

This from the fusion_hat driver code:

 // Calculate prescaler from period
  uint32_t frequency = 1000000 / period;
  uint32_t prescaler =
      PWM_CORE_FREQUENCY / frequency / (PWM_PERIOD_VALUE + 1) - 1;
  if (prescaler == 0)
    prescaler = 1;
  if (prescaler > 65535)
    prescaler = 65535;
7

That’s looks to be the firmware, which matches to the python library but is a bit more constrained.

Looking at this firmware, it appears the PWM frequency is derived from a period value expressed in microseconds:

frequency = 1000000 / period

The prescaler is then calculated using the standard timer equation…..

PSC = PWM_CORE_FREQUENCY / (frequency * (PWM_PERIOD_VALUE + 1)) - 1

So the MCU firmware seems to keep the timer period (ARR) fixed (“PWM_PERIOD_VALUE”) and adjusts the prescaler to achieve the requested frequency.

The prescaler is clamped to the 16-bit range (1–65535), which also suggests the underlying timer hardware is using a 16-bit PSC register? I cant think of any other reason to clamp this.

Combined with the 72 MHz timer clock visible in the Python library, this matches the usual MCU PWM formula:

f_pwm = f_clock / ((PSC+1)(ARR+1))

with ARR fixed and PSC computed dynamically.

But! Its been almost 2 years since I played with ROS etc on the sunfounder platforms, so definitely rusty and out of date. (Im approaching my robotics quite differently now.)

Sunfounder will be back in the office after the weekend, so probably better to await their definitive answer, not my dodgy reverse engineering!

8

Thanks @Spf650 ! I’ll have a go at trying a C++ driver with all that information.

9

Thanks @Spf650 for your help. I have a prototype cpp file to control a motor using the fusion_hat PWM class in sysfs and a prototype cpp encoder app using wiringPi GPIO library to read quadrature encoder at x2 resolution both working.

1 Like
10

I can also control the servos using sysfs in the same way using c++.

Now I just have to re-write everything as c++ classes, parse the ros2 command velocity (cmd_vel) into pwm duty cycles, use the encoders for PID control of the pwm signal and also publish the encoder data for odometry…

It’s getting exciting now. I hope the fusion-hat will work well with Ros2… so far it can do everything I’ve needed.

The biggest issue has been trying to find DC motors with right power and gearing for a simple differential drive robot… one DC motor of one brand unfortunately had sparking at the brushes and spiked the current higher than my HAT could tolerate and burnt out one motor controller. Thankfully the other motor channels and rest of the HAT still works. This time I have tested replacement motors prior to attaching to the HAT. A hard lesson learnt!

11

I found the tutorials on the automatic addisson very informative

Not sure if its of use to you?

1 Like
12

That is an amazing site and will definitely be helpful. I had thought about building a mecanum drive robot but will try a simple 2-wheel differential drive bot first. A mecanum bot could only support x1 resolution encoders given there are only 4 GPIO pins free (x2 res uses all 4) but that should be fine as I can get motor direction from the code. I would also have to buy another fusion-hat board given I blew up one motor channel so far…