What happens if you accidentally write to address 0x00000000 on an STM32 microcontroller? Answer: usually almost nothing, because most linker scripts by default map a bank of flash there, and you can’t write to flash normally. The flash controller does notice and sets an error flag, but most applications aren’t exactly checking the flash peripheral’s error flags on a regular basis.
However, if you use the HAL to try and perform a flash operation, it doesn’t bother checking the error flags *before* trying to perform an operation. It just tries, and reports any errors it observes at the end. So, if you have an application that occasionally makes a spurious write to the zero address, and also performs flash operations, it will manifest as spurious failures of the flash operations.
How might one go about discovering which part of a large application is accidentally writing to address 0? The debug hardware on the STM32 is unable to use a watchpoint for peripheral addresses, like the flash controller’s error status. What I ended up doing was using the SYSCFG_MEMRMP register to make address zero be an alternate mapping of SRAM after the application has started. After which, you can set a data watchpoint on address 0 to get a break exactly when the spurious write occurs.
For me, that puts the ISR table there, but that isn’t a problem because I only needed to do this temporarily to use a watchpoint.
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!
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).
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.
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 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.
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.
I’ve been developing a new bi-directional spread spectrumradio 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.
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.
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.
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!
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.
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.
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.
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.