Sunday, March 25, 2012

Eagle Tree Brushless RPM Sensors

In order to determine the amount of thrust that a motor is producing we must be able to measure the RPM of the motor. For a normal two wire DC motor the only solution would be to use some sort of optical sensor to watch the motor rotate and to count the number rotations. Usually this is done with an IR sensor that either senses a black stripe on the can of the motor, or watches the propeller and detects it's passing over the sensor.

With a brushless motor, we have a new option: monitoring the control pulses. A brushless motor has three control lines that go into the motor, and to make the motor spin the lines are pulsed in a specific order. The  timing of the pulses determine the speed of the motor, and the pulses must match the position of the motor rotor. Modern brushless motor controller chips (ESCs: Electronic Speed Controllers) have circuitry built into them that can automatically sense the position of the motor rotor and which could, in theory, be used by a host microcontroller to determine motor rotation speed. Unfortunately, most ESCs don't provide this sort of information to a host, so we'll have to measure the pulses directly and infer from the control pulses instead.

By taping into a brushless motor control wire we connect to the circuit show below. In that figure, each arrow is a pulse sent by the ESC to the motor, and each pulse is sent once per rotation. Now, imagine our RPM sensor has a tap right at the B marker. Pulses 1,4,2, and 5 all pass through B, so that's 4 pulses per rotation. For the remaining two pulses 3 and 6, the ESC sets B to be high impedance (ie unconnected). But for pulses 3 and 6 there is still a detectable pulse on B simply because a high impedance input can "skip" the b coil and measure the voltage at com, which has a pulse. With that, a single rotor revolution produces 6 measurable pulses.

In the above image, there are two magnetic poles. I suspect that if there were four magnetic poles the sensor would produce twice any many pulses, if there were six poles it would produce three times as many, and so on. I don't have a motor to test this on, but this is an interesting thread on the RCgroups forum that has a custom circuit to measure brushless RPM.

Anyway, this is my current theory on why the number of pulses have to be divided by 6. I've also heard  that outrunner motors have 6 poles, so that may be why. I'll have to take apart a motor and take a look at it to see what's up.

The Eagle Tree Brushless RPM sensor is the only ready made solution on the market. It's a very small device, priced at about $12 a piece, and it can sense the rotation rate of a single motor. It converts the brushless motor signals to a series of pulses where each pulse is equivelent to a rotation. We don't measure the signal directly from a brushless motor control line because there are very high voltages and back EMF from the motor, all creating a very nasty environment for our 3.3v microcontroller. So we use the Eagle Tree sensors instead.
The only Propeller source code that I could find came from a single forum post here. The code is under documented and very minimal, but it works. For the rest of this post I will analyze the object and show how it is used. I have attached a condensed version of the source code to the end of this post.

First, the circuit. In the object, it says
  Connect black to 3.3V, red to Gnd, white to prop pin  
And yes, it does work the way it is written. Despite going against standard color coding conventions, the red line is connected to ground and the black is to 3.3v (the Propeller VDD).

Next, connect both single red wires from the sensor to any two of the brushless motor lines. A single line would work, but I found that connecting both reduced erroneous readings on motor load situations. In a simple test, with only one lead connected there was a variation of 15 RPS, while with two there was a variation of 10 RPS (and a slightly higher bias).
The object will work for up to eight sensors, each with it's own Propeller I/O pin. These pins must be in a contiguous block of eight pins, regardless of how many sensors are actually used. The pins to use are declared by calling the setpins(_pinmask) function. Once the pins are set, call start to dedicate a new cog to the process.

The object times the number of clock cycles between rising edges of a pulse, and stores this value in the  Pins[n] variable. Most users probably don't care about the absolute time, so the getrps function will return the rotations per second of the specified channel. This function accounts for the six pulses per revolution, and the current system clock rate. It could be sped up a bit by making the division and multiplication done in assembly.

As currently written, the object supports up to 8 sensors, and regardless of how many you use the update time (minimum delta clock) will be the same since the code runs through the entire sequence on every iteration. You can remove the unused code, or if you have more than 8 motors (wow!) you can add more copies without too much loss of precision.

The (nearly complete) object. For the original, check out post four of this thread. For the latest and greatest, check out code.anzhelka.com.
Con
  Mhz    = (80+10)                                      ' System clock frequency in Mhz. + init instructions
   
VAR
  long  Cog
  long  Pins[8]
  long  PinShift                                          
  long  PinMask

PUB setpins(_pinmask)
'' Set pinmask for active input pins [­0..31]
'' Example: setpins(10_1001) to read from pin 0, 3 and 5
  PinMask := _pinmask
  PinShift := 0
  repeat 32
    if _pinmask & 1
      quit
    _pinmask >>= 1
    PinShift++ 
PUB start : sstatus
--- etc.
PUB stop
--- etc.
PUB getpinptr
  return @Pins
PUB getrpm(i) | delta
--- etc.
PUB getrps(i) | delta
'' Get the RPS of motor i (by index, not pin number)
'' Valid index range is 0-7
'' Returns -1 when no valid data
 if i > 7 OR i < 0 'Check Range
  return -1
 delta := Pins&#091;&#173;i&#093;
 if delta == 0
  return -1
 return (clkfreq / (delta*6))
DAT
        org   0
INIT    mov   p1, par                           ' Get data pointer
        add   p1, #4*8                          ' Point to PinShift
        rdlong shift, p1                        ' Read PinShift
        add   p1, #4
        rdlong pin_mask, p1                     ' Read PinMask
        andn  dira, pin_mask                    ' Set input pins

'=================================================================================

:loop   mov   d2, d1                            ' Store previous pin status
        waitpne d1, pin_mask                    ' Wait for change on pins
        mov   d1, ina                           ' Get new pin status 
        mov   c1, cnt                           ' Store change cnt                           
        and   d1, pin_mask                      ' Remove unrelevant pin changes
        shr   d1, shift                         ' Get relevant pins in 8 LSB
{
d2      1100
d1      1010
-------------
!d2     0011
&d1     1010
=       0010 POS edge
}
        ' Mask for POS edge changes
        mov   d3, d1
        andn  d3, d2

'=================================================================================

:POS    'tjz   d3, #:loop                       ' Skip if no POS edge changes
        mov   p1, par       ' Hub variable address
'Pin 0
        test  d3, #00_0001   wz    ' Change on pin?
        mov   d4, c1       ' Copy :loop count value to d4
        sub   d4, pe0       ' Subtract old count value from new count value ( delta(cv) = d4 - peo )
                  ' If pos change:
if_nz   cmp   d4, mintim wc      '  -> write c if d4 (delta count value) is less than minimum time
if_nz_and_nc wrlong d4, p1      ' -> write the delta count value to the hub if greater than minimum time
if_nz_and_nc mov   pe0, c1                      ' -> Store POS edge change cnt (system clk time, not delta)
            ' If no pos change:
if_z    cmp   d4, maxtim wc      '  -> write c if d4 (count value) is less than maximum time 
if_z_and_nc wrlong zero, p1      ' -> write zero to the hub if greater than maximum time

'Pin 1
        add   p1, #4
        test  d3, #00_0010   wz              ' ...
        mov   d4, c1
        sub   d4, pe1
if_nz   cmp   d4, mintim wc
if_nz_and_nc wrlong d4, p1
if_nz_and_nc mov   pe1, c1
if_z    cmp   d4, maxtim wc
if_z_and_nc wrlong zero, p1
'Pin 2
        add   p1, #4
        test  d3, #00_0100   wz
        mov   d4, c1
        sub   d4, pe2
if_nz   cmp   d4, mintim wc
if_nz_and_nc wrlong d4, p1
if_nz_and_nc mov   pe2, c1
if_z    cmp   d4, maxtim wc
if_z_and_nc wrlong zero, p1
'Pin 3
--- etc.
        jmp   #:loop

fit Mhz                                         ' Check for at least 1µs resolution with current clock speed
'=================================================================================
mintim  long  3000
maxtim  long  10_000_000
pin_mask long 00_0000
shift   long  0
c1      long  0      
d1      long  0
d2      long  0
d3      long  0
d4      long  0
p1      long  0
pe0     long  0
pe1     long  0
pe2     long  0
pe3     long  0
pe4     long  0
pe5     long  0
pe6     long  0
pe7     long  0
zero    long  0
        FIT   496