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

[arnaljl]

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]

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: