Tag Archives: stm32g4

Unlimited rotations for moteus

The moteus controller has always supported multiple turns when counting positions. It has a one-revolution magnetic encoder built in, but after turn on, it keeps track of how many turns have occurred. However, if you’ve followed previous moteus tutorials, you have probably noticed a persistent caveat that for accurate control, the position of the output shaft needs to stay within a hundred revolutions of 0.0 or so. Now, I’ll describe why that was, and what I’ve done to remove the limitation, allowing unlimited rotations!

Background

The moteus controller uses a somewhat unique integrated position / velocity / torque control loop. This formulation gives a couple of advantages: First, there is no bandwidth loss due to having a cascaded position and velocity controller. Second, when driven by a higher level controller, it can seamlessly switch between position, velocity, and torque control, or any combination of them without having to manage mode transitions.

The command consists of the following values, all as 32 bit floating point values (optionally upscaled from integer values using the register protocol).

  • Desired Position
  • Desired Velocity
  • Feedforward Torque
  • kp scale
  • kd scale
  • Maximum Torque

The control loop measures two quantities as input, the “current position” and the “current velocity”. The position is measured as a 32 bit signed integer, where one revolution of the magnetic encoder equals 65536 counts. The velocity is numerically differentiated across the most recent 6.4ms of movement.

There are two internal state variables as well: One is the “target position”. This captures the most recent position command, and is advanced by the velocity command at the full control rate. The other is the integrative term of the PID controller. Both of these are stored as 32 bit floating point values.

The problem

This structure poses a few inherent limitations. One, being that as the control position is sent as a floating point value, the resolution available for positioning decreases as you get further from 0. That probably isn’t a big limitation, as there aren’t many applications where you want to have both absolute positions and also unlimited revolutions.

The bigger limitation is in the “target position” internal state variable. It needs to be updated to take into account the current velocity command at every control cycle, or 1/40000 of a second. For a commanded speed of 0.01 revolutions per second, this incremental update is only 2.5e-7 of a revolution. Given that 32 bit floating point values only have roughly 7 decimal digits of mantissa available to them, you don’t have to get far beyond 0 before an update that small doesn’t even change the value at all.

The command format also has an option, such that if the command position is set to a floating point NaN value, it will “capture” the current position. This can be used to command velocity-only control with an implicit integrative term, or when combined with a stop position to move to a target at a fixed velocity. However, since “capturing” stores the value as a floating point value, significant precision can be lost. This was only a problem at larger position values, but at the maximum position before wraparound, the available capture resolution was measured in multiple degrees.

The resolution

The resolution was relatively straightforward. Instead of storing the “target position” as a floating point value, it is now stored as a 64 bit integer measured in 1/(2**32) of a magnetic encoder revolution. This gives sufficient precision to represent velocities as small as 0.0001Hz (0.036 dps) uniformly at all positions, while still having more absolute range than the measured current position value. The final PID controller is then expressed relative to the target position. This lets it still operate in floating point coordinates, but with no worry about large artifacts due to a position offset.

The only other implementation hurdle was making it run fast enough. Largely that revolved around ensuring there was never a need to convert between 64 bit integers and floating point values, which is relatively slow on the STM32G4.

The result

With this fix in place, it is possible to operate the controller safely at high velocities for arbitrary periods of time. Even when the “current position” value wraps around from positive to negative! Also, low speed control works just as well at any position offset. When operating in those “continuous rotation” applications, the user should just be careful about if the “desired position” field of the command should be set. Largely, it should be left as NaN for when used in continuous rotation applications.

Here’s a video showing high speed wraparound and low speed at arbitrary offsets.

Spread spectrum integration

I’ve been developing a new bi-directional spread spectrum radio to command and control the mjbots quad robot.  Here I’ll describe my first integration of the protocol into the robot.

To complete that integration, I took the library I had designed for the nrfusb, and ported it to run on the auxiliary controller of the pi3 hat.  This controller also controls the IMU and an auxiliary CAN-FD bus.  It is connected to one of the SPI buses on the raspberry pi.  Here, it was just a matter of exposing an appropriate SPI protocol that would allow the raspberry pi to receive and transmit packets.

Slightly unfortunately, this version of the pi3hat does not have interrupt lines for any of the stm32s.  Thus, I created a multiplexed status register that the rpi can use to check which of the CAN, IMU, or RF has data pending.  Then I slapped together a few registers which allowed configuring the ID and reading and writing slots and their priorities.

Then I refactored things around on the raspberry pi side so that one core would keep busy polling for one of those things to become available.  So far, for the things which access SPI, I’ve been putting them locked to an isolcpu cpu to get improved SPI timing.  Eventually, once I have interrupt lines, I might consolidate all of these down to a single core.  That, plus defining an initial mapping between the controls and slots resulted in:

Finally, I created a very simple GL gui application which connects to an nrfusb and a joystick.  It uses Dear ImGui to render a few widgets and glfw to window and read the joystick.

2020-04-03-145917_1280x720_scrot

While I was at it, I finally updated my joystick UI to make gait selection a bit faster, and got the robot to do a better job of switching out of the walk gait.  Thus the following video showing all of that hooked together.

Power distribution board r3

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.

dsc_0394

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:

dsc_0393

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:

dsc_0395

 

Bringing up CAN on the quad pi3 hat

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.

dsc_0347
A lot of scope probes!

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.

20200226-leg-transaction

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!

Bringing up the IMU on the pi3 hat

The next peripheral to get working on the quad’s raspberry pi interface board is the IMU. When operating, the IMU will primarily be used to determine attitude and angular pitch and roll rates.  Secondarily, it will determine yaw rate, although there is no provision within the IMU to determine absolute yaw.

To accomplish this, the board has a BMI088 6 axis accelerometer and gyroscope attached via SPI to the auxiliary STM32G4 along with discrete connections for interrupts.  This chip has 16 bit resolution for both sensors, decent claimed noise characteristics, and supposedly the ability to better reject high frequency vibrations as seen in robotic applications.  I am currently running the gyroscope at 1kHz, and the accelerometer at 800Hz.  The IMU is driven off the gyroscope, with the accelerometer sampled whenever the gyroscope has new data available.

My first step was just to read out the 6 axis values at full rate to measure the static performance characteristics.  After doing that overnight, I got the following Allan Variance plot.

20200304-bmi088-allan-variance

That gives the angular random walk at around 0.016 dps / sqrt(Hz) with a bias stability of around 6.5 deg/hr.  The angular random walk is about what is specified in the datasheet, and the bias is not specified at all, but this seems really good for a MEMS sensor.  In fact, it is good enough I could probably just barely gyrocompass, measuring the earth’s rotation, with a little patience.  The accelerometer values are shown there too, and seem fine, but aren’t all that critical.

Next up is turning this data into an attitude and rate estimate.

New quad raspberry pi interface board

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:

dsc_0339

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!

fdcanusb up at mjbots.com

I’ve received my first production run of the fdcanusb CAN-FD USB adapters and they are up for sale at mjbots.com!

fdcanusb_angle

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.

CAN bootloader for moteus r4.x

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.

 

Bringing up the fdcanusb

I introduced the fdcanusb previously, now I’ll describe some of the process and challenges in getting it to work.

Hardware

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.

Software

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.

Next steps

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.

 

More adventures in STM32G4 bringup

Last time I had an actually hard problem…, a mis-aligned stack which caused randomly misbehaving software.  This time, I had a more prosaic problem.  One that while not necessarily as interesting, was even more time consuming and frustrating.

The serial port wouldn’t work.  I had copied the module I used for DMA based receive and transmit from the STM32F4 and it just wasn’t working.  Nothing was written and nothing was received.  I carefully inspected the software many times.  I looked at the registers in the debugger and nothing seemed obviously amiss.  I read the datasheet to look for subtle differences in the theory of operation between the STM32F4 and STM32G4 but came up empty.

Eventually, I went completely out of the box and compiled one of the STM provided UART examples from STM32CubeMX.  That did work!  But how?  I once again carefully inspected its source and mine and saw no obvious differences.

Then, I took the ultimate slow path in debugging and binary searched, gradually transforming the STM provided example into my code half by half to figure out which transformation was the ultimate cause.  This did do the trick.  The final fix:

-  huart_.Init.WordLength = 8;
-  huart_.Init.StopBits = 1;
+  huart_.Init.WordLength = UART_WORDLENGTH_8B;
+  huart_.Init.StopBits = UART_STOPBITS_1;

That’s it.  In the old version, I passed in to the HAL the word size and number of stop bits as integers.  Somehow this actually worked, despite being documented as requiring the necessary #defines even in that version.

Notch another win up to: “it sure would be nice if there was less undefined behavior in C/C++”.