QuatCore

Оба алгоритма захвата - в загрузочный сектор! (512 байт)

Наконец-то соединил между собой алгоритмы захвата ближней и дальней дистанции. Довольно существенная часть для них является общей - это перемножение матриц, выделение крена, масштаба и нахождение вектора, и даже процедура SwapPoints (поменять две точки местами).

Всего они заняли 254 слова кода, или 508 байт, то есть по-прежнему влезли в один блок внутренней памяти ПЛИС (БВП, он же EAB, Embedded Array Block). Ровно в той ПЛИС, которую я мучаю, 5576ХС4Т, таких блоков 24 штуки, а в той, куда хотелось бы вписаться, 5576ХС6Т (дофига радстойкая, в т.ч к тяжёлым заряженным частицам) - их всего 10.

Ещё один блок, как минимум, должен уйти на оперативную память.
Но ещё где-то два (один на оперативную, один на код) - под алгоритм обнаружения точек на фотоприёмной матрице, тот самый "супрематический", который я ковырял-ковырял, да так до конца ещё не отладил.

Итого уже 40% доступной памяти для ХС6Т мы заняли, а ещё алгоритм сопровождения нужен! Он довольно-таки "мерзопакостный"...

Collapse )

И поглядим, что получается "по дальней дистанции", старые добрые координаты, соответствующие 300 метрам "прямой наводкой":


Ярко-зелёным показаны 4 точки, после того как их идентифицировали. В этот раз наиболее удобным мне показался порядок МБД-МДД2-МДД1-МДД3.

Синим выделен вектор с общей "экспонентой" 9, т.е значения X (от 0 до 2), Y, Z (от -1 до +1) нужно домножить на 29-1 = 256 метра.

Имеем X = 0x9591 = 38289 = 1,168 (в формате Q1.15). Умножаем на 256 - и выходит 299,13 метров.
Далее, Y = 0x0031 = 49 = 0,00149 (в формате Q1.15). Умножаем на 256 - выходит 38 сантиметров. Похоже, я и тут пожадничал в 4 раза (чтобы точность не потерять раньше времени), на самом деле должно быть 9,5 см "сдвиг вправо", т.к смотрим мы "левым глазом", а летим прямо по курсу.

И наконец, Z = 0xFE3D = -451 = -0,0138 (в формате Q1.15). Умножаем на 64 (помня, что "пожадничали") и получаем -0,881 метра. Это мы сейчас установили "началом координат мишени" центр стыковочного узла, но прибор закреплён на те самые 0,881 метра ВЫШЕ, вот и воспринимает картинку как сдвинутую вниз.

Кватернион (выделен фиолетовым) вообще получился единичным, т.е выражающий нулевой поворот. Да, так и должно быть, идём "прямой наводкой".

Это у нас работа по модельным данным, я макет мишени дальней дистанции так и не собрал до сих пор, всё с ближней дистанцией возился. По модельным данным оно приятно - всё совпадает :)
QuatCore

Алг. ближ. дист - нахождение вектора (ФИНАЛ)

Вот и домучали практически, осталось получить дальность и ещё две координаты, практически "в плавающей точке". У нас на весь вектор общая "экспонента" (возведение в степень двойки) и 3 "мантиссы", причём дальность должна быть беззнаковой (по сути от 1 до 2), а поперечные координаты - со знаком, от -1 до +1.

Collapse )

Всё это вместе (алгоритм ближней дистанции) компилируется в 209 слов кода (418 байт). Сразу же запустим и посмотрим дамп памяти по окончании работы:


Всего уходит 343 мкс на выполнение, и если будет такое желание, можно на 33 мкс элементарно ускориться (сделать при нормировке кватерниона 4 итерации вместо 13). Дальше уже сложнее, но можно ещё примерно на те же 33 мкс "поджаться", но ценой увеличения кода. Но особой нужды не вижу, сейчас мы даже в обратный ход развёртки влезаем с огромным запасом, занимая где-то 1/5.

Синим выделен кватернион, в этот раз нормированный, норма его равна 1,0000139, лучше в 16 битах не сделаешь.

Фиолетовым вектор. Первое число - "экспонента", здесь ноль, что означает 20-1. (всегда ещё единичку вычитаем, чтобы получить диапазон дальности от 0,5 до 300 метров). Далее, идёт координата X, она же дальность. 0xAAD0 = 43728 ≈ 1,3345. То есть, дальность равна 20-1·1,3345 ≈ 0,667 метра.

Следующая координата Y, отвечающая за "курс" (влево-вправо). 0x07C2 = 1986 ≈ 0,0606. Умножаем на 0,5 метра - и получаем 0,0303 метра, или 3 сантиметра сдвиг вправо. Логично: мишень стоит довольно ровно по горизонтали, иначе она бы просто не влезла :) Но сдвиг вправо отчётливо виден:



Размер одного пятна как раз 3 см.

Наконец, координата Z, отвечает за "тангаж" (вверх-вниз). 0x4B9B = 19355 ≈ 0,5907. Помножаем на 0,5 метра и выходит 29,5 сантиметра сдвиг ВВЕРХ. Тут небольшая ошибка: я за "центр поля зрения" взял точку (512;512), что было бы логично для 1024х1024, но пока у нас изображение 1024х720, и центр мишени попадает примерно в Y=263. Вот и получилось, что по мнению алгоритма мы отодвинулись аж на 250 пикселей, а это должно соответствовать 5,9 градусам, что при дальности 0,667 метра даёт смещение в 6,8 сантиметра! Видать, тут значения больше в 4 раза, чем надо - не хотел точность терять.

Прицел 120, трубка 15, бац-бац И МИМО!

Как будто бы работает, но теперь всю эту историю калибровать надо. Точнее даже, "разрабатывать методику калибровки". Но ещё хочется сейчас оба алгоритма объединить наконец-то. Ставим ставки: влезет в 256 слов?
QuatCore

Тестируем вычисление масштаба

Сразу посмотрим дамп памяти по окончании работы:


Фиолетовым отмечены метрики Манхэттенская и Чебышевская, синим - кватернион, тёмно-зелёным - посчитанный масштаб (мантисса) и количество точек к обработке (нумерация с нуля).

Видно, что в кватернионе два последних элемента занулились, так и задумано - крен это поворот вокруг оси X, а повороты вокруг осей Y и Z (те самые "ракурсы") по аффинному приближению считать - дело неблагодарное, лучше оставить нули. Если приглядеться, кватернион ненормирован, его "длина" составляет 0,995. Всё верно: мы пока решили нормализацию не вызывать. Хуже всего с нормой здесь будет при крене в ±90°, кватернион будет 1/2 + 1/2i, с нормой около 0,707 (один делить на корень из двух). Четырёх итераций по методу Ньютона хватит, чтобы отнормировать его, насколько это возможно в 16 битах.

Если записать в целочисленном десятичном виде нашу матрицу, получается так:


Сейчас проверим, правильно ли всё посчиталось...

Collapse )

Работает, как задумано. Не уверен, что выдаст что-то осмысленное прямо сейчас, всё-таки камера "некалиброванная", но это дело поправимое.
QuatCore

Алг. ближ. дист, вычисление масштаба

Вот знал, что имеющийся код, который предполагался универсальным и для ближней дистанции, и для дальней, всё же придётся причесать. Вон, с креном обнаружилось переполнение при работе с полметра (минимально возможной дистанции). Это мы поправили. Следующая на очереди - процедура "вычленения" масштаба из матрицы масштаба и ракурсов.

Старый код:
	;Состояние регистров к этому моменту:
	;X=AfTransf,
	;Y=QuatY,
	;Z=Matrix,
	;i=j=k=Inv=0,
	;C=Acc=Txx
	FindScale	proc
	;1. Вычисляем величины |X-Z| (т.е |Txx - Tyy| ) и |2Y| (например, |Txy+Tyx|, хотя можно 2|Txy| или 2 |Tyx| )
	;важно, что в данный момент Inv=0
				[SP]		0	;манхэттенская метрика, сумма модулей (метрика городских кварталов)
				[SP+1]		0	;чебышевская, максимум от модулей 
				;текущее значение будем хранить в регистре C - дешево и сердито!
				i		1
				j		3					
		@@loop:		Acc		[X+i]	;+Tyx (i=1), -Tyy (i=0)
				PM		[X+i^j]	;Txy (i=1), Txx (i=0)   ;поменяли местами чтобы проверить модуль
				Inv		1		;уже можно! Так что на следующей итерации будет "-"
				[Y+i]		0		;очистим две компоненты кватерниона
				ABS		Acc		;|Txy+Tyx| (i=1), |Txx-Tyy| (i=0)
				C		Acc		;сохранили это значение
				ADD		[SP]	
				[SP]		Acc	;обновили манхэттенскую
				Acc		C
				SUB		[SP+1]
				JL		@@skipCheb ;лучше подошел бы JGE и поменять местами C и [SP+1]. Разница при нуле-на 1 больше операций.
				[SP+1]		C	;обновили Чебышевскую					
		@@skipCheb:	iLOOP		@@loop
	;теперь в [SP] лежит манхэттенская метрика, а в [SP+1] - чебышевская
	;осталось посчитать выражение целиком - сумму Txx+Tyy с весом 1/2, а эти - с весами 0,332 и 0,168
	;или сначала всё сложить - а потом поделить пополам...
	;можно, если ввести команду UDIV2 - беззнаковое деление
	;по сути, SHR1 
	
	;кстати, у нас до сих пор j=3 :)
	;i=k=0
				Acc		[X+i]	;добавили Txx
				ADD		[X+i^j]	;и Tyy
	;теперь наши метрики берём с весами	
				C		[SP]
				FMA		11010	;ManhW
				C		[SP+1]
				FMA		21758	;ChebW
				
	;ага, сделали
	;продемонстрируем результат
				JO		@@endLoop
		@@shl1:		ADD		Acc
				j++		0
				JNO		@@shl1		
	;ага, сделали
		@@endLoop:	DIV2		UAC ;получается отрицательное значение от -16384 до -1 (число -1 при "делении на 2" так и останется -1)
				SUB		32767 ;вычитает 32767, что даст отрицательное значение от -49151 до -32766. Но в UAC отобразится от 16385 до 32768.
				;если теперь найти обратную величину, это должно получиться в диапазоне 32768..65532
				;а если мы вычтем ещё половинку, может выйти от 16384 до 32768, и диапазон 32768..65536.
				;так что ну её нахрен, эту половинку?
				;или можем уже на этапе Ньютона этот случай проверить.
				DIV2S		1 ;это для нужд округления. 					
				[SP]		UAC							
	FindScale	endp


Содержание регистров к началу процедуры претерпели изменения, но уже не столь серьёзные. А ещё мы решили, что без команды ijk некуда, поэтому можно её здесь применить и немножко сэкономить.

И ещё одна вещь: в [SP+1] у нас до сих пор лежит число, позволяющее отличить "ближнюю дистанцию" от "дальней". В первом случае там лежит пятёрка (т.е точки от 0 до 5), во втором - тройка (т.е от 0 до 3). Не затереть бы его ненароком!

Collapse )

Компилируется всё это дело (алг. ближ дистанции) в 191 слово кода (382 байта). Возможно, здесь я чуть перемудрил с нахождением масштаба, можно львиную долю этого кода выполнить с незначительным снижением точности. Это когда появится алгоритм сопровождения, надо будет основательную отработку устроить, смотреть, удаётся ли "втянуться" в сопровождение с различных ракурсов и различных масштабов, или происходит срыв.

А сначала хоть этот кусок надо потестировать...
QuatCore

Ошибка в измерении крена: ларчик просто открывался

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

У меня на фоне развлечений с матрицами сразу же мысль была, что я не с той стороны её умножал. Но сразу же терзали сомнения: А КАКАЯ РАЗНИЦА, С КАКОЙ СТОРОНЫ УМНОЖАТЬ? Когда "косинус" я получил, сложив диагональные элементы, а "синус" - вычитая друг из друга два оставшихся, то кажется не важным, с какой стороны умножать.

Так оно есть!
Возьмём произвольную матрицу 2x2:


И построим "матрицу поворота" из неё:


Умножим на неё "слева":


Получили симметричную матрицу, как и хотели, и тут никуда не денешься, нельзя назвать входную матрицу "неправильной", для произвольной матрицы должно получиться именно так!

А теперь попробуем взамен умножить на неё "справа":


Это ДРУГАЯ матрица, но всё равно симметричная, что бы мы не подали на вход!

Сразу же возникает два вопроса: какая из этих двух матриц верная, и почему же у нас не получилась ни та, ни другая???

Collapse )

Смотрим дамп памяти после работы исправленной программы:


Фиолетовым выделена матрица, вот теперь она действительно симметричная, оба элемента вне диагонали 0x007A, это примерно 0,0037.

Синим выделен кватернион. Два последних числа - это пока что co = 32427, si = 4718. Вот теперь всё правильно, с точностью до последнего бита. atan(4718/32427) ≈ 8,278°. Попробуем повернуть изображение на этот угол:


Уже получше, но не идеально. Всё-таки там и координаты пока "плюс-минус лапоть", т.к нахождение "яркостных центров пятен" ещё не делали.

Можно двигаться дальше, к вычислению масштаба.
Sidious

Водное на даче

Майские праздники у меня получились 2 раза по 3 дня, посерёдке 4 дня работал как ни в чём не бывало.

Было планов громадьё, но как водится, не всем суждено было сразу исполниться. Гораздо больше времени заняло приведение водопровода в полный порядок, а потом аккурат на 9 мая дачу опять затопило, пока я сидел в доме:

IMG20210509094416.jpg

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

Под катом страшный "тумблер судного дня", толпа вентилей, "лыжная база", картины потопа и бонусная Марта.

Collapse )
QuatCore

Тестируем вычисление крена (алг ближ дистанции)

После выполнения всех предыдущих этапов мы получили для тестового кадра такую матрицу аффинного преобразования:



Сейчас посмотрим, как из неё выделится крен. Проект синтезировался в 525 ЛЭ (процессор без периферии), лишние 3 ЛЭ за счёт увеличения ширины адресной шины ПЗУ с 7 до 8 бит, вообще не заметно почти. интрига в том, сколько будет весить уже вся программа в сборе - обнаружение точек, вычисление яркостных центров с субпиксельной точностью, захват ближней и дальней дистанции, сопровождение, и до кучи информационный обмен. Предельная частота: 30,3 МГц, нормально.

По окончании 330 мкс выполнение завершается. Посмотрим дамп памяти:


Сейчас разберёмся, соответствует ли оно действительности...

Collapse )

Неужели порядок матриц перепутал? С меня станется: до сих пор с какой стороны не умножай - получался бы практически одинаковый результат. Тогда даже хорошо, что у меня такой косяк со "сплющенной" картинкой - а то так и не заметил бы ничего, а ведь это неправильное умножение у меня ещё с прошлого года было! Впрочем, может я делаю поспешные заключения. Вернусь домой - и громко подумаю.
QuatCore

Алг. ближней дистанции, вычисление крена

Всех с прошедшими первомаем, днём радио и днём Победы!

А мы продолжим ковырять штуку... В общем-то, я говорил, что после вычисления аффинного преобразования (матрица 2х2 и вектор 1х2) дальше уже всё готово. Так примерно и есть, но мы чуть поменяли то, в каких регистрах что лежит, и не хочется грубо всё переопределять перед уже написанным кодом, лучше этот код чуточку подрихтовать, и посмотреть свежим взором, нельзя ли его улучшить?

Начнём с самой странной процедуры FindRoll - "нахождение крена". Она сначала находит синус и косинус угла крена, затем преобразует их в кватернион, выражающий этот крен (по сути, находит синус и косинус половинного угла), а под конец убирает из матрицы 2х2 аффинного преобразования крен, чтобы там остались только масштаб и "ракурсы"...

Collapse )

Теперь программа "ближней дистанции" компилится в 157 слов кода (314 байт), вышли-таки на 8-битную адресацию ПЗУ, ну ничего не поделаешь, сложный алгоритм всё-таки. И сейчас посмотрим на симуляции, правильно ли всё работает.
QuatCore

Тестируем алг. захвата - перемножение матриц

Наконец-то запишем матрицу из прошлой части в секцию .data нашей программы:

;заранее посчитанная матрица для ближней дистанции
;МБД 8-6-7-3-5-4,  отражатели 1-2 не берём, они в другой плоскости
ShortRangeAffine:
SaMat00	Int16	0x10D8	;0,13158
SaMat01	Int16	0xAC5F	;-0,65334
SaMat02	Int16	0x13B5	;0,15396
SaMat10	Int16	0x1FA3	;0,24715
SaMat11	Int16	0x0E03	;0,10948
SaMat12	Int16	0x13B5	;0,15396
SaMat20	Int16	0x0DDD	;0,10832
SaMat21	Int16	0x459E	;0,54387
SaMat22	Int16	0x13B5	;0,15396
SaMat30	Int16	0xF223	;-0,10832
SaMat31	Int16	0x459E	;0,54387
SaMat32	Int16	0x13B5	;0,15396
SaMat40	Int16	0xE05D	;-0,24715
SaMat41	Int16	0x0E03	;0,10948
SaMat42	Int16	0x13B5	;0,15396
SaMat50	Int16	0xEF28	;-0,13158
SaMat51	Int16	0xAC5F	;-0,65334
SaMat52	Int16	0x13B5	;0,15396


Напомним, что всё содержимое этой секции превращается в файл инициализации памяти (Memory Initialization File) QuatCoreData.mif, который затем становится составной частью конфигурации ПЛИС. Как только ПЛИС конфигурируется - в памяти уже лежит не мусор, а ровно то, что мы хотели - ОЧЕНЬ УДОБНО. По сути, мне не нужны никакие загрузчики, которые бы переносили содержимое ПЗУ в ОЗУ и всё в этом духе - уже всё сделано :)

Вот теперь есть всё необходимое, чтобы проверить процедуру перемножения матриц, которую мы написали здесь, попутно и заменив адресацию "4j" на "3j", т.к она нужнее. Вот всё вместе и проверим.

Для начала - когда мишень стоит горизонтально, но с небольшим наклоном, тупо чтобы влезть в поле зрения при 0,5 метрах.

Пока что всё это умещается в 115 слов кода (230 байт), это алг. ближней дистанции вплоть до перемножения матриц. Хотя ещё забьём, дурное дело не хитрое.

Выполнение всего алгоритма (нахождение 2 наиболее отдалённых, сортировка "слева направо", поиск центральных точек, проверка Берегового, перемножение матриц) занимает 226 мкс в этом случае. Сразу посмотрим дамп памяти, и если там всё хорошо - можно в "осциллограмме" не копаться.

Результаты вычислений должны лежать с адреса 0x52, их я выделил синим:


Что расшифровывается в такое:


Выглядит многообещающе.

Collapse )

Отлично! Остальная возня с этой матрицей у нас уже реализована, надо только с масштабом под конец не ошибиться. Но это уже на следующей неделе. Пожалуй, пора ехать на дачу, там конь не валялся...
QuatCore

"Нормируем" матрицу для вычисления аффинного преобразования

В прошлый раз мы получили такую матрицу:


Точнее, должны были получить такую, но я забыл, что уже начал её "причёсывать" и привёл в посте матрицу, где верхние 2 строки уже поделены на 8. Потом думаю "что-то не то, не могло же мне так повезти, что "сходу" все значения в диапазоне -1..+1, и все довольно крупные. Сегодня проверил и понял - действительно не могло :)

Предполагается, что мы посчитали её заблаговременно, а на этапе захвата умножаем её справа на матрицу 6x2, составленную из обнаруженных точек, расположенных в правильном порядке - и получим матрицу аффинного преобразования. Причём, если эта матрица обнаруженных точек будет в пикселях - то и вектор параллельного переноса (rx, ry) будет выражен в пикселях, а основная матрица 2х2 - в пикселях на метр.

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

Поэтому нужно эту матрицу ещё немного подрихтовать, чтобы после умножения на неё получать настолько информативные результаты, насколько это возможно :) И ещё, может, заставить её выправить "прямоугольные пиксели", которые сейчас получаются на моём макете?

Collapse )

Итак, матрицу, которую мы посчитали по красивой формуле, нужно ещё поделить на α - "количество пикселей на радиан", а потом нижнюю строку умножить на 2048, а верхние строки на 256. И результат представить в формате Q1.15:



И в память её надо поместить столбец за столбцом. Сейчас так и сделаем - и опробуем процедуру перемножения матриц...