I saw a recent Skyentific video and decided to have a try at it myself, check out the result:
Last time I covered the new software library that I wrote to help use all the features of the pi3hat, in an efficient manner. This time, I’ll cover how I measured the performance of the result, and talk about how it can be integrated into a robotic control system.
To check out the timing, I wired up a pi3hat into the quad A1 and used the oscilloscope to probe one of the SPI clocks and CAN bus 1 and 3.
Then, I could use
pi3hat_tool incantations to experiment with different bus utilization strategies and get to one with the best performance. The sequence that I settled on was:
- Write all outgoing CAN messages, using a round-robin strategy between CAN buses. The SPI bus rate of 10Mhz is faster than the 5Mbps maximum CAN-FD rate, so this gets each bus transmitting its first packet as soon as possible, then queues up the remainder.
- Read the IMU. During this phase, any replies over CAN are being enqueued on the individual STM32 processors.
- Optionally read CAN replies. If any outgoing packets were marked as expecting a reply, that bus is expected to receive the appropriate number of responses. Additionally, a bus can be requested to “get anything in the queue”.
With this approach, a full command and query of the comprehensive state of 12 qdd100 servos, and reading the IMU takes around 740us. If you perform that on one thread while performing robot control on others, it allows you to achieve a 1kHz update rate.
These results were with the Raspberry Pi 3b+. On a Raspberry Pi 4, they seem to be about 5% better, mostly because the Pi 4’s faster CPU is able to execute the register twiddling a little faster, which reduces dead time on the SPI bus.
The pi3hat r4.2, now in the mjbots store, has only minor hardware changes from the r4 and r4.1 versions. What has changed in a bigger way is the firmware, and the software that is available to interface with it. The interface software for the previous versions was tightly coupled to the quad A1s overall codebase, that made it basically impossible to use with without significant rework. So, that rework is what I’ve done with the new libpi3hat library:
It consists of a single C++11 header and source file with no dependencies aside from the standard C++ library and
bcm_host.h from the Raspberry Pi firmware. You can build it using the bazel build files, or just copy the source file into your own project and build with whatever system you are using.
Using all of the pi3hat’s features in a runtime performant way can be challenging, but libpi3hat makes it not so bad by providing an omnibus call which sequences accesses to all the CAN buses and peripherals in a way that maximizes pipelining and overlap between the different operations, while simultaneously maximizing the usage of the SPI bus. The downside is that it does not use the linux kernel drivers for SPI and thus requires root access to run. For most robotic applications, that isn’t a problem, as the controlling computer is doing nothing but control anyways.
This design makes it feasible to operate at least 12 servos and read the IMU at rates over 1kHz on a Raspberry Pi.
There is a command line tool,
pi3hat_tool which provides a demonstration of how to use all the features of the library, as well as being a useful diagnostic tool on its own. For instance, it can be used to read the IMU state:
# ./pi3hat_tool --read-att ATT w=0.999 x=0.013 y=-0.006 z=-0.029 dps=( 0.1, -0.1, -0.1) a=( 0.0, 0.0, 0.0)
And it can be used to write and read from the various CAN buses.
# ./pi3hat_tool --write-can 1,8001,1300,r \ --write-can 2,8004,1300,r \ --write-can 3,8007,1300,r CAN 1,100,2300000400 CAN 2,400,2300000400 CAN 3,700,230000fc00
You can also do those at the same time in a single bus cycle:
# ./pi3hat_tool --read-att --write-can 1,8001,1300,r CAN 1,100,2300000400 ATT w=0.183 x=0.692 y=0.181 z=-0.674 dps=( 0.1, -0.0, 0.1) a=(-0.0, 0.0,-0.0)
Next up I’ll demonstrate my performance testing setup, and what kind of performance you can expect in a typical system.
I’ve now got the last custom board from the quad A1 up in the mjbots store for sale, the mjbots pi3 hat for $129.
This board breaks out 4x 5Mbps CAN-FD ports, 1 low speed CAN port, a 1kHz IMU and a port for a nrf24l01. Despite its name, it works just fine with the Rasbperry Pi 4 in addition to the 3b+ I have tested with mostly to date. I also have a new user-space library for interfacing with it that I will document in some upcoming posts. That library makes it pretty easy to use in a variety of applications.
Finally, as is customary with these boards, I made a video “getting started” guide:
While I was able to make the r2 power distribution board work, it did require quite a bit more than my usual number of blue wires and careful trace cutting.
Thus I spun a new revision r3, basically just to fix all the blue wires so that I could have some spares without having to worry about the robustness of my hot glue. While I was at it, I updated the logo:
As seems to be the way of things, a few days after I sent this board off to be manufactured, I realized that the CAN port needed to actually be isolated, since when the switches are off, the ground is disconnected from the rest of the system. Sigh. Guess that will wait for r4.
Here is r3 all wired up into the chassis:
After getting the power to work, the next step in bringing up the new quad’s raspberry pi interface board is getting the FDCAN ports to work. As described in my last roadmap, this board has multiple independent FDCAN buses. There are 2 STM32G4’s each with 2 FDCAN buses so that every leg gets a separate bus. There is a 5th auxiliary bus for any other peripherals driven from a third STM32G4. All 3 of the STM32G4’s communicate with the raspberry pi as SPI slaves.
Making this work was straightforward, if tedious. I designed a simple SPI based protocol that would allow transmission and receipt of FD-CAN frames at a high rate in a relatively efficient manner, then implemented that on the STM32s. On the raspberry pi side I initially used the linux kernel driver, but found that it didn’t give sufficient control over hold times during the transmission. Since the SPI slave is implemented in software, I needed to leave sufficient time after asserting the chip select and after transmitting the address bytes. The kernel driver gives no control over this at all, so I resorted to directly manipulating the BCM2837s peripheral registers and busy loop waiting in a real time thread.
After a decent supply of bugs were squashed, I got to a point where the host could send off 12 queries to all the servos with the four buses all being used simultaneously, then collating the responses back. I haven’t spent much time optimizing the cycle time, but the initial go around is at around 1.0ms for a query of all 12 devices which is about 1/3 of the 3.5ms I had in the previous single-bus RS485 version.
Here’s a scope trace of a full query cycle with 2 of the 4 CAN buses on the top, and the two chip selects on the bottom. Woohoo!
With the new FD-CAN based moteus controllers I need a way for the raspberry pi to communicate with them. Thus I’ve got a new adapter board in house that I’m bringing up:
This one has 5 independent FD-CAN channels, an IMU, a port for an nrf2401l RF transceiver as well as a buck converter to power the computer from the main battery bus.
The prototypes were largely constructed by MacroFab, although I did the Amass connectors and the STM32s because supply chain issues prevented me from getting those parts to MacroFab in time.
Next I’ll start bringing up the various pieces!
I’ve received my first production run of the fdcanusb CAN-FD USB adapters and they are up for sale at mjbots.com!
While this is necessary for interacting with the moteus controller, it is also a fine general purpose CAN-FD adapter. At the moment, the USB interface is a platform independent line based serial one (Windows, Linux, MacOS). It doesn’t yet interoperate with SocketCAN on linux, but hopefully that will be resolved in the not too distant future.
One final piece of porting that needed to happen for the moteus controller r4.x series was the bootloader. The r3.x series has a bootloader, which allowed re-flashing the device over the normal data link, but that was largely specific to the RS485 and mjlib/multiplex framing format. Thus, while not particularly challenging, I needed to update it for the FD-CAN interface used on the r4.x board.
The update itself was straightforward: https://github.com/mjbots/moteus/compare/406f01…1123a9
For now, on the assumption I will in the not too distant future deprecate the r3.x series, just duplicated the entire bootloader, replacing all the communication bits with FDCAN and stm32g4 appropriate pieces. As before, this bootloader is designed to only operate after the normal firmware has initialized the device, and also is required to be completely standalone. To make code size easier to manage, it makes no calls to any ST HAL library and manipulates everything it needs purely through the register definitions.
Thankfully, the ST HAL sources are BSD licensed, otherwise I’m not sure I could have gotten the FD-CAN and flash peripherals to work just given the reference manual. With it, copying out the necessary constants made for an easy solution.
I introduced the fdcanusb previously, now I’ll describe some of the process and challenges in getting it to work.
My initial challenges were around the PCB design and manufacturing. To begin with, my very first revision was sent out for manufacturing with the same incorrect pinout as the moteus controller r4.0 and thus was only really usable as a paperweight. Second, the supply of STM32G474 chips seems to be spotty now, so for r2 I had to scavenge chips from the boards that had broken pinouts.
The r2 boards did have a DB9 pinout that was not industry standard, this time due to Eagle’s DB9 footprint being broken, but that will be easy enough to fix in r2.1.
The software had three things that needed to work, FDCAN, USB, and the finally all the glue logic. Getting FDCAN to work was remarkably easy: I just used the STM HAL APIs and was basically done.
USB was harder. The last time I made a virtual serial port for the STM32, the mjmech gimbal board, I used the STM provided CDC libraries. However, those are released only under a rather restrictive license, confusingly named the “Ultimate Liberty Software License Agreement”. Despite the claims in the title, you actually have very little liberty, in that the software can only be used on STM controllers and has notice provisions as well. As with the rest of the moteus universe, I wanted to be able to release this firmware under a permissive license so needed to look elsewhere.
Github and google found a number of candidates, and the one that I ended up starting from was dmitrystu/libusb_stm32. It was already licensed under the Apache 2.0, and supported controllers which looked like they would be very similar to the STM32G4. Sure enough, the STM32L433 has an identical USB peripheral and was already supported.
I ended up forking that repository into the fdcanusb one and modifying it slightly to be usable with C++ and mbed. Also, interestingly, the CDC demo used the same endpoint for RX and TX, which the linux drivers didn’t seem to like so I just gave them separate endpoints like I’ve done before. I then implemented a wrapper for the CDC class which presents the same asynchronous serial interface as everything else in mjbots/mjlib and was set to go.
The final piece of the software was all the glue and application logic. I just used PersistentConfig and CommandManager from mjlib, which gave a human readable ASCII interface out of the box. Then I wrote up the application logic in CanManager, which didn’t have to do much at this point.
In its current state, the fdcanusb firmware is capable of communicating with the moteus r4.X series controllers just fine. Next I’ll get a few more made to distribute with moteus development kits and see if there is any more demand for them.