46а. CORDIC: sine, cosine and phase in a few clock cycles

Motor control, a phase detector, coordinate transforms — every PWM period you need to compute a sine, a cosine or the angle of a vector. A software sinf is a series or a table with multiplications. The G4, some H7 parts and the U5 have a CORDIC coprocessor: you write an angle, you read the sine and cosine. Here: where the accuracy comes from and how many clock cycles it costs, in what form to feed the numbers, and how not to lose the gain on small details.

Sources: the reference manual RM0440 (G4, the CORDIC chapter) from the Keil pack, the pack headers. The accuracy is checked with our own model of the algorithm on a PC — it is compared against the RM0440 accuracy table. The code is built with armclang for G474. QEMU does not model this block.

What it is called in the documentation

Original	Meaning
CORDIC	computing functions by rotating a vector through ever smaller angles
q1.31, q1.15	fixed-point numbers: a sign and 31 or 15 fractional bits
ARG1, ARG2	the first and second arguments
RES1, RES2	the first and second results
PRECISION	the number of calculation cycles: 4 iterations per cycle
SCALE	the 2^-n scale for functions with a wide range
zero-overhead mode	writing the argument starts the calculation at once, reading waits for the result

46а.1 What it computes and who has it

The CORDIC on G4 (RM0440, 17.2): a 24-bit rotation engine, circular and hyperbolic modes. Functions: sine, cosine, phase atan2(y, x), modulus √(x² + y²), arctangent, the hyperbolic sinh, cosh, atanh, the natural logarithm and the square root.

The block is present in the pack headers of the STM32G431 and G474, STM32H723 and STM32U585; the F407 and H743 do not have it. On the G4, next to it sits the FMAC, a filter accelerator; it is covered in section 46б.

Sine and cosine are computed in one operation: for the "sine" function RES1 is m·sin θ and RES2 is m·cos θ, where m is the modulus from ARG2. With m = 1 these are simply sin and cos; with another m, it is a conversion from polar to rectangular coordinates.

46а.2 Numbers: q1.31 and the angle divided by π

Arguments and results are in fixed point (RM0440, 17.3.3). q1.31 is a sign and 31 fractional bits: value = code / 2³¹, range from −1 to 1 − 2⁻³¹. q1.15 is the same in 16 bits.

The angle for sine and cosine is in radians, divided by π: the whole circle from −π to π fits into [−1, 1]. Examples:

+π cannot be represented in q1.31, but −π is the same angle. ARG2 after reset is 0x7FFF FFFF, that is, 1 − 2⁻³¹: a modulus of "almost 1", so for the sine you do not have to write it.

46а.3 Accuracy and clock cycles

CORDIC rotates the vector by ±atan(2⁻ⁱ) at step i, and each step adds roughly one correct bit. The G4 block does four iterations per cycle; the number of cycles is set by the PRECISION field (1…15, iterations = 4 × PRECISION). Table 115 of RM0440 gives the residual error for sine, cosine, phase, modulus and arctangent. Our own model of the algorithm on a PC — rotation by ±atan(2⁻ⁱ), 4001 angles from −π to π — with the datapath rounded to 2⁻²³ and without rounding:

Up to 16 iterations the model and the RM agree: a bit per iteration. Beyond that the error is held not by the algorithm but by the word width: without rounding the model would go to 2⁻²³, but the 24-bit datapath stops it at about 2⁻²⁰ — RM0440 promises no worse than 2⁻¹⁹. There is no point in setting more than 6 cycles for the sine. For q1.15 the limit, 2⁻¹⁵, is reached after 4 cycles already, and 3 cycles (2⁻¹¹) are usually enough for motor control.

The CORDIC gain after 24 iterations is 1.64676; the block compensates for it itself, the program does not need to divide.

46а.4 Code: a write starts, a read waits

The zero-overhead mode (RM0440, 17.3.6): CSR is configured once, and each calculation is a write of the argument to WDATA. Reading RDATA while the calculation is running inserts bus wait states, so you do not need to check the RRDY flag:

void cordic_sincos_init(void)
{
    RCC->AHB1ENR |= RCC_AHB1ENR_CORDICEN;
    CORDIC->CSR = CSR(F_SIN, 6) | CORDIC_CSR_NRES;          /* два результата: sin, потом cos */
}

void cordic_sincos(int32_t angle_div_pi, int32_t *s, int32_t *c)
{
    CORDIC->WDATA = (uint32_t)angle_div_pi;                 /* запуск: одного аргумента хватает */
    *s = (int32_t)CORDIC->RDATA;                            /* шина ждёт, пока расчёт не кончится */
    *c = (int32_t)CORDIC->RDATA;
}

armclang compiles the calculation into one write and two reads — no loop, no call:

cordic_sincos:  MOV   r3,#0xc08
                MOVT  r3,#0x4002
                STR   r0,[r3,#-4]
                LDR   r0,[r3,#0]
                STR   r0,[r1,#0]
                LDR   r0,[r3,#0]
                STR   r0,[r2,#0]

WDATA is 0x4002 0C04, RDATA is 0x4002 0C08. The time between the write and the first read can be filled with your own work: the read will wait exactly as long as is left.

The phase of a vector atan2(y, x) takes two arguments: NARGS = 1, first x, then y (the cordic_phase function). The result is the angle divided by π.

46а.5 q1.15: one write, one read

With ARGSIZE = 1 and RESSIZE = 1 both arguments are packed into one word — ARG1 in the low half, ARG2 in the high half — and both results arrive in one read: RES1 at the bottom, RES2 at the top. The angle 1/6 with modulus 0x7FFF is the word 0x7FFF 1555. For the sine this is sin in the low 16 bits and cos in the high bits (cordic_sincos_q15). Half the bus accesses — and DMA can stream angles into WDATA and the results into memory.

46а.6 The other functions and SCALE

For the arctangent, the logarithm and the root the argument range is wider than [−1, 1]: the argument is multiplied by 2⁻ⁿ in advance, and n is written to the SCALE field — the block returns the result with the same scale, and the program multiplies it back (RM0440, 17.3.4, tables 112 and 114). For the other circular functions (sine, cosine, phase, modulus) SCALE = 0, for the hyperbolic ones 1. The scale costs accuracy: the RM warns that the error grows in proportion to the scale.

46а.7 Pitfalls

• The angle in radians instead of divided by π. 0.5 is not half a radian but π/2. The sine is "almost right" at small angles and completely wrong at large ones.
• NRES = 1, but only one read. The next calculation starts only when the expected number of results has been read (RM0440, 17.3.6): the second read must be done even if you do not need the cosine — or set NRES = 0.
• ARG2 is left over from the previous function. The second argument is kept until the function is changed. After a phase with y = 0.3 the sine is computed with modulus 0.3.
• Extra cycles. PRECISION above 6 for the sine gives nothing but delay: the 24-bit datapath is already at its limit.
• The q1.15 halves are swapped. ARG1 and RES1 are in the low 16 bits. Swap them, and sine and cosine trade places.
• Modulus greater than 1. m·sin θ stays within [−1, 1] only when m ≤ 1: large vectors are scaled in advance.

46а.8 Summary

• CORDIC is on G431, G474, H723 and U585; F407 and H743 do not have it. Sine and cosine come in one operation.
• The angle is in radians, divided by π, in q1.31 or q1.15; π/6 is 0x1555 5555.
• Four iterations per cycle, a bit per iteration: 2⁻¹⁵ in 4 cycles, 2⁻¹⁹ in 6 — our own model agrees with table 115 of RM0440.
• A write to WDATA starts the calculation, a read of RDATA waits for the result: sine and cosine are one write and two reads.
• q1.15 is one write and one read for both values.

Full sources

cordic46a.c — CORDIC on the STM32G474 at register level: sine, cosine and phase.

/* 46а: CORDIC STM32G474 по регистрам — синус и косинус в q1.31 и q1.15, фаза вектора.
   Собирается armclang: -DSTM32G474xx. */
#include <stdint.h>

enum { F_COS = 0, F_SIN = 1, F_PHASE = 2, F_MOD = 3 };      /* поле FUNC — RM0440, 17.4.1 */

#define CSR(func, cycles) (((uint32_t)(func) << CORDIC_CSR_FUNC_Pos) | ((uint32_t)(cycles) << CORDIC_CSR_PRECISION_Pos))

_Static_assert(CORDIC_BASE == 0x40020C00u, "CORDIC на AHB1");
_Static_assert(CSR(F_SIN, 6) == 0x61u, "синус, 6 тактов = 24 итерации");

/* синус и косинус: угол в радианах, делённый на пи, q1.31; модуль (ARG2) не пишем — после
   сброса он 0x7FFFFFFF, то есть 1 - 2^-31 */
void cordic_sincos_init(void)
{
    RCC->AHB1ENR |= RCC_AHB1ENR_CORDICEN;
    CORDIC->CSR = CSR(F_SIN, 6) | CORDIC_CSR_NRES;          /* два результата: sin, потом cos */
}

void cordic_sincos(int32_t angle_div_pi, int32_t *s, int32_t *c)
{
    CORDIC->WDATA = (uint32_t)angle_div_pi;                 /* запуск: одного аргумента хватает */
    *s = (int32_t)CORDIC->RDATA;                            /* шина ждёт, пока расчёт не кончится */
    *c = (int32_t)CORDIC->RDATA;
}

/* q1.15: аргументы упакованы в одно слово, результаты — тоже; одна запись и одно чтение */
void cordic_sincos_q15_init(void)
{
    CORDIC->CSR = CSR(F_SIN, 3) | CORDIC_CSR_ARGSIZE | CORDIC_CSR_RESSIZE;   /* 12 итераций: 2^-11 */
}

uint32_t cordic_sincos_q15(int16_t angle_div_pi)
{
    CORDIC->WDATA = 0x7FFF0000u | (uint16_t)angle_div_pi;  /* ARG1 — младшая половина, ARG2 = 0x7FFF */
    return CORDIC->RDATA;                                   /* sin — младшая половина, cos — старшая */
}

/* фаза вектора (x, y): atan2(y, x) / пи; оба аргумента q1.31 — NARGS = 1, сначала x */
int32_t cordic_phase(int32_t x, int32_t y)
{
    CORDIC->CSR = CSR(F_PHASE, 6) | CORDIC_CSR_NARGS;
    CORDIC->WDATA = (uint32_t)x;
    CORDIC->WDATA = (uint32_t)y;
    return (int32_t)CORDIC->RDATA;
}

Next: section 46б "FMAC: the G4 filter accelerator — FIR and IIR without the processor".