nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

QuatCore: вычисление матрицы аффинного преобразования

Исполняем очередную часть аффинного алгоритма.

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