nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

QuatCore: измерение крена

Реализуем очередную часть аффинного алгоритма на худо-бедно устоявшемся ассемблере, для которого уже спроектирована на верилоге аппаратная часть, и можно её запустить на симуляции в Quartus. Если повезёт, на этой неделе и до реальной ПЛИС доберёмся.

Это была одна из самых сложных частей: она должна сначала посчитать синус и косинус угла крена, затем превратить их в кватернион крена и "убрать" крен из матрицы аффинного преобразования, чтобы там остались только масштаб и ракурсы.

Пока я мучал это дело на эмуляторе - придумывал самые укуренные команды, вроде ABSP1D2 (взять модуль, прибавить единицу, поделить на два) или [Y+S] - к регистру Y прибавить флаг знака, и по этому адресу положить значение. А то и вовсе и [Y+~S] - то же самое, только знак сначала инвертируется.

Такие команды могли бы существовать - ABSP1D2 действительно можно сделать на АЛУ, пришлось бы добавить ещё один режим аккумулятору - сейчас их 7 [Spoiler (click to open)]idle, load, clear, RoundZero, ThreeHalf,MinusThreeHalf, Two, а был бы ещё OneHalf - и усложнить модуль управления, чтобы выдал правильные команды. Да и [Y+S] и [Y+~S] можно было бы впихнуть наверное, но пока всё-таки обойдёмся без них, благо это не так уж сложно.

В итоге, вместо 48 слов, которые занимал старый код, у нас сейчас выходит 52 слова - не так уж и плохо. Здесь мы впервые используем самую "кватернионную" команду FMPM (Fused Multiply - Plus Minus), "кватернионную" адресацию i^j, и для кучи - умножение знакового на беззнаковое число MULSU (Multiply Signed-Unsigned) для нормировки. Как будто этого мало - ещё адрес UAC (Unsigned ACcumulator) и команда SQRSD2 (SQuaRe Subtract divided by 2).

"С нахрапу" получить верный ответ не получилось, поэтому исследовал выполнение довольно подробно. Оказалось - ошибка пустяковая, я в компилятор неправильно забил "коды команд", две строчки перепутал, вместо знаково-беззнакового умножения подсунул целиком беззнаковое.


То удивительное чувство, когда результаты симуляции выдали до бита такой же ответ, как и самописный эмулятор до этого.


В первую очередь, приведём код этой процедуры:
[Может, не надо?][Оно что-то отожранное...]
	;состояние регистров к этому моменту
	;X = AffineMat (константная матрица 3х4, чтобы посчитать преобр)
	;Y = Points2D
	;Z = AfTransf (матрица 2х2 и вектор 1х2)
	;i=j=k=Inv=0
	;C, Acc - пофиг наверное
	FindRoll	proc
		;нам понадобятся [Z]+[Z+3] и [Z+1]-[Z+2]
				Y			QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
									;(потом они занулятся)
				;а можно Y вместо X, если надо
				i			1
				j			3
		@@sico:		Acc			[Z+i]
				Inv			i
				PM			[Z+i^j]
				[Y+i]		Acc
				iLOOP		@@sico
	;введя зело специал. команды [Z+i^3], PMi, можно сэкономить аж 2 строки (по 1 строке на команду!)
	;пока не будем этого делать...
	
	;теперь наших друзей надо отнормировать, причём динамический диапазон очень велик
	;максимум 13 итераций, давайте столько и делать...
	;здесь у нас появляется беззнаковое число, без него получалось бы очень грустно	
				CALL NormSiCo
	;здесь у нас i=j=k=Inv=0
	
	;теперь синус-косинус превращаем в кватернион, то есть в синус-косинус половинного угла
	;если co>0, то
	;QuatX = (1+abs(co))/2,
	;QuatY = si/2
	;в противном случае
	;QuatX = si/2,
	;QuatY = (1+abs(co))/2
	
	;сейчас X = AffineMat (матрица 3х4, уже неактуальна)
	;Y = QuatY (компоненты co / si),
	;Z = AfTransf (матрица 2х2 и вектор 1х2, к ним ещё вернёмся)
	;i=j=k=Inv=0
					
;				ABSP1D2	[Z+2j+k]	;самая укуренная команда-взять модуль B, прибавить единицу, и поделить на 2!
;замена для ABSP1D2:
				X		OneHalf
				ABS		[Y+k]
				DIV2	Acc
				ADD		[X+k]
;конец замены
	;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
	;но от адресов [Y+S] и [Y+~S] мы пока что избавились. Придётся их "сэмулировать"
				j		1
				JGE		@@skip
				i		1		;выходит i=S
		@@skip:		X		QuatA
				[X+i]		Acc
				DIV2		[Y+1]
				[X+i^j]		Acc			;по сути, Y+(~S)
				Y		QuatA
				CALL		NormSiCo	;подготовили первые 2 компонента кватерниона
					
	;сейчас X=Y=QuatA (2 компоненты кватерниона),
	;Z = AfTransf
	;теперь помножаем две матрицы, одна в явном виде - AfTransf (Z)
	;вторая - задана неявно, в виде co / si (в QuatY)
	;результат идет в AfTransf,но не сразу,
	;чтобы не повредить
	;здесь i=1, k неизвестен, j=0, Inv=0
				Y		QuatY
				i		1	;номер строки результата, и строки co/si
		@@i_loop:	k		1	;номер столбца результата, и столбца AfTransf
		@@k_loop:	j		1	;номер столбца co/si и строки AfTransf
				ZAcc		RoundZero	;обнулить до 1/2 мл. разр
		@@j_loop:	C		[Y+i^j]	
				FMPM		[Z+2j+k]
				jLOOP		@@j_loop
				[SP+2i+k]	Acc
				kLOOP		@@k_loop
				iLOOP		@@i_loop
	;результат лежит в стеке, теперь его нужно переместить на законное место...
				i			3
		@@out_loop:	C			[SP+i] 	;эх, грустно без полноценных 2 адресов. Может, сделать-таки?
				[Z+i]		C		
				iLOOP		@@out_loop	


	FindRoll	endp


Чтобы затем проследить выполнение программы, нам нужен листинг. Он генерится компилятором в файл RomContents.txt. [Spoiler (click to open)]
main proc
00  FD47              SP          StackAdr
01  8AFC              C           [SP]
02  FD83              SP          C
03  F3B0              CALL        AffineAlgorithm
04  A209              k           9           
05  00E8  @@loop:     NULL        [Z+k]  
06  AA7F              kLOOP       @@loop
07  B807  @@endless:  JMP         @@endless
main endp
AffineAlgorithm     proc
    Associate4Points    proc
        FindMDD3    proc
08  DD18                      Y           Points2D
09  F000                      [SP+1]      0   ;максимальная отдалённость, инициализируем нулём
0A  A103                      j           3
0B  A201          @@j_loop:   k           1   ;также от 0 до 3, чтобы все расстояния просуммировать
0C  FC00                      [SP]        0   ;здесь будет храниться текущий максимум
0D  A003          @@k_loop:   i           3   ;от 0 до 1, т.е значения X и Y
0E  80D9          @@i_loop:   Acc         [Y+2i+k]    ;загрузили одно значение
0F  83DA                      SUB         [Y+2j+k]    ;вычитаем второе
10  9C80                      SQRD2       Acc         ;возводим в квадрат
11  82FC                      ADD         [SP]        ;прибавляем к пред. значению
12  FC80                      [SP]        Acc
13  A87B                      iLOOP       @@i_loop        ;теперь то же самое для второй координаты
14  AA79                      kLOOP       @@k_loop
15  83F0                      SUB         [SP+1]  ;можно и "пожертвовать" значением в Acc,
16  B004                      JL          @@skip
17  8AFC                      C           [SP]
18  F083                      [SP+1]      C
19  CDA1                      X           j
1A  A971          @@skip:     jLOOP       @@j_loop
1B  A0CD                      i           X
1C  CD18                      X           Points2D
1D  ED18                      Z           Points2D
1E  F3B3                      CALL        SwapPoints  ;потёрли текущий максимум (лежал в [SP])-и хрен с ним
        FindMDD3    endp
        SortCCW     proc
1F  CD1A                      X           Fx1 ;чтобы индекс от 2 до 0 соотв. точкам (Fx1,Fy1) ... (Fx3, Fy3)
20  ED1C                      Z           Fx2 ;чтобы иметь сдвинутую на 1 адресацию
21  A301                      Inv         1   ;пока что выкинули команды ijk, без них чуть толще, но даже понятнее
22  F3B2                      CALL        ShiftOrigin
23  A101                      j           1
24  A001                      i           1
25  8AEA          @@loop:     C           [Z+2j+k]
26  90C1                      MUL         [X+2i+1]
27  8AE2                      C           [Z+2j+1]
28  93C9                      FMS         [X+2i+k]    ;нахождение "векторного произведения"
29  B002                      JL          @@skip
2A  F3B3                      CALL        SwapPoints
2B  A87A          @@skip:     iLOOP       @@loop
2C  A979                      jLOOP       @@loop
2D  A300                      Inv         0
2E  F3B2                      CALL        ShiftOrigin     
2F  CD1E                      X           Fx3
30  F3B3                      CALL        SwapPoints
        SortCCW     endp
    Associate4Points endp
    Compute4PointAffine proc
31  CD33                      X           AffineMat
32  ED72                      Z           AfTransf
33  A201                      k           1   ;номер строки результата (и строки AffineMat)
34  A102          @@k_loop:   j           2   ;номер столбца результата (и столбца Points2D)
35  A003          @@j_loop:   i           3   ;номер столбца AffineMat и строки Points2D
36  8803                      ZAcc        RoundZero   ;обнулить до 1/2 мл. разр
37  8AC7          @@i_loop:   C           [X+4j+i]
38  92D9                      FMA         [Y+2i+k]
39  A87E                      iLOOP       @@i_loop
3A  EA80                      [Z+2j+k]    Acc
3B  A97A                      jLOOP       @@j_loop
3C  AA78                      kLOOP       @@k_loop
    Compute4PointAffine endp
    FindRoll    proc
3D  DD7A                      Y           QuatY   ;в этом кватернионе построим,используя Y/Z значения как временные
3E  A001                      i           1
3F  A103                      j           3
40  80E4          @@sico:     Acc         [Z+i]
41  A3A0                      Inv         i
42  81EC                      PM          [Z+i^j]
43  D480                      [Y+i]       Acc
44  A87C                      iLOOP       @@sico
45  F3B1                      CALL NormSiCo
46  CD63                      X       OneHalf
47  84D8                      ABS     [Y+k]
48  8C80                      DIV2    Acc
49  82C8                      ADD     [X+k]
4A  A101                      j       1
4B  B102                      JGE     @@skip
4C  A001                      i       1       ;выходит i=S
4D  CD78          @@skip:     X       QuatA
4E  C480                      [X+i]   Acc
4F  8CD0                      DIV2    [Y+1]
50  CC80                      [X+i^j] Acc         ;по сути, Y+(~S)
51  DD78                      Y       QuatA
52  F3B1                      CALL    NormSiCo    ;подготовили первые 2 компонента кватерниона
53  DD7A                      Y           QuatY
54  A001                      i           1   ;номер строки результата, и строки co/si
55  A201          @@i_loop:   k           1   ;номер столбца результата, и столбца AfTransf
56  A101          @@k_loop:   j           1   ;номер столбца co/si и строки AfTransf
57  8803                      ZAcc        RoundZero   ;обнулить до 1/2 мл. разр
58  8ADC          @@j_loop:   C           [Y+i^j] 
59  91EA                      FMPM        [Z+2j+k]
5A  A97E                      jLOOP       @@j_loop
5B  F980                      [SP+2i+k]   Acc
5C  AA7A                      kLOOP       @@k_loop
5D  A878                      iLOOP       @@i_loop
5E  A003                      i           3
5F  8AF4          @@out_loop: C           [SP+i]  ;эх, грустно без полноценных 2 адресов. Может, сделать-таки?
60  E483                      [Z+i]       C       
61  A87E                      iLOOP       @@out_loop                  
    FindRoll    endp
    FindScale   proc
62  FC00                      [SP]        0   ;манхэттенская метрика, сумма модулей (метрика городских кварталов)
63  F000                      [SP+1]      0   ;чебышевская, максимум от модулей 
64  A103                      j           3                   
65  A001                      i           1
66  80E4          @@loop:     Acc         [Z+i]   ;Txy (i=1), Txx (i=0)
67  81EC                      PM          [Z+i^j] ;+Tyx (i=1), -Tyy (i=0)
68  8480                      ABS         Acc     ;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
69  8A80                      C           Acc     ;сохранили это значение
6A  82FC                      ADD         [SP]    
6B  FC80                      [SP]        Acc ;обновили манхэттенскую
6C  80F0                      Acc         [SP+1]
6D  8383                      SUB         C
6E  B102                      JGE         @@skipCheb ;лучше подошел бы JGE и поменять местами [SP+2] и [SP+1]. Разница при нуле-на 1 больше операций.
6F  F083                      [SP+1]      C   ;обновили Чебышевскую   
70  A301          @@skipCheb: Inv         1       ;так что на следующей итерации будет "-"
71  A875                      iLOOP       @@loop
72  80E4                      Acc         [Z+i]   ;добавили Txx
73  82EC                      ADD         [Z+i^j] ;и Tyy
74  CD31                      X           MetricW
75  A001                      i           1
76  8AF4          @@finExpr:  C           [SP+i]
77  92C4                      FMA         [X+i]
78  A87E                      iLOOP       @@finExpr
    FindScale   endp
    FindVector  proc
    FindVector  endp
79  A201                      k           1
7A  D800          @@clean:    [Y+k]       0
7B  AA7F                      kLOOP       @@clean
7C  B8FF              JMP         [--SP]
AffineAlgorithm     endp
SwapPoints  proc
7D  A201              k           1
7E  80EA  @@swap:     Acc         [Z+2j+k]
7F  8AC9              C           [X+2i+k]
80  EA83              [Z+2j+k]    C
81  C980              [X+2i+k]    Acc
82  AA7C              kLOOP       @@swap
83  B8FF              JMP         [--SP]
SwapPoints  endp    
ShiftOrigin proc
84  A201                  k           1
85  A002  @@k_loop:       i           2   ;как всегда, чтобы Y и X
86  80C9  @@i_loop:       Acc         [X+2i+k]
87  81D8                  PM          [Y+k]
88  C980                  [X+2i+k]    Acc
89  A87D                  iLOOP       @@i_loop
8A  AA7B                  kLOOP       @@k_loop
8B  B8FF                  JMP         [--SP]
ShiftOrigin endp
NormSiCo    proc
8C  A10C                  j           12 ;если "передавать" i как аргумент, то можно регулировать число итераций
8D  A001  @@norm:         i           1       ;две компоненты
8E  8802                  ZAcc        ThreeHalf       ;выставить 3/2
8F  9FD4  @@innorm:       SQRSD2      [Y+i]   ;вычесть половину от квадрата 
90  A87F                  iLOOP       @@innorm
91  8A82                  C           UAC
92  A001                  i           1
93  94D4  @@inmult:       MULSU       [Y+i]   ;уможение знакового B на беззнак. C
94  D480                  [Y+i]       Acc     ;вернули на место!
95  A87E                  iLOOP       @@inmult                
96  A977                  jLOOP       @@norm
97  B8FF                  JMP         [--SP]
NormSiCo    endp



Смотрим, что там происходит.

У нас с прошлого этапа есть матрица


В регистре Z помещён адрес нулевого элемента этой матрицы. Нужно сложить элементы главной диагонали и обозвать результат: co (ненормированное значение косинуса), вычесть друг из друга два оставшихся элемента и обозвать результат: si. Всё это дело умещается на одном скриншоте:


Мы наблюдаем этот кусочек кода:
3D  DD7A                      Y           QuatY   ;в этом кватернионе построим,используя Y/Z значения как временные
3E  A001                      i           1
3F  A103                      j           3
40  80E4          @@sico:     Acc         [Z+i]
41  A3A0                      Inv         i
42  81EC                      PM          [Z+i^j]
43  D480                      [Y+i]       Acc
44  A87C                      iLOOP       @@sico
45  F3B1                      CALL NormSiCo


Видим, как шестнадцатеричные значения в листинге соответсвуют регистрам PC (program counter) и шинам DestAddr в "осциллограмме". Значение SrcAddr, увы, в первой строчке, где QuatY, не совпадает - с тех пор я решил чуть передвинуть переменные в памяти, чтобы потом можно было одним куском показать и кватернион, и матрицу. А дальше всё соответствует.

Как сказано в комментарии, мы запихиваем co и si в компоненты Y и Z кватерниона взаимной ориентации, как "временное пристанище". На стек класть не хотим - ещё процедуру вызывать, потрётся. А здесь - почему бы и нет...

Мы здесь опять страдаем из-за хитроумной адресации. Нам всего-то нужно посчитать:
co = Z[0]+Z[3];
si = Z[1]-Z[2];

причём адреса всех 4 слагаемых - константы, заранее известные.
Это могло быть что-то вроде
Acc [Z]
ADD [Z+3]
[co]  Acc
Acc [Z+1]
SUB [Z+2]
[si] Acc

но прямая адресация вот что-то совсем не лезет, или адресация через регистры+маленькие константы. Поэтому на ровном месте придумываем цикл по i от 1 до 0. С помощью строки
Inv i

и используя команду АЛУ PM (plus-minus), добиваемся, чтобы на первой итерации было вычитание (находим si), а на второй - сложение.
А присвоив
j   3

и используя адресацию i^j, добиваемся, чтобы второе слагаемое располагалось по диагонали от первого.

На осциллограмме можно наблюдать, как всё происходит - как присваиваем значения индексным регистрам i, j, как на первой итерации Inv=1, а на второй: Inv=0. Как на первой итерации мы обращаемся по адресам F7, F8, а на второй - F6 и F9.

И можем отследить получившиеся результаты, и куда они заносятся. По адресу F2 мы заносим si = -1, а по адресу F1: co = 0x368 = 872.

Теперь эти значения надо отнормировать, чтобы они действительно соответствовали косинусу и синусу. Для этого мы вызываем процедуру NormSiCo, вот её листинг:

NormSiCo    proc
8C  A10C                  j           12 ;если "передавать" i как аргумент, то можно регулировать число итераций
8D  A001  @@norm:         i           1       ;две компоненты
8E  8802                  ZAcc        ThreeHalf       ;выставить 3/2
8F  9FD4  @@innorm:       SQRSD2      [Y+i]   ;вычесть половину от квадрата 
90  A87F                  iLOOP       @@innorm
91  8A82                  C           UAC
92  A001                  i           1
93  94D4  @@inmult:       MULSU       [Y+i]   ;уможение знакового B на беззнак. C
94  D480                  [Y+i]       Acc     ;вернули на место!
95  A87E                  iLOOP       @@inmult                
96  A977                  jLOOP       @@norm
97  B8FF                  JMP         [--SP]
NormSiCo    endp


Очень неэффективная по времени штука - если бы добавить предварительный этап с двоичными сдвигами, можно было бы в несколько раз ускориться, но пока насущной необходимости не возникло, будем оптимизировать по занимаемой памяти...

Вот осциллограмма работы:


Инициализируем j=12, i=1, после чего мы находим выражение

3/2 - co2/2 - si2/2

Если величины отнормированы, то co2+si2 = 1, и мы должны получить единицу. В противном случае - получится число, на которое умножим и si, и co, чтобы они приблизились к единичной норме.

вычитание из аккумулятора половинки от квадрата числа - это ровно одна команда АЛУ, SQRSD2. Именно для того она и сделана - на удивление полезная вещь, иначе пришлось бы перекладывать из порожнего в пустое взад вперёд разные промежуточные результаты. А так - цикл из ДВУХ команд, одна из которых iLOOP - где ещё такое встретишь?

Мы видим, что на первой итерации мы "скармливаем" значение -1 (si), на второй - значение 0x368. Результат этого действа мы видим в самой конце осциллограммы, когда мы помещаем в регистр C значение UAC. Использовать Acc нельзя - там НАСЫЩЕННЫЙ результат 32767. А вот UAC даёт 16 бит "как есть", начисто игнорируя самый старший бит аккумулятора. И здесь мы получаем ровно то, что надо: BFF4 = 49140 = 1,4996337890625 в беззнаковом формате 1.15. Т.е норма наших co/si настолько мала, что в выражении
3/2 - co2/2 - si2/2

двух слагаемых практически не чувствуется.

Именно на это значение мы должны умножить co и si, причём обязательно нужно рассматривать co и si как знаковые (они могут принимать значения от -1 до 1), а вот общий множитель - беззнаковым, потому что иначе 3/2 будет интерпретировано как -1/2, и дело швах.

Этот кусок я не записал - первый раз там фигня пошла, с полностью беззнаковым умножением - и вышло больно. Вместо -1 получилось 65535, которое умножалось на 49140, и выходило переполнение. За 13 итераций оно умудрилось войти в какой-то странный установившийся режим. Потом, когда адреса подправил - всё заработало удивительно скучно. Значение "-1" так и осталось на месте, умножение его на величину чуть меньше 1,5 не даёт перескочить на -2. А вот 0x368 = 872 за 12 итераций доползло до 0x7FFF = 32767, причём здесь абсолютно принципиально было наличие "насыщения" при использовании Acc, иначе мы бы пришли к 32768, т.е от ≈1 перескочили бы в -1, последствия были бы катастрофическими.

Так что возвращаемся из процедуры и смотрим, что там дальше.

А там вычисление кватерниона поворота. Когда-то, за счёт самых укуренных команд, на это уходило всего 7 слов, но теперь, по мере того, как QuatCore всё сильнее "попсеет", нужно уже 13 слов:
46  CD63                      X       OneHalf
47  84D8                      ABS     [Y+k]
48  8C80                      DIV2    Acc
49  82C8                      ADD     [X+k]
4A  A101                      j       1
4B  B102                      JGE     @@skip
4C  A001                      i       1       ;выходит i=S
4D  CD78          @@skip:     X       QuatA
4E  C480                      [X+i]   Acc
4F  8CD0                      DIV2    [Y+1]
50  CC80                      [X+i^j] Acc         ;по сути, Y+(~S)
51  DD78                      Y       QuatA
52  F3B1                      CALL    NormSiCo    ;подготовили первые 2 компонента кватерниона


По адресу OneHalf в оперативной памяти лежит значение 0x4000 = 16384, т.е "половинка". Сначала мы находим выражение
(1+abs(co))/2.
Вместо одной команды ABSP1D2, сейчас мы идём более мелкими шажочками: сначала берём модуль, затем делим на два, и прибавляем одну вторую.

Затем, в зависимости от знака co, нужно загнать это значение либо в QuatA (скаляр, при co больше нуля), либо в QuatX. А второе значение, si/2 - в оставшуюся позицию. Мы используем флаг знака, который должен остаться ещё с команды MULSU (в нормировке). Раньше мы придумали адреса [Y+S] и [Y+~S], сейчас решили обойтись без них. А именно, если знак "+", мы "перепрыгнем" через присвоение i = 1, а к этому моменту у нас i=k=0. В противном случае оно выполнится. Т.е двумя командами мы по сути сделали "i=S". Можно было бы модифицировать выходной мультиплексор АЛУ, чтобы он мог подать бит знака на шину данных, тогда обошлись бы 1 командой. Запомним это, и пока делать не будем :)

Адресация [X+i] у нас уже есть. А чтобы обратить знак, у нас есть XOR, т.е [X+i^j], когда мы постановили j=1. Ну до чего же эта XOR'овская адресация полезной оказывается - сказал бы мне кто год назад - не поверил бы ни в жисть!



Мы видим исполнение всех этих команд. Пока что случай наиболее тривиальный, co=32767 так и превращается в 32767, а -1 так и остаётся -1. Затем мы вызываем нормировку, которая проходит совсем тривиально, оставив оба значения "как есть". Потратив лишнее слово, мы могли бы хоть здесь сократить число итераций, зная, что здесь разброс начальных значений существенно ниже, но мы ОЧЕНЬ жадные пока что...

Ну и остаётся последняя часть нахождения крена - его устранение из матрицы аффинного преобразования. По сути, умножение матрицы 2х2 на другую матрицу 2х2, которая генерится "на ходу" из значений co и si. Вот соответствующий листинг:

53  DD7A                      Y           QuatY
54  A001                      i           1   ;номер строки результата, и строки co/si
55  A201          @@i_loop:   k           1   ;номер столбца результата, и столбца AfTransf
56  A101          @@k_loop:   j           1   ;номер столбца co/si и строки AfTransf
57  8803                      ZAcc        RoundZero   ;обнулить до 1/2 мл. разр
58  8ADC          @@j_loop:   C           [Y+i^j] 
59  91EA                      FMPM        [Z+2j+k]
5A  A97E                      jLOOP       @@j_loop
5B  F980                      [SP+2i+k]   Acc
5C  AA7A                      kLOOP       @@k_loop
5D  A878                      iLOOP       @@i_loop


Три вложенных цикла. И то самое "кватернионное ядро", за которое этот процессор получил своё название. Адресация i^j заставляет косинусы лечь по главной диагонали, а синусы - по бокам. Формирование сигнала PM (plus-minus) из регистров i,j, Inv - правильно расставляет знаки множителей, чтобы косинусы были со знаком "плюс", как и "нижний" синус, а верхний - со знаком "минус". Аккумулятор на то и аккумулятор, чтобы "накапливать" в себе слагаемые без потери точности. ZAcc RoundZero - специальное "обнуление", когда в аккумулятор ложится 1/2 младшего разряда, благодаря чему значение на выходе оказывается округлённым до ближайшего целого.

Я очень долго проверял правильность знаков и адресов, скриншотил направо и налево, после чего долго сопоставлял это с листингом:










И да, ошибку допустил там, где победа была так близка! Ведь мы не можем умножать матрицу "на месте" - сначала результаты мы помещаем на стек, а потом надо со стека переписать на исходное место. И вот там я забыл, что в нынешней реализации запрещена пересылка из памяти в память одной командой, исключительно из жадности (это требует 2 декодеров и 2 формирователей эффективного адреса). Надо бы свой компилятор модернизировать, чтобы он предупреждал о нелепом сочетании DestAddr и SrcAddr, которые не сулят ничего хорошего...

Поправил, что делать:
5E  A003                      i           3
5F  8AF4          @@out_loop: C           [SP+i]  ;эх, грустно без полноценных 2 адресов. Может, сделать-таки?
60  E483                      [Z+i]       C       
61  A87E                      iLOOP       @@out_loop  

Из-за нашей жадности программа удлинилась ещё на 2 байта.

Ещё нужно не забыть обнулить Y- и Z-компоненты кватерниона, которые мы использовали как временные:
79  A201                      k           1
7A  D800          @@clean:    [Y+k]       0
7B  AA7F                      kLOOP       @@clean

возможно, не стоит с этим торопиться, раз уж регистр Y указывает именно туда, пущай ещё послужит в качестве локальных переменных.

И вот что выходит:

Всё в порядке, но результат немножко разочаровывает - по сути, мы нашли околонулевой крен, который не смог никоим образом повлиять на матрицу преобразования, она осталась неизменной.

В "картинке для привлечения внимания" (в начале поста) ситуация поинтереснее, когда нам подсунули картинку с мишенью, повёрнутой на 90 градусов, с дистанции 30 метров. Ровно те же исходные значения, как здесь. И конечные результаты также совпадают с точностью до бита.

Пока ещё не все варианты проверены - до сих пор у нас косинус получался положительным. Надо бы ещё посмотреть вариант "вверх тормашками", что вся наша машинерия с битом знака и формированием кватерниона работает корректно.


Впервые QuatCore построил кватернион!

Ещё интересная вещь - программа подросла настолько, что понадобилось 8 бит адресации ROM. И ВНЕЗАПНО QuatCore уменьшился в размерах до 436 ЛЭ, тогда как при 7 битах он был свыше 440 ЛЭ! Как именно это работает - не знаю. Знаю лишь, что каждые 8 LE (logic element) объединяются в LAB (Logic Array Block), а уже он через интерконнекторы соединяется с другими LAB и EAB (Embedded Array Block, проще говоря, память), и работать с 8 битами ему очень комфортно :)

Так что наверное хорошо, что не стал ещё изобретать велосипед в плане битности - положил 16 бит, и дело с концом.

В следующей части - нахождение масштаба.
Tags: ПЛИС, математика, программки, работа, странные девайсы
Subscribe

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • atan на ЧЕТЫРЁХ умножениях

    Мишка такой человек — ему обязательно надо, чтоб от всего была польза. Когда у него бывают лишние деньги, он идёт в магазин и покупает какую-нибудь…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 13 comments

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • atan на ЧЕТЫРЁХ умножениях

    Мишка такой человек — ему обязательно надо, чтоб от всего была польза. Когда у него бывают лишние деньги, он идёт в магазин и покупает какую-нибудь…