Embedded HandbookПриложения46а. CORDIC: синус, косинус и фаза за несколько тактов
Управление двигателем, фазовый детектор, преобразование координат —
каждый период ШИМ нужно посчитать синус, косинус или угол вектора.
Программная sinf — это ряд или таблица с умножениями. У G4, у части
H7 и у U5 есть сопроцессор CORDIC: пишешь угол, читаешь синус и косинус.
Здесь — откуда берётся точность и сколько тактов она стоит, в каком виде
подавать числа и как не потерять выигрыш на мелочах.
Опоры: справочное руководство RM0440 (G4, глава CORDIC) из пака Keil, заголовки паков. Точность проверена своей моделью алгоритма на ПК — она сверена с таблицей точности RM0440. Код собран armclang для G474. QEMU этот блок не моделирует.
Как это называется в документации
Оригинал Перевод CORDIC вычисление функций поворотами вектора на всё меньшие углы q1.31, q1.15 числа с фиксированной точкой: знак и 31 или 15 дробных бит ARG1, ARG2 первый и второй аргументы RES1, RES2 первый и второй результаты PRECISION число тактов расчёта: по 4 итерации за такт SCALE масштаб 2^-n для функций с широким диапазоном zero-overhead mode запись аргумента сразу запускает расчёт, чтение ждёт результат
46а.1 Что считает и у кого есть
CORDIC у G4 (RM0440, 17.2): 24-битный движок поворотов, круговой и гиперболический режимы. Функции: синус, косинус, фаза atan2(y, x), модуль √(x² + y²), арктангенс, гиперболические sinh, cosh, atanh, натуральный логарифм и квадратный корень.
Блок есть в заголовках пака у STM32G431 и G474, STM32H723 и STM32U585; у F407 и у H743 его нет. У G4 рядом стоит FMAC — фильтр-ускоритель; о нём — раздел 46б.
Синус и косинус считаются одной операцией: для функции «синус» RES1 — m·sin θ, RES2 — m·cos θ, где m — модуль из ARG2. С m = 1 это просто sin и cos; с другим m — переход из полярных координат в прямоугольные.
46а.2 Числа: q1.31 и угол, делённый на π
Аргументы и результаты — в фиксированной точке (RM0440, 17.3.3). q1.31 — знак и 31 дробный бит: значение = код / 2³¹, диапазон от −1 до 1 − 2⁻³¹. q1.15 — то же в 16 битах.
Угол для синуса и косинуса — в радианах, делённый на π: весь круг от −π до π помещается в [−1, 1]. Примеры:
| Величина | Значение | Код q1.31 |
|---|---|---|
| угол π/6 | 1/6 | 0x1555 5555 |
| sin(π/6) | 0,5 | 0x4000 0000 |
| cos(π/6) | 0,866025 | 0x6ED9 EBA1 |
| угол π/2 | 0,5 | 0x4000 0000 |
| угол ±π | −1 | 0x8000 0000 |
+π в q1.31 не представимо, но −π — тот же угол. ARG2 после сброса — 0x7FFF FFFF, то есть 1 − 2⁻³¹: модуль «почти 1», и для синуса его можно не писать.
46а.3 Точность и такты
CORDIC поворачивает вектор на ±atan(2⁻ⁱ) на шаге i, и каждый шаг добавляет примерно один верный бит. Блок G4 делает четыре итерации за такт; число тактов задаёт поле PRECISION (1…15, итераций = 4 × PRECISION). Таблица 115 RM0440 даёт остаточную ошибку для синуса, косинуса, фазы, модуля и арктангенса. Своя модель алгоритма на ПК — поворот на ±atan(2⁻ⁱ), 4001 угол от −π до π — с округлением тракта до 2⁻²³ и без него:
| Тактов | Итераций | RM0440, q1.31 | Модель, тракт 2⁻²³ | Модель без округления |
|---|---|---|---|---|
| 1 | 4 | 2^-3 | 2^-3,0 | 2^-3,0 |
| 2 | 8 | 2^-7 | 2^-7,1 | 2^-7,1 |
| 3 | 12 | 2^-11 | 2^-11,0 | 2^-11,0 |
| 4 | 16 | 2^-15 | 2^-15,0 | 2^-15,0 |
| 5 | 20 | 2^-18 | 2^-18,7 | 2^-19,0 |
| 6 | 24 | 2^-19 | 2^-20,4 | 2^-23,0 |
До 16 итераций модель и RM совпадают: бит за итерацию. Дальше ошибку держит уже не алгоритм, а разрядность: без округления модель шла бы к 2⁻²³, а 24-битный тракт останавливает её около 2⁻²⁰ — RM0440 обещает не хуже 2⁻¹⁹. Больше 6 тактов для синуса ставить незачем. Для q1.15 предел — 2⁻¹⁵ уже после 4 тактов, а 3 такта (2⁻¹¹) обычно хватает управлению двигателем.
Коэффициент удлинения CORDIC после 24 итераций — 1,64676; блок компенсирует его сам, в программе делить не нужно.
46а.4 Код: запись запускает, чтение ждёт
Режим без накладных расходов (RM0440, 17.3.6): CSR настраивается один раз, а каждый расчёт — запись аргумента в WDATA. Чтение RDATA, пока расчёт идёт, вставляет такты ожидания шины, поэтому проверять флаг RRDY не нужно:
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 собирает расчёт в одну запись и два чтения — ни цикла, ни вызова:
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 — 0x4002 0C04, RDATA — 0x4002 0C08. Время между записью и первым чтением можно занять своей работой: чтение подождёт ровно столько, сколько осталось.
Фаза вектора atan2(y, x) — два аргумента: NARGS = 1, сначала x, потом y
(функция cordic_phase). Результат — угол, делённый на π.
46а.5 q1.15: одна запись, одно чтение
С ARGSIZE = 1 и RESSIZE = 1 оба аргумента упакованы в одно слово —
ARG1 в младшей половине, ARG2 в старшей, — и оба результата приходят
одним чтением: RES1 внизу, RES2 вверху. Угол 1/6 с модулем 0x7FFF —
слово 0x7FFF 1555. Для синуса это sin в младших 16 битах и cos
в старших (cordic_sincos_q15). Вдвое меньше обращений к шине — и DMA
может гнать поток углов в WDATA, а результаты — в память.
46а.6 Остальные функции и SCALE
Для арктангенса, логарифма и корня диапазон аргумента шире [−1, 1]: аргумент заранее умножают на 2⁻ⁿ, а n пишут в поле SCALE — результат блок выдаёт с тем же масштабом, и его обратно умножают в программе (RM0440, 17.3.4, таблицы 112 и 114). Для остальных круговых функций (синус, косинус, фаза, модуль) SCALE = 0, для гиперболических — 1. Масштаб стоит точности: RM предупреждает, что ошибка растёт пропорционально масштабу.
46а.7 Грабли
- Угол в радианах, а не делённый на π. 0,5 — это не полрадиана, а π/2. Синус «почти правильный» на малых углах и совсем не тот на больших.
- NRES = 1, а читают один раз. Следующий расчёт начнётся, только когда прочитано ожидаемое число результатов (RM0440, 17.3.6): второе чтение надо сделать, даже если косинус не нужен, — или ставить NRES = 0.
- ARG2 остался от прошлой функции. Второй аргумент хранится, пока не сменили функцию. После фазы с y = 0,3 синус посчитается с модулем 0,3.
- Лишние такты. PRECISION больше 6 для синуса не даёт ничего, кроме задержки: 24-битный тракт уже на пределе.
- Перепутаны половины q1.15. ARG1 и RES1 — в младших 16 битах. Перепутали — и синус с косинусом поменялись местами.
- Модуль больше 1. m·sin θ не выйдет за [−1, 1] только при m ≤ 1: большие векторы масштабируют заранее.
46а.8 Итоги
- CORDIC — у G431, G474, H723 и U585; нет у F407 и H743. Синус и косинус — одной операцией.
- Угол — в радианах, делённый на π, в q1.31 или q1.15; π/6 — 0x1555 5555.
- Четыре итерации за такт, бит за итерацию: 2⁻¹⁵ за 4 такта, 2⁻¹⁹ за 6 — своя модель сходится с таблицей 115 RM0440.
- Запись в WDATA запускает расчёт, чтение RDATA ждёт результат: синус и косинус — одна запись и два чтения.
- q1.15 — одна запись и одно чтение на оба значения.
Полные исходники
cordic46a.c — CORDIC STM32G474 по регистрам: синус, косинус и фаза.
/* 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;
}
Дальше: раздел 46б «FMAC: фильтр-ускоритель G4 — КИХ и БИХ без процессора».