На удивление всё просто: эта процедура занимает 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 раза, если "припрёт", а если совсем припрёт - ещё и попробовать реализовать уравновешенный четверичный умножитель!