Velocity estimation is made possible by a cool feature of the HostMot2 firmware:

The FPGA synthesizes a configurable-frequency "timestamp clock" by dividing ClockLow by the value in the Quadrature Counter Timestamp Divider Register. ClockLow is 33 MHz on the PCI cards and 50 MHz on the 7i43.

The current value of the Timestamp Clock can be read from the 16-bit Timestamp Count (TSC) register.

When a quadrature counter instance in the HostMot2 FPGA detects a transition in its input Gray code, it increments the count and latches both the (16-bit) count and the 16 bits of the timestamp clock into the Counter Register.

Some Random Notes:

  • The encoder Count & Time ("C&T") are both 16-bit registers, which are latched and read together.

  • The encoder Count is initialized to 0 at firmware load time.

  • The Timestamp clock runs at 1 MHz (can run faster, but 1 meg is pretty fast and makes it easy to think about).

  • The Timestamp Counter is 16 bits wide. It rolls over about every 65 milliseconds.

  • In the timing pics below i'm using a very slow servo loop of about 50 Hz. The interval between reading the C&T register and the TSC register is also rediculously large.



The velocity estimator used by the driver is similar to one described by David Auslander in a paper titled " Vehicle-based Control Computer Systems" (UCB ITS PRR 95 3).

Algorithm notes:

  1. The algorithm maintains some internal state.

    • Current-Motion-Mode: Stopped or Moving.

      • Stopped means V=0 and there is no known previous datapoint. This is the starting state. If a new datapoint comes while we're in the Stopped state, we record the datapoint as the "old" datapoint and move to the Moving state.

      • Moving means there is a record of a recent previous datapoint (Counts and Timestamp). If we're in the Moving state and no new datapoint is seen for some runtime-configurable amount of time (250 ms or so perhaps), the algorithm forgets the old datapoint and moves to the Stopped state.

    • Old datapoint: (Count, Timestamp) datapoint of most recently seen encoder edge.

      Only valid when Current Motion Mode is Moving.

      The count is the raw 16-bit count from the firmware, extended to signed 32-bit. In other words, the count in the datapoint has been adjusted for rollover (both positive and negative).

      The timestamp is the raw timestamp of that edge. Time has not been adjusted for rollovers, that's handled elsewhere.

    • R: The timestamp rollover counter.

      Only valid when Current Motion Mode is Moving.

      This is the number of times the timestamp clock has rolled over its 16-bit counter since the "old" datapoint.

    • Previous Time of Interest: This is a relevant timestamp from the previous time through the loop. (Described in the section on Rollover Detection below).

      Only valid when Motion Mode is Moving.

      Think of this as "the time up through which we've tracked TSC rollovers so far".

  2. On startup, the Current Motion Mode is set to Stopped, so Old Datapoint, R, and PTI are all dont-cares.

  3. If the Motion Mode is Stopped and no new datapoint comes in: Nothing to do, return.

  4. If the Motion Mode is Stopped and a new datapoint comes in:

    • Set Old Datapoint to the new datapoint.
    • Set R to 0.
    • Set PTI to the new datapoint's timestamp.
    • Set Motion Mode = Moving.
    • Leave V at 0 for now.
    • Finished!

  5. Motion Mode is Moving, no new datapoint available:

    • TI = TSC, check for rollover.
    • dT is ((TI - ODT) + (R * 2^16))
    • dT > Horizon? If so V = 0 and MM = Stopped.
    • Still waiting: use UBVE
    • PTI = TI

  6. Motion Mode is Moving, new datapoint comes in:

    • TI = T, check for rollover
    • dT is ((TI - ODT) + (R * 2^16))
    • R = 0
    • Use RTVE
    • Old Datapoint = new datapoint.
    • PTI = TI

  7. Rollover detection:

    • Rollover detection happens right after reading the registers, before doing anything else.

    • If there was a new datapoint, the Time of Interest Ti is the datapoint's timestamp.

    • If there was no new datapoint, Ti is the TSC.

    • If Ti > 2^15, rollover detection reports no rollover, records prevTi=Ti, and it's done.

    • If Ti > prevTi, rollover detection reports no rollover, records prevTi=Ti, and it's done.

    • If we get here, it's a rollover! Increment rollover count R.

  8. Relative Time Velocity Estimator (RTVE):

    Used when the C&T read indicates a new datapoint n (in addition to having an old datapoint m).

    V = dS/dT = (Cn-Cm) / ((Tn-Tm) + (R*2^16))

    Reset R to 0.

  9. Upper Bound Velocity Estimator (UBVE):

    Used when the C&T read indicates no new datapoint.

    So make one up (n), using the previous datapoint m and the current TSC: (Cn=Cm+1, Tn=TSCn). The Cm+1 is + or - 1 to be in the same direction as Vm

    V = dS/dT = (Cn-Cm) / ((Tn-Tm) + (R*2^16))

    There was no real datapoint, so we didn't use up our rollovers, so we don't reset R to 0.

  10. Timestamp clock speed and servo period are constrained together. C shows nearly a worst-case scenario: edge comes just after the C&T read at 2, gets picked up at 3, then we need to detect rollover at 4.

    The servo frequency must be fast enough that Ti has time to switch from an old datapoint-T to a TSC before the rollover. Two servo periods must be less than half the rollover time.


Legend:


1. No encoder edge

2. Encoder edge before TSC read

3. Encoder edge between TSC read and C&T read

A. No rollover

B. Rollover happens first

C. Rollover happens second

D. Rollover happens third

There is no Case D1



Case A1: No rollover, no encoder edge.

MM = Stopped

Nothing to do.

MM = Moving

TI is TSC. Do rollover check. We detect rollover here if PTI was pre-rollover, which means the previous time through the loop was case C1 or C2 or D2 or D3.

dT is ((TI - OT) + (R * 2^16)). dT > Horizon? If so V = 0 and MM = Stopped.

We'll wait a little longer, Use UBVE.

PTI gets the value of TI (TSC).


Case A2: No rollover, encoder edge before TSC read.

MM = Stopped

Old Datapoint = new datapoint

R = 0

MM = Moving

PTI = T

MM = Moving

TI = T, do rollover detection. We detect rollover here if PTI was pre-rollover.

Use RTVE.

OD = new datapoint

PTI = TI (= T)


Case A3: No rollover, encoder edge between TSC read and C&T read.

Identical to A2.


Case B1: Rollover happens first, no encoder edge

MM = Stopped

Nothing happens.

MM = Moving

TI = TSC. Check for rollover. Rollover detected, because PTI is either T or TSC from the previous servo loop, which ended just before the rollover.

Compute dT, check for Stop.

Use UBVE.

PTI = TI (= TSC)


Case B2: Rollover happens first, encoder edge before TSC read.

MM = Stopped

Identical to A2.

MM = Moving

TI = T, check for rollover, rollover detected, because PTI is either T or TSC from the previous servo loop, which ended just before the rollover.

RTVE

PTI = TI (= T)


Case B3: Rollover happens first, encoder edge between TSC read and C&T read

Identical to B2


Case C1: Rollover happens second, no encoder edge

MM = Stopped

Nothing to do.

MM = Moving

TI = TSC, check for rollover, no rollover detected (because TSC is just before the rollover).

UBVE

PTI = TI (= TSC)


Case C2: Rollover happens second, encoder edge before TSC read.

MM = Stopped

Like A2.

MM = Moving

TI = T, check for rollover, no rollover detected (because T is just before the rollover).

RTVE

PTI = TI (= T)


Case C3: Rollover happens second, encoder edge between TSC read and C&T read

Identical to B2 and B3.


Case D2: Rollover happens third, encoder edge before TSC read.

MM = Stopped

Like A2.

MM = Moving

TI = T, check for rollover, no rollover detected (because T is just before the rollover).

RTVE

PTI = TI (= T)


Case D3: Rollover happens third, encoder edge between TSC read and C&T read.

Identical to D2.