nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Нахождение крена на быстром QuatCore

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

Алгоритм описан здесь, его первое "реальное" исполнение на медленном QuatCore - здесь.

Пока изобретал ассемблер и вставлял туда самые странные команды (ABSP1D2 - взять модуль числа, прибавить 1, и поделить на два) - код получался на 48 слов. Когда в итоге эти команды логично "впихнуть" в железо не удалось, код увеличился до 52 слов. А теперь ещё и Hazard'ы появились, которые могут потребовать ещё NOPов повставлять здесь и там. Но и мы немножко поднаторели! И вместо дальнейшего увеличения объёма, удалось его сократить до 47 слов, причём с небольшой дополнительной функциональностью. Раньше эта процедура оставляла "мусор" в компонентах Y,Z кватерниона, и туда надо было заносить нули позже. Теперь у нас формируется корректный кватернион. Без этого и вовсе было бы 45 :)


Начнём с процедуры NormSiCo, для нормировки вектора на плоскости. Так она выглядела раньше:
;по адресу Y лежит два числа, двухмерный вектор
;нужно его отнормировать.
;по окончании работы: i=j=0
NormSiCo 	proc
			j		12 ;если "передавать" i как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций,
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 300 раз пока что...
@@norm:			i		1		;две компоненты
			ZAcc		ThreeHalf		;выставить 3/2
@@innorm:		SQRSD2		[Y+i]	;вычесть половину от квадрата 
			iLOOP		@@innorm
			;теперь на содержимое аккумулятора надо домножить Si и Co
			C		UAC
			i		1
@@inmult:		MULSU		[Y+i]	;уможение знакового B на беззнак. C
			[Y+i]		Acc		;вернули на место!
			iLOOP		@@inmult				
			jLOOP		@@norm
			JMP		[--SP]
NormSiCo	endp


Она занимает 12 слов, вроде неплохо. Но можно и лучше, причём не только в плане компактности, но и в плане быстродействия! По-моему, я думал одну и ту же процедуру применить как для нормировки двумерных векторов, так и для кватернионов, и поэтому "заложил" внутренний цикл. Может, когда-нибудь это и пригодится, но пока совершенно излишне. Цикл на 2 итерации, тело которого состоит из 1 команды - ЭТО АБСУРД. Возможно, когда я это писал, не был уверен, что смогу прибавить единичку к индексу "нахаляву". Но теперь таблица при мне, знаю что ЛЕГКО:

@@norm:	ZAcc	ThreeHalf		;выставить 3/2
	SQRSD2	[Y+1]
	SQRSD2	[Y+i]	;вычесть половину от квадрата 


Ах да, ещё нужно тогда потребовать, чтобы i=0 при входе в процедуру. Надо будет придумать Assert'ы, хотя бы для эмулятора, чтобы и такие вещи можно было отлавливать.

Мало того, что сократили код на одну строку, так ещё и ускорили его, избежав накладных расходов при прыжке.

Дальше, когда две компоненты домножаются на одно и то же число, в теле цикла уже 2 команды. Кажется, что лучше всё-таки "развернуть" цикл. Команд останется столько же, 4, а накладных расходов меньше:

	MULSU	[Y+i]	;уможение знакового B на беззнак. C
	[Y+i]	Acc		;вернули на место!
	MULSU	[Y+1]
	[Y+1]	Acc


Так-то оно так, но увы, Hazard'ы тут как тут. Ведь если сдвинуть правый столбец вниз (как оно реально исполняется), получаем следующее:
	С	[Y+i]
	MULSU	Acc
	[Y+i]	[Y+1]
	MULSU	Acc
	[Y+1]	@@norm


Память используется одновременно и на чтение, и на запись. Можно добавить NOP, но мы очень жадные - лучше оставить цикл :) Эти накладные расходы разносят по времени чтение и запись, уж пущай.

Приведём полный код процедуры:
;по адресу Y лежит два числа, двухмерный вектор
;нужно его отнормировать.
;необходимо, чтобы i=0. 
;после завершения процедуры, i=j=0, значения в Acc и C меняются
NormSiCo 	proc
		j	12 ;если "передавать" j как аргумент, то можно регулировать число итераций
;при первом вызове нужно 13 итераций, при втором хватило бы и 6 итераций,
;но мы пока пытаемся оптимизировать по памяти кода, а не по быстродействию
;по быстродействию запас в 300 раз пока что...
@@norm:		ZAcc	ThreeHalf		;выставить 3/2
		SQRSD2	[Y+1]
		SQRSD2	[Y+i]	;вычесть половину от квадрата 
	;теперь на содержимое аккумулятора надо домножить Si и Co
		i	1
		C	UAC
@@inmult:	MULSU	[Y+i]	;уможение знакового B на беззнак. C
		[Y+i]	Acc		;вернули на место!
		iLOOP	@@inmult		
		jLOOP	@@norm
		JMP	[--SP]
NormSiCo	endp


Ладно, одно слово выцарапали - уже радость.

Теперь рассмотрим процедуру FindRoll. Сколько-нибудь заметно уменьшить её размер удалось, отказавшись от умножения матриц "на месте". Зачем-то я результат умножения сохранял в стек, а потом отдельно переписывал из стека на исходное место. Сейчас попросту выбрал другую область памяти для хранения исходной матрицы, аккурат на месте матрицы 6х6 для алгоритма сопровождения. Если уж выполняется алгоритм захвата, значит был срыв сопровождения, или его пока не наступило, но в любом случае там лежит "мусор", который не жалко и перезаписать :)

	;состояние регистров к этому моменту
	;X = AffineMat (константная матрица 3х4, чтобы посчитать преобр)
	;Y = Points2D
	;Z = Matrix (матрица 2х2 и вектор 1х2, пока не AfTransf)
	;i=j=k=0
	;Inv неизвестен
	;C, Acc - пофиг наверное
	FindRoll	proc
		;нам понадобятся [Z]+[Z+3] и [Z+1]-[Z+2]
				Y		QuatY 	;в этом кватернионе построим,используя Y/Z значения как временные
						;(потом они занулятся)
				i		1
				j		3
		@@sico:		Acc		[Z+i]
				Inv		i
				PM		[Z+i^j]
				[Y+i]		Acc
				iLOOP		@@sico
	
	;теперь наших друзей надо отнормировать, причём динамический диапазон очень велик
	;максимум 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 = Matrix (матрица 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 = Matrix
	;теперь помножаем две матрицы, одна в явном виде - Matrix (Z)
	;вторая - задана неявно, в виде co / si (в QuatY)
	;результат идет в AfTransf
	;здесь i=1, k неизвестен, j=0, Inv=0
				Y		QuatY
				X		AfTransf	;сюда результат складируем
				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
				[X+2i+k]	Acc
				kLOOP		@@k_loop
				iLOOP		@@i_loop

	;здесь у нас i=j=k=0
	;теперь у нас должна получиться матрица, включающая в себя масштаб и ракурс (ужатие вдоль некоторой оси), но не крен...
	;ещё кватернион подчистим - по Y,Z может лежать всякий мусор
				[Y+k]		0
				[Y+1]		0
	FindRoll	endp


Сначала находим сумму диагональных элементов и разность оставшихся. Здесь даже с "произвольной" адресацией шибко компактнее не вышло бы. Загрузить одно значение, прибавить второе, результат сохранить, снова загрузить, вычесть, сохранить - уже 6 команд, а у нас тут 7. Вот здесь цикл даже на 2 итерации имеет смысл. И действительно, по окончании этого кода у нас i=0, как и надо для процедуры NormSiCo.

Потом косинус торжественно берём по модулю, делим пополам и прибавляем 1/2. Нормально...

А вот дальше всё не совсем хорошо: маячит Hazard на память. Но его легко побороть, переместив j=1, вставив его между

[X+i]		Acc


и
DIV2		[Y+1]


и ещё можно немножко поменять местами строки, проставить между арифметическими операциями инициализацию регистров X/Y, чтобы не возникал Hazard по АЛУ. Он не требует NOP, но такт все равно пропускается - некрасиво!

А дальше идёт "умножение матриц", там всё чётко. И, наконец, мы "ручками" зануляем компоненты Y,Z кватерниона.

Запускаем транслятор. Он находит Hazard уже под самый конец:

[Y+k]		0
[Y+1]		0
FindRoll  endp
JMP             [--SP]


но не будем с ним бороться - в дальнейшем туда добавятся ещё несколько частей, пусть транслятор сам вставляет NOP.

В этот раз вся программа влезает в 127 слов (в прошлый раз уже превышала 128), что не может не радовать.

Глянем, выполнится ли оно как надо?

Синтез всего процессора на этот раз выдал Critical Warning: некоторые цепи могут работать на частоте только 24,6 МГц, не дотягивая до заданных 25 МГц. И симулятор действительно выдал "глюк", когда PC не с того не с сего переключилось с 0x7B сразу в 0x7F, что расположено вообще на пределами нашего кода. Там были сплошные нули, так что процессор дошёл до адреса 0, и всё началось сначала - инициализация стека и всего остального.

На 20 МГц всё выполняется как надо, вот результаты данного этапа:


Кватернион получился 32767 - 1i + 0j + 0k, т.е крен в 12 угловых секунд. Матрица осталась неизменной, не хватило разрядности, чтобы хоть как-то её изменить.

Тайминги - как наш мир в последнее время, реагирует на каждый чих. Ведь с прошлого раза поменялся только модуль QuatCoreCallTable, процедур уже стало 4, и поменялись их адреса, стали более крупными. Надо всё-таки научить транслятор более красиво распределять адреса, чтобы минимизировать количество ЛЭ, необходимых для QuatCoreCallTable. Боюсь только, что эвристики, хорошо работающие для 4 процедур, заработают уже не столь хорошо для большего количества. Но всяко, у меня уже будет отведено место в трансляторе, куда сажать эти эвристики, и дальше гораздо легче пойдёт.

Сейчас, к примеру, нам нужно на какие-нибудь из 16 команд CALL0..CALLF посадить следующие адреса:

0x0D=0000_1101
0x74=0111_0100
0x6C=0110_1100
0x65=0110_0101


Сейчас они "лежат" в командах CALL0..CALL3, т.е просто по порядку, и чтобы преобразовать номер команды (4 бита) в соответствующий адрес, уходит аж 3 ЛЭ. Интересно, почему не 4, возможно, инверсию он считает возможным сделать "нахаляву" где-нибудь во входах или выходах.

А можно вообще без логических элементов обойтись, только соединителями. Видно, что старший (7-й) разряд, также 1-й и 2-й вообще константы, ну это и verilog видит, здесь не проблема.
Далее, есть всего 4 независимых выхода, это 0-й, 3-й, 4-й и 5-й. А 6-й повторяет 5-й. Раз их всего 4, то можно просто каждый присоединить к своему входному биту. Прямо в наглую:
0000_1101 = 0011 = CALL3
0111_0100 = 1100 = CALLC
0110_1100 = 1010 = CALLA
0110_0101 = 1001 = CALL9


(выписываем только выделенные жирным разряды, и получается номер команды, которая вызывает данную процедуру)

Сейчас поиграемся, чего уж там.

Когда я ручками написал модуль:
module QuatCoreCallTable (input [7:0] SrcAddr, output [RomWidth-1:0] addr);
parameter RomWidth = 7;

assign addr[6] = SrcAddr[3];
assign addr[5] = SrcAddr[3];
assign addr[4] = SrcAddr[2];
assign addr[3] = SrcAddr[1];
assign addr[2] = 1'b1;
assign addr[1] = 1'b0;
assign addr[0] = SrcAddr[0];
endmodule


Он, разумеется, синтезировался в 0 ЛЭ, а в составе всего QuatCore дал уменьшение в 4 ЛЭ (494 ЛЭ вместо 498, при 32-битном аккумуляторе, 8-битной адресной шине на ROM и RAM) и, что самое важное, увеличение предельной частоты с 24,63 до 25,71 МГц!


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

Recent Posts from This Journal

  • Нахождение двух самых отдалённых точек

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

  • Слишком общительный счётчик

    Вчера я чуть поторопился отсинтезировать проект,параметры не поменял: RomWidth = 8 вместо 7, RamWidth = 9 вместо 8, и ещё EnableByteAccess=1, чтобы…

  • Балансируем конвейер QuatCore

    В пятницу у нас всё замечательно сработало на симуляции, первые 16 миллисекунд полёт нормальный. А вот прошить весь проект на ПЛИС и попробовать "в…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments