nabbla (nabbla1) wrote,
nabbla
nabbla1

Category:

Аффинный алгоритм+инф обмен, УСПЕХ

Когда первый раз увидел "целевую информацию", очень удивился значениям 0x0002, 0x0002, 0x0001, 0x01B7, 0x002A и 0xFE7E на месте ковариационной матрицы шума измерений. Ну, я знал, что ковариационной матрицы там пока нет, это пространство используется для "промежуточных вычислений", но мне казалось, там должна была лежать матрица аффинного преобразования, выражающая, как из координат мишеней "в метрах" получить их же координаты "в пикселях", чтобы они максимально соответствовали тому, что мы "увидели" на текущем кадре. Последние 4 значения соответствовали, а вот первые две "двойки" - бред какой-то...

Наконец, перечитав свои старые посты, разобрался, в чём же тут дело!
[Spoiler (click to open)]
Оказывается, первые два значения я "затираю" в процедуре нахождения масштаба. Раньше чебышевскую и манхэттенскую метрики я укладывал на стек ([SP] и [SP+1]), но решил [SP+1] не трогать, т.к там лежит "количество точек", которое ещё понадобится чуть позже.

Да, двоечка и там, и там - похожи на правду.


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


Мне аж показалось, прибор меня материт как может! Там, где должны быть результаты (вектор и кватернион) - сплошные нули, и какие-то совершенно непонятные "промежуточные данные"...


Опять закомментировал в программе прыжок в начало работы (чтобы она могла обрабатывать "кадр" за "кадром"), заменив бесконечным циклом - и запустил на симуляции.

Как ни странно, за 500 мкс моделирования процессор так и не добрался до бесконечного цикла и продолжал что-то "перемалывать", хотя вся работа должна завершиться примерно за 200 мкс!

Стал искать, где же он "отклонился от курса", обнаружил, что он за каким-то лешим полез в алгоритм ближней дистанции, хотя 4 обнаруженных точки должны были направить его в дальнюю дистанцию. С удивлением обнаружил, что уже в самом начале работы, когда вызывается процедура IsZero ("костыль", заменяющий собой флаг нуля в АЛУ), переходит он вовсе не туда, а на процедуру ShiftOrigin:



Вот фрагмент, откуда вызывается процедура (листинг кода):
00B  80C0          Acc     [X+1]
00C  8305          SUB     4
00D  F3BA          CALL    IsZero  ;проверяем RR_Count == 4 (что означает дальнюю дистанцию)
00E  B806          JL      FindMDD3    ;переходим на алгоритм захвата дальней дистанции


Как раз исполнилась часть SrcAddr команды по адресу 0x00D. Но прыжок куда-то совсем не туда:
ShiftOrigin proc
112  A204                  k           1
113  A000  @@k_loop:       i           2   ;как всегда, чтобы Y и X
114  80E8  @@i_loop:       Acc         [Z+k]
115  83C9                  SUB         [X+2i+k]
116  C980                  [X+2i+k]    Acc
117  A851                  iLOOP       @@i_loop
118  AA52                  kLOOP       @@k_loop
119  B0FF                  JMP         [--SP]
ShiftOrigin endp
IsZero proc
11A  8480      ABS Acc
11B  8336      SUB 1
11C  80C0      Acc [X+1]
11D  B0FF      JMP [--SP]
IsZero endp


Причём как забавно - аккурат в начало процедуры прыгнул, только вот НЕ ТОЙ.

Сразу возникли сомнения в таблице вызове процедур, QuatCoreCallTable.v. Она каждый раз формируется заново при компиляции программы, и сейчас выглядит так:

//адреса для вызова процедур, "вшитые" в модуль QuatCorePC
module QuatCoreCallTable (input [7:0] SrcAddr, input NMI, input [nmiWidth-1:0] nmiN, output [RomWidth-1:0] addr);
parameter RomWidth = 9;
parameter nmiWidth = 2;
wire [8:0] Caddr;
wire [8:0] Iaddr;
	assign Caddr[8]=SrcAddr[3];
	assign Caddr[7]=SrcAddr[2];
	assign Caddr[6]=Caddr[7];
	assign Caddr[5]=Caddr[7];
	assign Caddr[4]=SrcAddr[1];
	wire [3:0] index = SrcAddr[3:0];
	assign Caddr[3]=
		(index == 4'h4)? 1'b1:
		(index == 4'hA)? 1'b0:
		(index == 4'h8)? 1'b0:
		(index == 4'hA)? 1'b0:
			1'b0;
	assign Caddr[2]=
		(index == 4'h4)? 1'b0:
		(index == 4'hA)? 1'b0:
		(index == 4'h8)? 1'b0:
		(index == 4'hA)? 1'b0:
			1'b0;
	assign Caddr[1]=
		(index == 4'h4)? 1'b0:
		(index == 4'hA)? 1'b1:
		(index == 4'h8)? 1'b0:
		(index == 4'hA)? 1'b1:
			1'b0;
	assign Caddr[0]=
		(index == 4'h4)? 1'b1:
		(index == 4'hA)? 1'b0:
		(index == 4'h8)? 1'b0:
		(index == 4'hA)? 1'b0:
			1'b0;
//Адреса процедур:
// FindCentralPoint = 00E9(поле Src = B4) 
// IsZero           = 0112(поле Src = BA) 
// NormSiCo         = 0100(поле Src = B8) 
// ShiftOrigin      = 0112(поле Src = BA) 
// SwapPoints       = 0100(поле Src = B8) 
	assign Iaddr=
		(nmiN == 2'h0)? 9'h0:
		(nmiN == 2'h1)? 9'h0:
		(nmiN == 2'h2)? 9'h0:
				9'h0;
assign addr = NMI? Iaddr : Caddr;
endmodule


И тут прямо в комментариях что-то неладное: IsZero указывает на тот же адрес, что и ShiftOrigin, а NormSiCo - туда же, куда и SwapPoints. Неудивительно, что ничего не работает, с такой-то чехардой!!!

В общем, компилятор "удружил", первый раз такая ошибка возникла. Стал смотреть, в чём же там дело. Ему дали 5 адресов:

0x0E9 = 0 1110 1001 (FindCentral)
0x11A = 1 0001 1010 (IsZero)
0x107 = 1 0000 0111 (NormSiCo)
0x112 = 1 0001 0010 (ShiftOrigin)
0x100 = 1 0000 0000 (SwapPoints)


Есть 16 доступных команд для вызова процедур, от 0xB0 до 0xBF, и нужно как-то эти 5 процедур распихать по 16 доступным местам и сформировать код, который из 4 младших бит кода команды (SrcAddr[3:0]) делать адрес процедуры, куда надо обратиться. Желательно, чтобы код был максимально простым.

Компилятор действовал методично, слева направо. Прочитал первый "столбец", т.е старший бит этих 5 процедур, "01111". Понял, что это не "всегда ноль" и не "всегда единица", и ранее ему такая последовательность не попадалась. Решил: а давайте SrcAddr[3] будет совпадать с этим битом. Ничему не противоречит - так и будет.

То есть, для вызова процедуры FindCentral будет использоваться команда с младшими 4 битами 0xxx (x - пока не определённые биты), для всех остальных - 1xxx. С помощью оставшихся 3 битов можно записать 8 различных значений, а процедур всего 4 - УКЛАДЫВАЕМСЯ!

Далее, компилятор читает второй "столбец", "10000". Это также не "все нули" и не "все единицы", и ранее такого не было, и компилятор по своей дурости решает сопоставить этот столбец с SrcAddr[2].

Формально он прав: теперь процедура FindCentral имеет номер 01xx, остальные процедуры - 10xx, тогда как 00xx и 11xx не используются вовсе. По-прежнему для представления 4 процедур хватает места, т.к с помощью 2 бит как раз можно все 4 и обозначить.

Затем компилятор видит несколько повторяющихся столбцов "10000" и громко радуется - их все просто можно "соединить вместе", не расходуя ЛЭ.

Далее идёт столбец "01010" - такого ещё не было, и его можно сопоставить SrcAddr[1], это также ничему не противоречит. Получаем такую "простыню":

000x - не используется
001x - не используется
010x - FindCentral
011x - не используется
100x - SwapPoints, NormSiCo
101x - IsZero, ShiftOrigin
110x - не используется
111x - не используется


И вот этим компилятор загнал себя в угол! Ни один последующий столбец ему уже не удаётся сопоставить с SrcAddr[0], т.к либо не "разделятся" SwapPoints и NormSiCo, либо IsZero и ShiftOrigin.

Как результат, по каждому следующему столбцу компилятор писал: "будет выражено через ЛЭ", что само по себе не криминал.

А дальше у меня была обработка ситуации, когда не все биты SrcAddr[] сопоставлены битам адреса, но там разбирался мега-халявный вариант, когда просто процедур настолько мало, или так повезло что столбцы повторяются, а многие просто всегда нули или всегда единицы, и лишние биты попросту оказались не нужны! Там единственная проблема была, что каждой процедуре назначалась своя группа. Сначала группа ровно одна, под номером 0, затем, с сопоставлением первого бита - групп становилось две, собственно "0xxx" и "1xxx". Затем 4, "00xx", "01xx", "10xx" и "11xx", и так далее. Если сопоставлялись все 4 бита - групп становилось 16, и это фактически и был номер команды, которой данная процедура должна вызываться.

Поэтому всё, что оставалось - умножить "номер группы" на 2 в степени "количества незадействованных бит".

Ровно так и было сделано сейчас, с печальными последствиями. Процедуры SwapPoints и NormSiCo сидели в одной группе, четвёртой, а стали сидеть в восьмой. И та же история с IsZero и ShiftOrigin - были в пятой, стали в десятой.

Забавно, как до сих пор такой ситуации не происходило. Даже неделю назад всё нормально было, РОВНО ОДНА ДОПОЛНИТЕЛЬНАЯ СТРОЧКА КОДА СЛОМАЛА СПИНУ ВЕРБЛЮДУ! Как только SwapPoints стал лежать не по 0x0FF, а по 0x100 - и возник "разбаланс".

Собственно, всё необходимое уже есть, чтобы сделать нормально. У меня есть массив GroupIndex, длина которого равна количеству процедур. В нашем случае, в нём лежало {2,5,4,5,4}, т.е номер группы для каждой процедуры. И второй массив, GroupsPop, длиной всегда 16, контролировал, сколько элементов лежит в каждой группе. Сейчас это было {0,0,0,0, 1,0,0,0, 2,0,2,0, 0,0,0,0}, его уже заранее приготовили к "удвоению" групп, и по умолчанию он все количества перенёс в чётные номера.

Вроде бы исправил, попробуем ещё разок откомпилить. Смотрим QuatCoreCallTable.v:

//адреса для вызова процедур, "вшитые" в модуль QuatCorePC
module QuatCoreCallTable (input [7:0] SrcAddr, input NMI, input [nmiWidth-1:0] nmiN, output [RomWidth-1:0] addr);
parameter RomWidth = 9;
parameter nmiWidth = 2;
wire [8:0] Caddr;
wire [8:0] Iaddr;
	assign Caddr[8]=SrcAddr[3];
	assign Caddr[7]=SrcAddr[2];
	assign Caddr[6]=Caddr[7];
	assign Caddr[5]=Caddr[7];
	assign Caddr[4]=SrcAddr[1];
	wire [3:0] index = SrcAddr[3:0];
	assign Caddr[3]=
		(index == 4'h4)? 1'b1:
		(index == 4'hB)? 1'b1:
		(index == 4'h9)? 1'b0:
		(index == 4'hA)? 1'b0:
			1'b0;
	assign Caddr[2]=
		(index == 4'h4)? 1'b0:
		(index == 4'hB)? 1'b0:
		(index == 4'h9)? 1'b1:
		(index == 4'hA)? 1'b0:
			1'b0;
	assign Caddr[1]=
		(index == 4'h4)? 1'b0:
		(index == 4'hB)? 1'b1:
		(index == 4'h9)? 1'b1:
		(index == 4'hA)? 1'b1:
			1'b0;
	assign Caddr[0]=
		(index == 4'h4)? 1'b1:
		(index == 4'hB)? 1'b0:
		(index == 4'h9)? 1'b1:
		(index == 4'hA)? 1'b0:
			1'b0;
//Адреса процедур:
// FindCentralPoint = 00E9(поле Src = B4) 
// IsZero           = 011A(поле Src = BB) 
// NormSiCo         = 0107(поле Src = B9) 
// ShiftOrigin      = 0112(поле Src = BA) 
// SwapPoints       = 0100(поле Src = B8) 
	assign Iaddr=
		(nmiN == 2'h0)? 9'h0:
		(nmiN == 2'h1)? 9'h0:
		(nmiN == 2'h2)? 9'h0:
				9'h0;
assign addr = NMI? Iaddr : Caddr;
endmodule


Да, теперь всё корректно. Неоптимально, безусловно, но корректно :) Вообще, можно было бы компилятору ещё немножко мозгов подкинуть, чтобы он не по порядку обрабатывал эти разряды, а начинал с того, который наиболее результативно разбивает процедуры в две группы. Думаю, тут это уже сработало бы. А то и самый старший разряд почти "вхолостую" прошёл, а следующий за ним, являющийся инверсией от предыдущего - СОВСЕМ ВХОЛОСТУЮ. Сэкономили бы пару ЛЭ. Потому и не буду пока этого делать - задача далеко не приоритетная.

Ещё посмотрим на фрагмент листинга с вызовом этой процедуры:

00B  80C0          Acc     [X+1]
00C  8305          SUB     4
00D  F3BB          CALL    IsZero  ;проверяем RR_Count == 4 (что означает дальнюю дистанцию)
00E  B806          JL      FindMDD3    ;переходим на алгоритм захвата дальней дистанции


Ага, раньше CALL IsZero (который вообще-то является "красивой" записью [SP++] CALL(IsZero) ) превращался в F3 BA, теперь в F3 BB. Похоже на правду...

Что ж, ещё разок синтезируем - и прошиваем в макет. Не забыв убрать бесконечный цикл в конце.



Другое дело, никакой ругани, смайлики сплошные :)

Первый раз запросили целевую информацию - получили фактически нули. Затем послали "Синхронизация (с СД)" с нулевой отметкой времени и снова запросили целевую информацию. И затем ещё раз, только в этот раз слово данных 00 50, означающее, что прошло 80 раз по 100 мс. Смотрим ответы в двух последних случаях.

00 30, точнее 30 00 (вечно Endian мешается) - ответное слово, "всё в порядке".
3C 3C - заголовок массива "целевая информация".
01 00, точнее 00 01 - слово признаков (флагов), пока только один "зажжён", режим захвата.
Ещё раз 01 00, т.е 00 01 - метка времени, когда данные были обработаны. Да, на обработку ушло более 195 мкс, но меньше 390 мкс, всё правильно. Во второй раз 01 A0, т.е A0 01 - да, это взяли 7-битное значение 0x50, сделали эти 7 бит старшими (вышло 0xA000) и прибавили единичку, которая ушла на обработку. Всё верно.

00 00 - скорость сближения, её пока не считали
09 00, то есть 00 09 - общая экспонента для вектора. Означает дальность между 29-1 = 256 метров и 29 = 512 метров.
91 95, т.е 0x9591 = 38289 - мантисса для координаты X. В целом, дальность составляет 38289/32768 * 256 = 299,13 метров.
31 00, т.е 0x0031 = 49 - мантисса для координаты Y. В кои-то веки совпало с предыдущим разом. По-моему, тут я в 4 раза завысил величину, а должен получаться 9,5 сантиметра сдвиг вправо (корабль стоит совершенно строго на нормали к стыковочному узлу, в 300 метрах от станции, но прибор смещён на 9,6 сантиметра влево). Да, 49/32768 * 256 м / 4 = 9,6 см.

3d fe, т.е 0xFE3D = -451 - мантисса для координаты Z. Также в точности совпало с предыдущим разом. Это должно означать -451 / 32768 * 256 м / 4 = -0,88 м. Да, потому что мы стоим на 88 см ВЫШЕ стыковочного узла.

ff 7f, т.е 0x7FFF = 32767 - скалярная часть кватерниона, фактически единица.
А остальные компоненты нулевые. Всё верно.

Фуух, заработало...

Теперь хочу ещё реализовать atan(y/x) и с его помощью выдачу результатов в виде углов, а там пора и к обнаружению точек на изображении вернуться, я же это безобразие так и не отладил до конца!
Tags: ПЛИС, программки, работа, странные девайсы
Subscribe

  • Тестируем atan1 на QuatCore

    Пора уже перебираться на "железо" потихоньку. Решил начать с самого первого алгоритма, поскольку он уже был написан на ассемблере. В программу внёс…

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 4 comments