BLDC motor phase 3: finding the limits of MicroPython

With motor position sensor and motor drivers in hand, I set to work actually implementing the “real” control algorithm. I was expecting MicroPython to be “too slow” for this kind of work but, initially it seemed to be working well so I thought I’d push it as far as I could…

The key control loop of the field oriented control (FOC) algorithm that I had in mind is as follows:

  • Measure the angle of the wheel.
  • Convert that to an angle relative to the magnetic poles of the motor.
  • To rotate in one direction, add ~90 degrees to that pole angle.
  • To rotate in the other direction, subtract ~90 degrees to that pole angle.
  • Drive the coils to create a magnetic field at the adjusted angle.
  • Vary the strength of the drive to vary the torque (and hence the speed).

What does those steps actually look like?

Measuring the angle of the wheel starts with the PIO programs in phase 2. Once the PIO programs are configured:

sm0 = rp2.StateMachine(0, measure_high_time, freq=125_000_000,  in_base=pin18, jmp_pin=pin18)
sm1 = rp2.StateMachine(4, measure_interval, freq=125_000_000, in_base=pin18, jmp_pin=pin18)

Reading the PIO values looks like this in MicroPython:

    # Read the interval from the measure_interval PIO
    # sm1.get() blocks until the result is ready.
    #
    # PIO program sends 0xffffffff minus the count so 
    # we need to undo that...
    ivl = 0xffffffff - sm1.get()

    while True:
        # Get the "on" time from the measure_high_time PIO.
        high = 0xffffffff - sm0.get()

        # Extract out the angle measurement; which should be 
        # between 0 and 4095.  The 4119 and 15 come from the
        # datasheet of the sensor.
        duty = high*4119//ivl - 15

        # angle in degrees would be (duty * 360) / 4096
        ...
        

Once we have the wheel angle, we need to convert it to an angle relative to the coils/magnets in the motor. This accounts for the fact that a BLDC motor has three wires wound through it (let’s call them A, B, C) but they are not just wound into 3 separate electromagnets at 120 degrees. Instead, there are many small elecromagnets around the circumference of the motor: first an A coil, then a B coil, then a C and so on ABCABC… The rotor has a similar but different number of permanent magnets attached. The overall effect is that, to spin the motor through 360 degrees, we need to cycle power to the 3 coils, N times instead of just once. It’s like the motor is “geared down”.

With that waffly explanation out of the way(!) in code it’s very simple, we just multiply the angle by half the number of “poles” in the motor, 11 in my case. I chose to keep representing angles by 0-4095 because powers of 2 are generally easier to work with. We also need to add a calibration offset to account for the alignment of the sensor on the motor:

pole_angle = (duty * 11 + calibration_offset) % 4096

OK, now for an easy step; depending on which direction we want to rotate, add or subtract 90 degrees. In my prototype I just hard coded it. We add 1024 because the “degrees” I’m using run from 0-4095:

pole_angle = (pole_angle + 1024) % 4096

Why 90 degrees? It has been worked out that, when two magnets (in our case the permanent magnets in the rotor and the electromagnets we’re about to drive) are at 90 degrees to each other, that gives maximum torque. And, with energy being conserved, maximum torque means doing maximum work to drive the motor round and minimal work to make heat in the coils. We need to constantly keep moving the magnetic field to always be 90 degrees ahead of the permanent magnets in order to maximise torque and minimise heat build up.

OK, we’ve got an angle that we want to drive the magnetic field to, how do we do that? The literature makes it sound very difficult and I suspect that there are much more complex algorithms that take more factors into account, but the basic idea is to drive the three inputs of the motor with 3 sine wave voltages, each 120 degrees out of phase with each other. That energises the three coils in a way that the overall voltage sums to zero but, by varying the phase of the three inputs together, we can rotate the magnetic field.

In microcontroller land, we can’t really alter the voltage, but we can use PWM, which is good enough. At the top of the program we set up 3 PWM pins. It’s important that the PWMs run in phase with each other and unfortunately, MicroPython doesn’t expose that feature so we need to poke directly at the PWM registers…

# Define our PWM pins
pwms = [PWM(Pin(n)) for n in [14, 15, 16]]

# Set the PWM frequency.
f = 40000
for pwm in pwms:
    pwm.freq(f*2)  # *2 for phase accurate PWM

# Define constants for the PWM registers that we need to poke.
# These come from the RP2040 datasheet.
PWM_BASE = 0x40050000
PWM_EN = PWM_BASE + 0xa0
PWM_INTR = PWM_BASE + 0xa4

# Note: CH0 and CH7 are the right banks for pins 14-16.
CH0_CSR = PWM_BASE + 0x00
CH0_CTR = PWM_BASE + 0x08
CH7_CSR = PWM_BASE + 0x8c
CH7_CTR = PWM_BASE + 0x94

# Disable all PWMs.
machine.mem32[PWM_EN] = 0
# Set phase correct mode on the two PWM modules that
# relate to pins 14-16.
machine.mem32[CH0_CSR] = machine.mem32[CH0_CSR] | 0x2
machine.mem32[CH7_CSR] = machine.mem32[CH7_CSR] | 0x2
# Reset the counters so that the PWMs start in-phase.
machine.mem32[CH0_CTR] = 0
machine.mem32[CH7_CTR] = 0
# Enable the PWMs together so they stay in sync.
machine.mem32[PWM_EN] = (1<<0) | (1 <<7)

Then, since the RP2040 has plenty of RAM but floating point is slow, I created a lookup table holding the values of sine(angle) for various fractions of the circle where the output value is scaled correctly to be a PWM value (MicroPython always uses 0-65535 for PWM values).

lut_len = 4096
# Add a slight offset to compensate for the on delay of the 
# motor driver.
offset = 0.016
lut = [
    min(
        int(
            (
                (math.sin(i * 2 * math.pi / lut_len) + 1)/2 +
                 offset
            ) / 
            (1+offset) * 
            65535
        ),
        65535
    ) for i in range(lut_len)]

With the look up table pre-calculated, setting the PWM in the main loop is as simple as:

pwms[0].duty_u16(lut[angle%lut_len])
pwms[1].duty_u16(lut[(angle+(4096/3))%lut_len])
pwms[2].duty_u16(lut[(angle+(4096*2/3))%lut_len])

With all that glued together and after much debugging (missing the divide by 2 in the look-up table and having all the values wrap being my favourite!) It actually worked… but the motor juddered every few revs. (Sorry, didn’t get a video of this stage.)

After trying a few things to debug I tried adding a debug pin and toggled it high at the start of the loop and then low at the end. That found the problem:

Most of the time, the debug pin (green) toggled on for a few microseconds each time the position sensor sent a pulse (yellow) but sometimes it was multiple milliseconds!

While debugging code by oscilloscope was new to me, it immediately made me think of garbage collection. Turns out that MicroPython’s GC takes a few ms to run, which is fine for a lot of tasks but not for BLDC control. I did try a few approaches to tame the GC (using the machine code decorators in MicroPython, for example) but I think the PWM library was doing allocations under the hood. To avoid that, I’d have needed to directly poke memory and wouldn’t be able to use any libraries without fear of introducing GC again. Not much fun! It was time to switch to the C SDK…

BLDC controller phase 2: adding feedback through the magic of PIOs

After my hello world of BLDC control I waited eagerly for some breakout boards to arrive. To get past the hello world stage I needed:

  • Some sort of position sensor. I chose the AS5048A Hall effect sensor because it was known to work well with SimpleFOC and the motor that I had bought came with the right sort of magnet to work perfectly with it.
  • A decent motor driver. Up to now I’d been using two L298Ns (to get 3 half bridges), which I had on hand, but that’s all there is to recommend it! At time of ordering, TI seemed to have the best available stocks of motor drivers so I picked up a DRV8313 breakout board. The DRV8313 is a dedicated three-phase motor driver capable of 2.5A and 60V, more than enough for my needs.

The position sensor arrived first and I set about connecting it to my Pico. There are two options for that: SPI, or PWM. I had planned to use SPI because it’s faster and the Pico has an SPI port. However, the pads on the breakout board for SPI were super-small and I didn’t have any wire on hand that was small enough(!) Maybe I should try the PWM option for now? But how to interface PWM to the Pico…?

The PWM output from the sensor sends an (approx) 1KHz signal to the Pico and it varies the “on” time of the signal depending on the angle of the wheel. To decode that precisely, you need to measure the “on” time and the “off” time precisely.

Yellow trace is the PWM signal.

Now, the “straightforward” way to do that would be to set an interrupt on the pin and to use the microsecond clock to measure the time between changes in the signal in the interrupt handler. But the RP2040’s datasheet basically says you’re a clown if you don’t use the PIO peripheral for all your bit-banging needs. I don’t want Eben to think I’m a clown! So, I dove into PIO assembler…

Each PIO peripheral has several state machine cores, which are like mini CPUs that execute their very specialised instructions with very precise timing. The specialised instructions can do things like read the state of a pin, push some data to the main processor over a FIFO, or pull data from the main processor and push it out on one or more pins. In fact, one instruction can often do several of those things, all in one clock cycle. The trade off for this precision and specialisation is that each PIO block has only 32 words of instruction memory, shared between 4 cores!

So, what did I come up with? First I wrote a PIO program to measure the “on” time. How hard can it be, just write a program that:

  • Waits for the pin to go high.
  • Increments a counter while high.
  • When pin goes low, push the counter to the CPU.

Waiting for a pin to change is very easy in the PIO, but incrementing a counter turned out to be a problem! The PIO has no “add” or “increment” instruction! 🤔

You can write the PIO code in MicroPython using a special syntax:

@rp2.asm_pio()
def measure_high_time():
    wrap_target()

    # Set x register to 0
    mov(x, null)

    # Wait for a HIGH.
    wait(1, pin, 0)

    label("high_loop")
    # ???? what goes here ????

    jmp(pin, "high_loop")

    mov(isr, x)
    push(noblock)

    # Not a real instruction, tells the PIO that, after
    # the push(), it should continue after the wrap_target().
    # Saving a jump is cool when you've only got 32 
    # instructions!
    wrap()

Scouring the PIO manual in the RP2040 datasheet, it took me a while to see the answer, but in the end it was the only option. The only “arithmetic” operation that the PIO has is “decrement x and jump if x is non-zero”. If we only have “decrement”, can we do that instead?

  • Set X to something large
  • Wait for pin to be high
  • While pin is high, decrement X (and somehow turn the unwanted jump into a no-op)
  • When pin goes low, send x to CPU

It took me a while to puzzle out how to set X to something large but I came up with this:

wrap_target()
# x = 0
mov(x, null)
# Decrement x, which wraps around to 0xffffffff
# x was 0 so the jump falls through.
jmp(x_dec, "wait")

# Wait for a HIGH.
label("wait")
wait(1, pin, 0)

label("high_loop")
# Decrement x; jump always fires because x is non-0.
jmp(x_dec, "cont_high_loop")
# Jump lands on next instruction anyway, staying inside
# Loop.
label("cont_high_loop")
jmp(pin, "high_loop")

# Send x to the CPU.
mov(isr, x)
push(noblock)

wrap()

And it worked! The value sent to the CPU is 0xffffffff minus the count but that’s easily corrected.

I was able to adapt the approach to make a second PIO program that measures the full cycle time of the PWM (i.e. “on” time + “off” time). That was a little trickier because there’s no equivalent to “jmp(pin)” that loops while a pin is low. The code is here in case it’s useful.

Of course, as soon as I showed Lance my code, he Googled the problem and found someone else had an even neater solution. Turns out you can save a whole instruction(!) by using

mov(x, invert(null))

to set x to 0xffffffff directly. You live and learn!