Speed estimator (position tracking loop), phase wrap issue

[Yansi]

Hello,

Background: I am trying to implement a position tracking loop or speed estimator respectively.  There is an quadrature encoder, from which the pulses are decoded and counted using a timer.  A 16 bit unsigned position value in the range od 0 to 2Pi is then derived from it. (0=0 rad, 65535 = 2Pi rad). Or this value can be whatever suitable form, even signed 16bit, i.e. scaled to -Pi to +Pi rad range. I need to estimate the instantaneous value of rotational speed (using a standard 2nd order loop using a PI regulator and integrator) .

The principle is described here: https://www.embeddedrelated.com/showarticle/530.php, see chapter "Tracking Loops".

The simple continuous time model assumes the position value increments (or decrements) infinitely and does not have bounds on the value, unlike the fixed point arithmetic I need to implement it in. Therefore I found the best solution to scale the full rotation on the full scale range of the unsigned (or signed) integer variable and then to "somehow magically" deal with the wrapping problem.

The problem with the wrapping of the Position and PositionEstimation was found not to be easy to be solved, if at all in this shape or form of the implementation. When the Position wraps, the PositionError value goes of course haywire, as it suddenly changes sign and saturates. Please see below the source code and the plot.

Please note regulator output is scaled to +-597, which effectively brings the PI reg output on the range of +-8192 RPM, as the calculation period is 66.7us (15kHz frequency). The number 597 is derived from how many 0-2pi wraps (full revolutions) are needed for the required +-8192 range this way:  (8192 * 2^16) / (60 * 15000) = 596.5.

The estimated speed seems to be correct in that part of the waveform, where the value settles, however gets periodically disrupted by the unpleasant overflow (or underflow) occurring at the Position signal.

What do I do wrong? How to handle the wrap?




Thank you for you help,
Y.

//EDIT: I might have finaly found the root cause of the issue. Will test thoroughly tomorrow. Now looking good, still a lot of things to be tweaked here and there.

[Kalvin]

You can get the absolute position from the encoder. Using the modulus properties of the integer values you can compute the difference between two positions ie. the change of the position without any problems.

Code: [Select]

#include <stdio.h>
#include <stdint.h>

#define STEP 1007

int16_t angle = -30000;

int main(void)
{
    printf("Forward:\n");
    for (int n = 0; n < 100; n++)
    {
        int16_t prev = angle;
        angle = angle + STEP;
        int16_t delta = angle - prev;
        printf("%d, %d, %d\n", n, angle, delta);
    }

    printf("Backwards:\n");
    for (int n = 0; n < 100; n++)
    {
        int16_t prev = angle;
        angle = angle - STEP;
        int16_t delta = angle - prev;
        printf("%d, %d, %d\n", n, angle, delta);
    }

    return 0;
}


[Yansi]

Not sure if I understand right what you have tried to explain me with your code snippet, but to explain further:

The encoder in my application is being wired to a HW timer with a function that supports directly counting pulses from a quadrature encoder via two input capture channels.  The timer reload value is set to the encoder pulse number times 4 (counting on every transition up or down). Meaning that for a 500 pulse encoder, the timer counts 2000 units per revolution. This value is further scaled to the full 16bit signed range (-Pi to 0 to +Pi).

The position integrator also utilizes a 16bit signed value, being a -Pi to +Pi full scale. Subtracting these two figures (the encodr scaled position and the integrated value) wraps correctly, no invalid overflow/underflow occurs. INT16 = INT16 - INT16  yields always correct value.

The problem was likely in my use of a saturation after the subtraction - the subtraction result got likely by my mistake typecast to int32 (which is what the saturation instruction expects) and this definitely screws the result up. The need for the saturation there is due to the REGC_SHIFT, which in the code above is 0. Purpose of that is that I can scale the PositionError value up to obtain higher PI controller gains, than unity gain. (The PI controller from the arm_math library is written so that it has a maximum of unity gain).

Currently, without the bit shift (increased gain) it is working fine. Please see below. A recording of a constant accelerating motor (current limited), then decelerating in free spin mode (supply disconnected).
(Please ignore the glitches at the end of the curve, that is a bug/fail in the STLINK programmer).