На удивление всё просто: эта процедура занимает 12 слов, или 24 байта. В сравнении с той версией, которую мы гоняли на эмуляторе, только переставили местами несколько индексных регистров, чтобы получилось немного меньше лишних инициализаций (выгодно цикл с малым числом итераций делать внешним!) и ложилось на текущий набор команд.
И сделали хоть некое подобие "вывода данных на экран", чтобы можно было сразу же посмотреть, что получилось, и если всё правильно - не лезть в дебри, как мы делали это несколько дней подряд...
Кстати, мы можем в квартусе задать формат данных - десятичные знаковые:
Да, в дебри можно не лезть - всё считает правильно. Правда, для этого пришлось немного перелопатить код первых двух процедур, поменять повсюду регистры X и Y между собой.
Вот листинг всей программы (на данный момент):
main proc
00 FD47 SP StackAdr
01 8AFC C [SP]
02 FD83 SP C
03 F3B0 CALL AffineAlgorithm
04 A205 k 5
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 F3B2 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 F3B1 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 F3B2 CALL SwapPoints
2B A87A @@skip: iLOOP @@loop
2C A979 jLOOP @@loop
2D A300 Inv 0
2E F3B1 CALL ShiftOrigin
2F CD1E X Fx3
30 F3B2 CALL SwapPoints
SortCCW endp
Associate4Points endp
Compute4PointAffine proc
31 CD33 X AffineMat
32 ED76 Z AfTransf
33 A201 k 1 ;номер столбца результата (и столбца Points2D)
34 A102 @@k_loop: j 2 ;номер строки результата (и строки AffineMat)
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
FindRoll endp
FindScale proc
FindScale endp
FindVector proc
FindVector endp
3D B8FF JMP [--SP]
AffineAlgorithm endp
SwapPoints proc
3E A201 k 1
3F 80EA @@swap: Acc [Z+2j+k]
40 8AC9 C [X+2i+k]
41 EA83 [Z+2j+k] C
42 C980 [X+2i+k] Acc
43 AA7C kLOOP @@swap
44 B8FF JMP [--SP]
SwapPoints endp
ShiftOrigin proc
45 A201 k 1
46 A002 @@k_loop: i 2 ;как всегда, чтобы Y и X
47 80C9 @@i_loop: Acc [X+2i+k]
48 81D8 PM [Y+k]
49 C980 [X+2i+k] Acc
4A A87D iLOOP @@i_loop
4B AA7B kLOOP @@k_loop
4C B8FF JMP [--SP]
ShiftOrigin endp
Замена X на Y была связана с небольшой "неортогональностью" команд - можно обращаться по адресам [X+4j+i], и по адресам [Z+4j+i], а вот по адресу [Y+4j+i]-нельзя, вместо этого будет [Y+Tr[j]+i], где Tr[j]=j(j+1)/2. Ещё надо будет подумать о хорошей мнемонической записи. Сначала у меня вобще было Treug[], сейчас Tr, но это не очень хорошо - можно со следом матрицы спутать. Понятно, в мнемонике команды АССЕМБЛЕРА не должно быть следа матрицы, это ну никак не "атомарная" операция, но всё равно как-то неаккуратненько.
Само перемножение матриц - замечательная процедура, мы инициализируем три адреса - в [X] матрица 3х4, которая задаётся "заблаговременно" исходя из положения мишеней на объекте, в [Y] - матрица 4х2, составленная из наших 4 точек, а в [Z] - результирующая матрица 3х2, первые 2 строки которой представляют собой матрицу 2х2 аффинного преобразования, а третья - транспонированный вектор 1х2 аффинного преобразования.
В начале кода программы мы несколько поменяли эти строки:
MinusThreeHalf EQU 0 Two EQU 1 ThreeHalf EQU 2 RoundZero EQU 3
Т.к при проектировании АЛУ оказалось более удобным расположить эти константы именно в таком порядке.
И ещё поменяли местами два столбца матрицы 3х4, поскольку решили поменять местами две точки, расположив МБД в самом конце:
;заранее посчитанная матрица для нахождения коэф. аффинного преобразования ;МДД3-МДД1-МДД2-МБД AffineMat: AfMat00 Int16 4090 ;адр 33h AfMat01 Int16 10092 AfMat02 Int16 -14492 AfMat03 Int16 310 AfMat10 Int16 -13992 ;адр 37h AfMat11 Int16 8627 AfMat12 Int16 -2011 AfMat13 Int16 7376 AfMat20 Int16 1199 ;адр 3Bh AfMat21 Int16 -5752 AfMat22 Int16 -3495 AfMat23 Int16 -5807 ;адр 3Eh
Ещё и нижнюю строку помножили на -1, по-моему так удобнее получится в алгоритме сопровождения. К сожалению, в начальной формулировке алгоритма сопровождения мы так офигительно задали систему координат, что координата X всегда получалась отрицательной. Так что пусть и вектор параллельного переноса знак поменяет - и оно вроде бы устаканится...
После выполнения всех вычислений, в процедуре main выполняется следующий код:
k 5
@@loop: NULL [Z+k]
kLOOP @@loop
Мы просто "берём" 6 компонент результирующей матрицы, и "загоняем их в /dev/null". Это ничего не даёт, но по шине адреса и по шине данных мы компактно видим содержание интересующей нас памяти. Это, ясное дело, для проверки работы на симуляторе.
Для отработки "на железе", думаю, появится новый DestAddr, например 1-й, типа такого:
k 5
@@loop: UART0 [Z+k]
kLOOP @@loop
и 16-битное значение сразу будет отправляться по UART, а если не окончилась предыдущая передача - программа приостановится в этом месте. Совершенно не хочу UART в том виде, как их делают в микроконтроллерах, когда "на ровном месте" напридумывали десяток конфигурационных регистров - ладно скорость, чётность и стоповые биты, так ещё организация очередей на вход и выход, с прерываниями, когда "почти окончилось" и "почти заполнился", чуть ли не "прямой доступ к памяти".
Здесь это всё совершенно незачем. Выдача 16 бит на выход должна быть самым простым действием в коде!
У нас уже накопилось 77 слов кода, это уже 7-битная адресация. Размер QuatCore поэтому вырос до 453 ЛЭ (пока без эвристик по CallTable). Впервые посмотрели на команду FMA (Fused Multiply-Add) и ZAcc (загрузить одну из 4 "длинных" констант в аккумулятор) - всё хорошо.
Дальше по списку - нахождение крена, его превращение в кватернион поворота и "избавление" от крена в матрице аффинного преобразования.
UPD. Процедура умножения матриц очень компактная, но отжирает на удивление много времени - мы уже потратили на всё про всё 455 мкс при тактовой частоте 4 МГц. Но допустимо потратить 200 мс - ещё есть запас :) И можно ускориться в 2-4 раза, если "припрёт", а если совсем припрёт - ещё и попробовать реализовать уравновешенный четверичный умножитель!
