CORDIC Phases results largely differs from q1.31 to q1.15

arnaljl · ‎2025-01-09

Hello everyone !

I've recently hit a snag in a motor FOC project of mine: the CORDIC unit on a STM32G431xB doesn't give out similar q1.15 results when the input are 32 and 16 bits.

In the current setup, both sine an cosine coefficient are pre-stored as q1.15 values in two separate sensor input buffers (from hall resolvers).

In order to simulate actual q1.31 value, I simply left-shift the buffered q1.15 values by 16 upon use, and tolerates the LSB loss for the purposes of the tests.

Here's an excerpt of the test method :

[main loop]

float *pResA = resultsA, *pResB = resultsB; // For the purpose of the test, our q1.15 results will be converted to degrees as f32 values.

cnt = NB_ANGLE_SAMPLES;

CordicUnit::clearError();

while (cnt-- && !CordicUnit::hasError()) {

// Input sin/cos coeffs' are stored in q1.15 buffers, addressed through *pSin and *pCos.

// Both functions returns q1.15 results

*pResA++ = 180.0f + 180.0f * ((float)CordicUnit::atan2_i31_o15(int32_t(*pSin) << 16, (int32_t)(*pCos) << 16) / 32767); // fed truncated Q1.31 values.

*pResB++ = 180.0f + 180.0f * ((float)CordicUnit::atan2_i15_o15(*pSin, *pCos) / 32767); // fed the actual Q1.15 values.

pSin++;

pCos++;

}

[/main loop]

One would expect resultA and resultB contents to vary only by a few percents, however, that's not the case :

results from atan2_i31_o15

results of CORDIC Phase (atan2) with two Q1.31 iinputs

results from atan2_i15_o15

Results of CORDIC Phase (atan2) with two Q1.15 inputs

Wut ?

Here are the declarations & definitions of both, bare-metal CordicUnit::atan2xxxxx functions :

[declarations]

enum CordicFunction_e : uint32_t{

CORDIC_ATAN2_I31_O15 = (CORDIC_CSR_RESSIZE | CORDIC_NBWRITE_2 | (4 << CORDIC_CSR_PRECISION_Pos) | (2 << CORDIC_CSR_FUNC_Pos)),

CORDIC_ATAN2_I15_O15 = (CORDIC_CSR_ARGSIZE | CORDIC_CSR_RESSIZE | (4 << CORDIC_CSR_PRECISION_Pos) | (2 << CORDIC_CSR_FUNC_Pos)),

};

[/declarations]

[definitions]

int16_t CordicUnit::atan2_i31_o15(int32_t y, int32_t x)

{

if (_completedLast == false)

return 0;

CORDIC->CSR = CordicFunction_e::CORDIC_ATAN2_I31_O15;

CORDIC->WDATA = (uint32_t)x;

CORDIC->WDATA = (uint32_t)y;

_completedLast = false;

uint32_t start = micros(); // timeout timestamp

// Wait for completion, with some ample timeout (500µs)

while ((CORDIC->CSR & CORDIC_CSR_RRDY) == 0 && ((micros() - start) < 500))

asm(" nop");

// Timed out ?

if ((CORDIC->CSR & CORDIC_CSR_RRDY) == 0) {

_cordicHasErrors = true;

return 0;

}

_completedLast = true;

return (CORDIC->RDATA & 0xFFFFU); // discards the MSW and its modulus result

}

int16_t CordicUnit::atan2_i15_o15(int16_t y, int16_t x)

{

if (_completedLast == false)

return 0;

CORDIC->CSR = CordicFunction_e::CORDIC_ATAN2_I15_O15;

CORDIC->WDATA = (((uint32_t)y) << 16) | (uint32_t)x;

_completedLast = false;

uint32_t start = micros(); // timeout timestamp

// Wait for completion, with some ample timeout (500µs)

while ((CORDIC->CSR & CORDIC_CSR_RRDY) == 0 && ((micros() - start) < 500))

asm(" nop");

// Timed out ?

if ((CORDIC->CSR & CORDIC_CSR_RRDY) == 0) {

_cordicHasErrors = true;

return 0;

}

_completedLast = true;

return (CORDIC->RDATA & 0xFFFFU); // discards the MSW and its modulus result

}

[/definitions]

I'm quite at a loss here.

Using either q1.15 or q1.31 inputs isn't a costly time issue, but I cannot fathom why the results differ so much.

Any idea ?

arnaljl · ‎2025-02-25

Nothing ? Was my exposé too long ? :confounded_face:

Well, having no response at all, and the CORDIC phase calculations still returning gibberish
I had to implement the CORDIC atan2 on the sensor side ( hosted by a measly CH32V003, but it manages to push out samples at 25Ksps, so I'm not complaining -yet- )

Here's the function code, for those interested : its crude, unoptimized, but gets the job done :

/** @brief Returns the angle of a cartesian coordinate relative to zero, expressed in the range [-1.0; 1.0]
 * @PAram y,x Q15 signed numbers representing the sine and cosine components of the angle to be returned. */
*/
int16_t cordic_atan2_q15(int16_t y, int16_t x)
{
    // Handle special cases where x or y is zero
    if (x == 0 && y == 0) {
        return 0;
    }
    if (x == 0) {
        return (y > 0) ? 16384 : -16384; // π/2 or -π/2 in scaled Q15
    }
    if (y == 0) {
        return (x > 0) ? 0 : 32767; // 0 or π (clamped to 32767)
    }

    // Determine the quadrant and take absolute values of x and y
    int quadrant;
    int16_t abs_x = abs(x);
    int16_t abs_y = abs(y);

    if (x > 0 && y > 0) {
        quadrant = 0;
    } else if (x < 0 && y > 0) {
        quadrant = 1;
    } else if (x < 0 && y < 0) {
        quadrant = 2;
    } else {
        quadrant = 3;
    }

    // CORDIC algorithm in vectoring mode
    int32_t x32 = (int32_t)abs_x;
    int32_t y32 = (int32_t)abs_y;
    int32_t z   = 0;

    // Precomputed atan_table in scaled Q15 (atan(2^-i)/π * 32768), rounded to nearest integer
    static const int16_t atan_table[] = { 8192, 4836, 2555, 1297, 650, 326, 163, 81, 41, 20, 10, 5, 3, 1 };
    const int num_iterations          = sizeof(atan_table) / sizeof(atan_table[0]);

    for (int i = 0; i < num_iterations; ++i) {
        int32_t x_shifted = x32 >> i;
        int32_t y_shifted = y32 >> i;

        if (y32 >= 0) {
            x32 += y_shifted;
            y32 -= x_shifted;
            z += atan_table[i];
        } else {
            x32 -= y_shifted;
            y32 += x_shifted;
            z -= atan_table[i];
        }
    }

    // Adjust the angle based on the original quadrant
    int32_t adjusted_angle;
    switch (quadrant) {
        case 0:
            adjusted_angle = z;
            break;
        case 1:
            adjusted_angle = 32768L - z;
            break;
        case 2:
            adjusted_angle = z - 32768L;
            break;
        case 3:
            adjusted_angle = -z;
            break;
        default:
            adjusted_angle = 0;
            break;
    }

    // Clamp the result to the valid Q15 range [-32768, 32767]
    if (adjusted_angle > 32767) {
        adjusted_angle = 32767;
    } else if (adjusted_angle < -32768) {
        adjusted_angle = -32768;
    }

    return (int16_t)adjusted_angle;
}

Hope it helps. Never got the CORDIC coprocessor to work correctly anyway. :sad_but_relieved_face: