nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Аффинный алгоритм (алг. захвата) написан!

Часть 1 - нахождение наиболее отдалённой точки
Часть 2 - сортировка против часовой стрелки
Комментарий об операторе упорядочивания
Часть 3 - нахождение матрицы преобразования
Часть 4 - нахождение крена
Математическая интерлюдия
Часть 5 - нахождение масштаба
Финал!

Написан и пройден по шагам от начала до конца (на эмуляторе) - работает!

Занимает 166 слов кода (слово-16 бит), в поле Src используется 56 различных значений из 256 возможных (из них 31 это Immediate-значения, они же литералы, оставшиеся 25 - другие адреса), а в поле Dest - 48 различных значений из 256 возможных. То есть, в нынешнем виде размер слова мог бы быть не 16 бит, а 12 бит :) Другое дело, изобразить 512-байтовый блок памяти в ПЛИС в виде 341 слова по 12 бит каждое - это ещё умудриться нужно... Так что, если так и останутся незадействованные биты - я лучше контрольные суммы туда впихну, пущай проверяет, вдруг там какая тяжелая заряженная частица шмальнула.

Но и другое соображение есть - когда адресов чуть больше, чем надо, можно попытаться сколько-нибудь логично их рассредоточить, чтобы отдельные биты напрямую управляли модулями, не требуя сложного декодирования.



На входе этого алгоритма - координаты 4 точек на фотоприёмной матрице:
Fx0		Int16 53	;адр 30
Fy0		Int16 -7

Fx1		Int16 480	;адр 32
Fy1		Int16 -30

Fx2		Int16 -539	;адр 34
Fy2		Int16 -302

Fx3		Int16 400	;адр 36
Fy3		Int16 -997


Это соответствует такой картинке:


Матрица 1024х1024, но мы измеряем координаты с субпиксельной точностью и выдаём "домноженные на 64", чтобы использовать 16-битные значения "на полный размах". Ноль соответствует середине матрице (оптической оси), ось X - "вправо", ось Y - "вверх". (вообще, лучше было бы озаглавить их Y,Z, чтобы "билось" с последующим изложением)

Выходом является кватернион взаимной ориентации (как наш приборчик с объективом и фотоприёмной матрицей расположен относительно МКС или другого корабля, на котором установлены "катафоты") Quat (его компоненты QuatA, QuatX, QuatY, QuatZ), а также вектор параллельного переноса, на который надо передвинуть нашу систему координат, чтобы она совместилась с системой координат "катафотов". Именно в этом векторе "закодирована" дальность и поперечные сдвиги, которые можно пересчитать в тангаж и курс, но я бы не спешил этого делать...

Если открыть скриншот и посмотреть содержимое памяти, мы и увидим звёздочки напротив Quat, а также полей Exp ("экспонента"), Tx, Ty, Tz. Звёздочки означают, что эти значения были изменены от последней точки останова. Сейчас здесь содержатся посчитанные значения. Кватернион в этот раз просто единичный, а вектор в "метрах" находится так:
Tx_real = 2Exp-1 * Tx / 32768 = 299,82 метра,
Ty_real = 2Exp-2 * Ty / 32768 = 0,1 метра,
Tz_real = 2Exp-2 * Tz / 32768 = -0,008 метра


"Модельное" положение - (300;0;0), но наш приборчик сдвинут по оси Y на 8,5 сантиметров влево, поэтому положение мишени мы увидели на 10 см правее. Это даёт ошибку по углу в 0,0028°, при допустимом значении в 0,1°. Допустимая точность измерения дальности: 5 метров, мы же ошиблись на 18 сантиметров, причём главный источник ошибки - исходные данные для этого алгоритма, а не сам алгоритм. В общем, можно попытаться ещё немного "вылизать", но пока сойдёт.


Процедура AffineAlgorithm условно разбита на 5 частей, причём первая - ещё на 2 половинки:

AffineAlgorithm proc
  Associate4Points proc
    FindMDD3 proc
      ...
    FindMDD3 endp
    SortCCW proc
      ...
    SortCCW endp
  Associate4Points endp
  FindRoll proc
    ...
  FindRoll endp
  FindScale proc
    ...
  FindScale endp
  FindVector proc
    ...
  FindVector endp
     JMP   [--SP]
AffineAlgorithm endp


Напомним, что сами эти строки proc / endp не генерируют никакого кода, а лишь разделяют локальные метки и сами служат метками, а также отображаются в "откомпилированном" листинге, чтобы хоть как-то ориентироваться в простыне кода.

Пока мы показали одну "реальную" команду, JMP [--SP].

Символ [--SP] транслируется в число 231 (E7h). Когда на 8-битной шине SRC (source) появляется такой адрес, все его игнорируют, кроме модуля MEM - он выдаёт на 16-битную шину данных значение, взятое из памяти из адреса SP-1, а к следующему такту обновляет значение указателя стека.

Символ JMP транслируется в число 180 (B4h), на которое откликается модуль PC (Program Counter). Он осуществляет безусловный переход по абсолютному адресу, который пришел на 16-битную шину данных.

Всё вместе это - команда возврата из процедуры.

Теперь давайте рассмотрим код "процедуры" FindMDD3. Эта процедура находит, какая из 4 точек наиболее отдалена от остальных, и меняет эту точку местами с точкой X0, Y0, чтобы по нулевому индексу располагалась именно МДД3 (мишень дальней дистанции - 3):

        ;состояние регистров неизвестно (за искл. PC и SP)
        FindMDD3    proc
                    X           Points2D
                    Y           Points2D    ;пригодится при замене и сортировке                 
                    [SP+1]      0   ;максимальная отдалённость
        ;а в [SP+2] - индекс точки с макс. отдаленностью. Её иниц. не надо-заведомо на одной из итераций запишется

                    j           3   ;от 0 до 3, т.е 4 наши точки
        @@j_loop:   i           3   ;также от 0 до 3, чтобы все расстояния просуммировать
                    [SP]        0  
        @@i_loop:   k           1   ;от 0 до 1, т.е значения X и Y
        ;а теперь непосредственно прибавляем к акк. ([X+2i+k]-[X+2j+k])^2
        @@k_loop:   Acc         [X+2i+k]    ;загрузили одно значение
                    SUB         [X+2j+k]    ;вычитаем второе
                    SQR         Acc         ;возводим в квадрат
                    ADD         [SP]        ;прибавляем к пред. значению
                    [SP]        Acc
                    kLOOP       @@k_loop        ;теперь то же самое для второй координаты
                    iLOOP       @@i_loop
        ;хорошо, нашли сумму расстояний от точки j до всех остальных
        ;она лежит в Acc и в [SP]
        ;нужно сравнить с самым большим, и если больше - заменить
        ;самое большое значение мы храним в [SP+1],
        ;а его индекс - в [SP+2]
                    SUB         [SP+1]  ;можно и "пожертвовать" значением в Acc,
        ;т.к в [SP] лежит точно такое же!
                    JL          @@skip
        ;дистанция оказалась больше - надо обновить максимум и его индекс
                    [SP+1]      [SP]    ;двухпортовая память-это зашибись!
                    [SP+2]      j
        @@skip:     jLOOP       @@j_loop
        ;по окончании этих циклов, у нас в [SP+2] лежит индекс точки, которая является МДД3
        ;осталось эту точку поменять местами с точкой под индексом 0...
                    i           [SP+2]
                    k           1
        @@swap:     Acc         [Y+k]
                    [Y+k]       [X+2i+k]
                    [X+2i+k]    Acc     ;ага, поменяли местами одну из координат
                    kLOOP       @@swap  ;а теперь и обе
            
        ;на этом первая часть марлезонского балета закончена.
        FindMDD3    endp


Выглядит громоздко, но компилируется в 25 слов (50 байт). Первым делом мы заносим в адресные регистры X,Y адрес массива точек с фотоприёмной матрицы, который мы изобразили в начале поста:

X           Points2D
Y           Points2D    ;пригодится при замене и сортировке


Далее, у нас будет 3 "локальных переменных", лежащих по адресам SP, SP+1 и SP+2. Мы после долгих метаний решили, что стек может с тем же успехом расти "вверх", и что занесение в стек выполняется через "адрес" [SP++], а извлечение из него: [--SP]. Таким образом, обращаясь к [SP], мы не потревожим адрес возврата - он лежит чуть ниже. Так можно делать, если мы не вызываем других процедур (иначе наши переменные затрутся новыми адресами возврата) - так оно и будет.

Мы собираемся посчитать сумму квадратов дальностей от каждой из точек до остальных точек. Где она будет максимальна - ту точку мы и "обзовём" МДД3. Не будем хранить все суммы квадратов дальностей - только максимальное значение в "переменной" [SP+1], и индекс точки, которая дала максимальное значение - в [SP+2]. Текущее значение мы будем накапливать в [SP].

Поэтому следующей строчкой мы обнуляем максимальную "отдалённость":
[SP+1]      0   ;максимальная отдалённость


далее начинаем цикл по переменной j (номер точки, дальность от которой до всех остальных мы сейчас вычисляем):

j           3

присвоили значение 3, всё просто.

Далее мы видим метку внешнего цикла, и первая команда внутри него: начать следующий цикл, по переменной i.
@@j_loop:   i           3   ;также от 0 до 3, чтобы все расстояния просуммировать


Эта переменная указывает на точку, расстояние до которой считаем в данный момент. По-хорошему, не стоило бы считать дальность точки до себя самой (проверить условие i!=j), но оно всё равно получится нулевым - почему бы не посчитать :) Так-то понятно, что мы одни и те же значения находим по 2 раза, сначала считая от i до j, потом от j до i, и с точки зрения экономичного счёта стоило бы найти 6 взаимных расстояний (0-1, 0-2, 0-3, 1-2, 1-3, 2-3) и затем сложить их в разных комбинациях, но это приводит к очень громоздкому, практически "ручному" коду и большому количеству переменных. Так что пока делаем "оптимизацию по размеру программы", и считаем "в лоб".

Две "собаки" в начале метки говорят о том, что она "локальная" - действует только в пределах текущей процедуры. В действительности компилятор сразу же, найдя такую метку, "расширяет её" до FindMDD3::j_loop, и уже её вносит в таблицу меток, проверяя, что там нет повторов.

Далее, обнуляем дальность от точки с индексом j до всех остальных:
[SP]        0  


и начинаем ЫШШО один цикл, по k от 0 до 1. k=0 соответствует координате X, k=1 - координате Y. Если уж мы ввели 3 индексных регистра и 3 команды цикла (например для перемножения матриц), то почему бы ими не воспользоваться:

 @@i_loop:   k           1   ;от 0 до 1, т.е значения X и Y


И вот теперь, наконец-то, начинаются вычисления. Мы находим выражение (X[i][k] - X[j][k])2, где первый индекс - номер точки, второй - номер координаты (X или Y):

Acc         [X+2i+k]    ;загрузили одно значение
SUB         [X+2j+k]    ;вычитаем второе
SQR         Acc         ;возводим в квадрат


Вроде всё понятно. Напомним, каждая команда по-прежнему занимает 16 бит, 8 бит на адрес Dest (левый столбец), ещё 8 бит - на адрес Src (правый столбец). Выражение [X+2i+k] выглядит весьма "мощно", но это просто название одного из 256 адресов :) Всего на разные способы адресации зарезервировано 32 адреса, но пока что во всей программе понадобилось только 16 из них для Src, и 13 для Dest.

Теперь прибавим это выражение к нашей сумме, и поместим её назад в "локальную переменную":
ADD         [SP]        ;прибавляем к пред. значению
[SP]        Acc


И завершаем 2 цикла из 3:
kLOOP       @@k_loop
iLOOP       @@i_loop


Эти "команды" работают примерно также, как loop в x86 ассемблере: проверяет, равна ли нулю переменная (i,j или k). Если да, ничего не делаем, переходя к следующей строке. В противном случае уменьшаем переменную на единичку и осуществляем прыжок по относительному адресу, взятому с шины данных. Правая часть, @@k_loop или @@i_loop на этапе компиляции превращается в этот самый относительный адрес. Мы имеем -5 для @@k_loop и -7 для @@i_loop. Затем число -5 превращается в 123, а число -7 - в 121, именно они заносятся в поле SRC данных команд. Это Immediate-значения, они же литералы. Значения от 0 до 63 интерпретируются "как есть", а значения 64..127 проходят "расширение знака", превращаясь в -64 .. -1. Пока что этого хватает.

Выйдя из двух внутренних циклов, мы получаем по адресу [SP] "отдалённость" точки с индексом j до всех остальных, а по [SP+1] - максимальную "отдалённость". Причём, текущая "отдалённость" уже лежит в Acc, что весьма удобно. Вычтем одно из другого:
SUB         [SP+1]


Если результат оказывается отрицательным, значит, у нас уже был более подходящий "кандидат", и мы пропускаем следующие строки:
JL          @@skip

В эмуляторе (см. скриншот в начале поста, справа снизу) есть флаги Sign (знак) и OFLO (overflow, переполнение), но в действительности они, скорее всего, не нужны - мы просто будем проверять соответствующие биты аккумулятора.

Вот две команды, которые обновляют значение максимальной "отдалённости" и индекса самой дальней точки:
[SP+1]      [SP]    ;двухпортовая память-это зашибись!
[SP+2]      j


В большинстве ассемблеров команды "память-память" запрещены, а если и есть, то довольно "экзотические", типа MOVS в x86. Когда память "внешняя", в этом видна логика - в каждый момент времени можно либо читать из памяти, либо записывать в неё, так что подобные команды требуют внушительного микрокода.

Но внутренняя память ПЛИС - как минимум двухпортовая (даже в нашем весьма потрёпанном жизнью 5576ХС4Т, аналоге Flex10k), то есть в каждый момент можно осуществлять чтение по одному адресу и запись по другому (могут возникнуть интересные моменты, если адреса совпадают), и переписать из одного адреса в другой не составляет труда. По сути, выход памяти на "чтение" коммутируется на нашу шину данных, и к ней же подключается вход на "запись". Мелочь, а приятно!

Ну и оканчиваем внешний цикл:
@@skip:     jLOOP       @@j_loop


Интересный момент, по выходу из всех циклов мы получим i=j=k=0, что довольно приятно. Огромнейшего "зоопарка" режимов адресации удаётся избежать благодаря тому, что индексные регистры "естественным образом" возвращаются к нулю, и можно использовать более сложные режимы, где половина слагаемых уйдёт.

На этом этапе мы уже знаем, какая точка максимально отдалена, её индекс хранится в [SP+2]. Осталось поменять её местами с "нулевой" точкой:
             i           [SP+2]
             k           1
 @@swap:     Acc         [Y+k]
             [Y+k]       [X+2i+k]
             [X+2i+k]    Acc     ;ага, поменяли местами одну из координат
             kLOOP       @@swap  ;а теперь и обе


Как ни обидно, однако команду XCHG в таком процессоре никак не сделать: шина данных всего одна! Так что приходится использовать временную переменную, как обычно, в 3 строки. Так что даже здесь мы ввели цикл, сэкономив аж одно слово кода и в очередной раз избежав команд с прямой адресацией к памяти. Сейчас мы обращаемся к памяти только через регистры - X,Y,Z,SP, к которым можно прибавлять индексные регистры. Возможно, и прямая адресация пригодилась бы, но пока во всём аффинном алгоритме мы практически обошлись без неё (её введение сэкономило бы буквально 2 слова кода) .

Посмотрим, что происходит с регистрами и памятью после выполнения этой "процедуры":


Регистры X,Y указывают на начало массива точек - всё верно. Z так и не был инициализирован, что тоже логично. (это фича эмулятора - запоминать, какие регистры и адреса памяти не инициализированы, чтобы грязно выругаться, если мы начнём брать оттуда данные!). SP=63 - мы видим, что в первой строчке кода ему присвоили значение 62 (очень удобное, т.к оно ещё попадает в значения -64..+63, а вот дальше, когда стек растёт, мы сможем обратиться к нему только через [SP], [SP+1] и пр. ). Но по адресу 62 был записан адрес возврата в основную процедуру, адрес 2. И теперь мы возились с адресами 63, 64, 65, в них видны "следы нашей жизнедеятельности".

i=3 - это индекс точки, которая и оказалась МДД3. j=k=0, поскольку они использовались только в циклах и занулились по завершении. Регистр Inv мы пока не трогали, ещё не пришло его время.

В Acc осталось последнее "временное" значение при перемещении точек 0 и 3. Регистры B,C на самом деле недоступны через шину данных - это входные регистры АЛУ. Для сложений, вычитаний, взятия модуля, максимума, деления на 2 и ещё нескольких операций используется только регистр B, а для умножений, в т.ч с накоплением - ещё и регистр C. Значение регистра B после операции вообще бессмысленно, но в эмуляторе мы "вывели его наружу" и заставили хранить исходное значение, чтобы было удобнее отлаживать математические операции, не вычисляя в уме, куда именно указывает очередной [X+2i+k]. А вот регистр C после долгих раздумий было решено сделать регистром циклического сдвига. Когда осуществляется умножение, на каждом такте он сдвигается влево, и используется только крайний левый бит. Но по окончании работы, он возвращается в исходное состояние, что может очень пригодиться. Скажем, нужно много значений умножить на общий множитель - тогда мы его поместим в C, и он оттуда никуда не денется.

И наконец, мы видим, какие ячейки памяти поменялись, и как именно. Точки 0 и 3 поменялись местами - всё верно. Ведь именно точка, расположенная справа снизу на картинке, является наиболее отдалённой. Первоначально она была последней в списке, поскольку обнаружение делалось "сверху вниз, слева направо", но теперь мы поместили её первой, поскольку это удобно для последующей обработки.

Следующий этап - расположить оставшиеся 3 точки (с индексами 1..3) против часовой стрелки...


Мы пока объяснили работу 1-й части алгоритма из 6, а именно 26 слов из 166. Продолжение следует!
Tags: ПЛИС, кватернионы-это просто (том 1), программки, работа, странные девайсы
Subscribe

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

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

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

    Вчера я чуть поторопился отсинтезировать проект,параметры не поменял: 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 

  • 3 comments