CORDIC (розрахунок кута)

[radioman]

Розрахунок кута за формулою Джорджа Пейна дає значну похибку в діапазоні кутів 14-25 та 60-78 градусів та незначну при 0, 45 та 90 градусів. Більш точним є обчислення за алгоритмом CORDIC; його різні варіанти реалізації подано на сайті University of Glasgow .
В "безмежній павутині" викладено однакову реалізацію на різних площадках, але без посилання на першоджерело, тому і я його не подаю

Код: Виділити все

//cordic
// https://radioman.com.ua/viewtopic.php?f=9&t=20&p=80#p80
/*
одиниця = 0x40000000 (з плаваючою точкою 1073741824.00000)
величина зворотна коефіцієнту деформації 1 / k = 0x26DD3B6A (з плаваючою точкою  0.6072529350088812561694)
у таблиці cordic_tab зберігаються розраховані значення atan (2 ^ -i) * mul 
*/
#define _CORDIC_32_
//константи
#define cordic_1K 0x26DD3B6A    // 1/k = 0.6072529350088812561694
#define half_pi 0x6487ED51      // pi / 2
#define MUL 1073741824.000000   // 1.0 = 1073741824.00000
#define CORDIC_NTAB 32
int cordic_tab [] = {
  0x3243F6A8, 0x1DAC6705, 0x0FADBAFC, 0x07F56EA6, 0x03FEAB76, 0x01FFD55B, 0x00FFFAAA, 0x007FFF55,
  0x003FFFEA, 0x001FFFFD, 0x000FFFFF, 0x0007FFFF, 0x0003FFFF, 0x0001FFFF, 0x0000FFFF, 0x00007FFF,
  0x00003FFF, 0x00001FFF, 0x00000FFF, 0x000007FF, 0x000003FF, 0x000001FF, 0x000000FF, 0x0000007F,
  0x0000003F, 0x0000001F, 0x0000000F, 0x00000008, 0x00000004, 0x00000002, 0x00000001, 0x00000000
};
//змінні
int intSY;
int intSX;
int intCordic;
/*------------------------------------------------------------------------------------------------------------*/
intCordic = cordic32_atan2(intSX, intSY, MUL);
// так як алгоритм видає результат в радіанах, отримане значення потрібно помножити на 57.29578 для переведення у градуси
Serial.print("CORDIC="),Serial.print(intCordic * 57.29578 / MUL)  
/*------------------------------------------------------------------------------------------------------------*/
int cordic32_atan2(int s, int c, int n)
{
  int k, d, intAngle = 0;
  int re = c, im = s, tx;
  n = (n > CORDIC_NTAB) ? CORDIC_NTAB : n;
  if (n < 3) n = 3;
  for (k = 0; k < n; ++k)
  {
    d = im >> 31;
    intAngle += ((cordic_tab[k] ^ d) - d);
    tx = re;
    re += (((im >> k) ^ d) - d);
    im -= (((tx >> k) ^ d) - d);
  }
  return intAngle;
}

На форумі, звідки я його взяв, обговорення даної теми припинилося з 2013 року із зауваженням автору про його неточність. Я не тестував його на всіх значеннях, але в металодетекторі на 50 копійок 1994 року він розрахував кут 54 градуси, наближена формула в 52 (при розрахунковій фазі 53 градуси); фазовий кут на кусочок стальної катанки становив -32.5 градуси (наближена формула - 35, розрахунковий - 33).
Значення кута, котрий потрібно розрахувати, має бути в інтервалі від -90 до 90 (в моїх експериментах функція "зависала" при наближенні кута до 100 градусів.

PS. Більш детальні роз'яснення (на англійській) у прикріпленому файлі:

cordic.7z

CORDIC

(122.6 Кіб) Завантажено 1100 разів

[pawa]

Можна і по звичайній таблиці Брадіса обчислювати, тим більше значень не так вже й багато треба.

[radioman]

Можна і по звичайній таблиці Брадіса обчислювати, тим більше значень не так вже й багато треба.

Код в студію (с)

PS. Для експериментів з розрахунком ВДІ приєднав табличку ексель.

SIN_COS.zip

VDI

(12.64 Кіб) Завантажено 1128 разів

[pawa]

А ще задавшись необхідною точністю можна розкласти у ряд Тейлора.

[radioman]

А ще задавшись необхідною точністю можна розкласти у ряд Тейлора.

Точність достатня до 1 градуса (для VDI); для балансу грунту - вища, але там не критична швидкодія.

PS. Хоча, для балансу грунту поворотом вектора достатньо "привести" Х та У до 1. Псевдокод:

Код: Виділити все

// псевдокод
intSX/sqrt(intSX*intSX+intSY*intSY)*1.00000;
intSY/sqrt(intSX*intSX+intSY*intSY)*1.00000;

[pawa]

Ось обчислення за таблицею Брадіса:

Код: Виділити все

const uint8_t BradisTableLength = 89;
const uint16_t BradisTable[BradisTableLength] = {
	18, 36, 54, 72, 90, 108, 126, 144, 162, 181, 199, 218, 236, 255, 274, 294,
	313, 333, 353, 373, 393, 414, 435, 456, 478, 499, 522, 544, 568, 591, 615,
	640, 665, 691, 717, 744, 772, 800, 829, 859, 890, 922, 955, 989, 1024, 1060,
	1098, 1137, 1178, 1220, 1265, 1311, 1359, 1409, 1462, 1518, 1577, 1639, 1704,
	1774, 1847, 1926, 2010, 2100, 2196, 2300, 2412, 2534, 2668, 2813, 2974, 3152,
	3349, 3571, 3822, 4107, 4435, 4818, 5268, 5807, 6465, 7286, 8340, 9743, 11704,
	14644, 19539, 29324, 58665
};

uint8_t atang (uint16_t x, uint16_t y) {
	if (y > 0){
		uint32_t tmpX = x;
		tmpX = tmpX << 10;
		x = tmpX / y;
		for (uint8_t i = 0; i < BradisTableLength; i++){
			if (x < (*(BradisTable + i))) return i;
		}	
	}
	return 90;
}

Значення округлюються до найменшого, тобто треба доповнити код алгоритмом обчислення з апроксимацією, що дасть більш точний результат.

[radioman]

Гарна робота

... треба доповнити код алгоритмом обчислення з апроксимацією, що дасть більш точний результат.

Не думаю, що варто ускладнювати код - коливання величини кута, вирахуваного іншим методом, становить біля 0,5 градуса; так що цієї точності більш чим достатньо:

Коливання величини кута

Код не враховує від'єних значень "Х" і працює в цьому випадку з помилкою. Також спостерігається "випадіння" одного градуса (2-0); у зв'язку з цим я для Ардуіно дещо модифікував сам алгоритм - інвертував знак вхідних даних по "Х" при від'ємних значеннях та додав ще одне значення у масив:

Код: Виділити все

//брадіс 
// https://radioman.com.ua/viewtopic.php?f=9&p=81#p81
const int intBradisTableLength = 90;    // розмір масиву з таблицею Брадіса
// Масив з таблицею тангенсів Брадіса домножених на 1024 (2 в 10 ступені)для цілочисельних розрахунків
int BradisTable[intBradisTableLength] = {
  18, 36, 54, 72, 90, 108, 126, 144, 162, 181, 199, 218, 236, 255, 274, 294,
  313, 333, 353, 373, 393, 414, 435, 456, 478, 499, 522, 544, 568, 591, 615,
  640, 665, 691, 717, 744, 772, 800, 829, 859, 890, 922, 955, 989, 1024, 1060,
  1098, 1137, 1178, 1220, 1265, 1311, 1359, 1409, 1462, 1518, 1577, 1639, 1704,
  1774, 1847, 1926, 2010, 2100, 2196, 2300, 2412, 2534, 2668, 2813, 2974, 3152,
  3349, 3571, 3822, 4107, 4435, 4818, 5268, 5807, 6465, 7286, 8340, 9743, 11704,
  14644, 19539, 29324, 58665, 91964
};
/****функція визначення кута за таблицею Брадіса (pawa)****/
int atang (long x, long y) {
  if (x < 0) x = -1 * x;                                      // якщо x< 0 інвертуємо знак
  if (y > 0) {                                                // "У" завжди повинен бути позитивним
    long tmpX = x;
    tmpX = tmpX << 10;                                        // домножуємо "Х" на 1024=2 в 10 ступені (у таблиці домножено) - зсув виконується швидше
    x = tmpX / y;                                             // розрахунок тангенса кута "помноженого" на 1024
    for (int i = 0; i < intBradisTableLength; i++) {          // "перебігаємо" по таблиці від меншого кута до більшого
      if (x < (*(BradisTable + i))) return i;                 // якщо тангенс вимірюваного кута менший табличного - рухаємось далі
    }
  }
  return 90;                                                  // якщо "У" від'ємний (помилка аналогової частини) - виводимо кут в 90 градусів
}

PS. Даний код можна модифікувати для повороту вектора, додавши ще одну таблицю із значеннями sin/cos. Але функція може повертати лише одне значення; для того щоб обійти це обмеження можна використати присвоєння значення змінним у функції, котрі будуть "видні" за її межами (volatile).

[pawa]

Не думаю, що варто ускладнювати код

Оце добре!