nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Аффинный алгоритм + информационный обмен

Возвращаемся к нашим баранам, к макету видеоизмерителя параметров сближения.

В нём готов и сколько-нибудь отлажен модуль информационного обмена, работающий автономно, без помощи процессора. И также готов модуль DMA (Direct Memory Access), позволяющий и процессору, и модулю информационного обмена обращаться к одной и той же оперативной памяти, но пока не отлажен.

Хочется попробовать всё это запустить "с нахрапу", но для этого надо программу подправить. Возьму пока "аффинный алгоритм", который у меня влез в "загрузочный сектор" (512 байт), но надо всю память перераспределить, в соответствии с этой табличкой:




Подадрес 0x00 (адреса в памяти 0x000..0x01F) - в МКО он зарезервирован под команды управления, они могут передать или запросить только 1 слово данных. Причём в случае передачи этого слова нам, мы его не запишем. Логика такова, что первое слово мы всегда сверяем (это "заголовок массива"), а потому не перезаписываем. Если запросят чтение - выдадим слово по нулевому адресу, "пускай", пока подобные команды задействовать не собираемся, с чистой совестью считаем этот подадрес "свободным".

Это, пожалуй, к лучшему, туда мы в первую очередь запихнём последовательность для задания тактовой частоты:

EthDisable	db	2,3,0x22,0x54,0x00,0x01,3,0x22,0x66,0x00,0x18,2,0x22,0x6F,0x02


это описывается в файле SetClock.asm, здесь 3 сообщения для Ethernet-контроллера, чтобы он отключил всё, что можно отключить, а также выставил тактовую частоту ClkOut равной 25 МГц.

Хоть здесь и написано db (define byte), располагаются они не "компактно" (старший байт - младший байт, старший байт - младший байт), а по возрастающим адресам, сначала в младших байтах. Итого, 15 слов из 32 на "нулевом подадресе" уже заняли.

И сюда же запихаем всевозможные строки, если всё-таки решим и ЖК-экранчик задействовать, а в нём выдавать данные по-человечески. А вообще, можно почти все "закоулки" объединить в связанный список Heap для алгоритма обнаружения.

Подадрес 0x11 (адреса в памяти 0x020..0x03F) - "полётное задание". Давайте его определим в памяти и дадим некие "значения по умолчанию":

;подадрес 0x11 (адреса 0x020..0x03F) - полётное задание
ORG 0x020
FT_header	dw	0xAAAA	;заголовок массива (коды Рида-Мюллера)
FT_cnt		dw	0xFFFF		;циклический счётчик сообщений "полётное задание" (чего-то заказчики перемудрили)
FT_flags	dw	0		;всевозможные флажки, может быть введём режим Bit Banding в итоге, чтоб к ним обращаться:
;0:1	- LED (режим работы осветителя)
;2	- UseCartes (0:использовать "сферические координаты", как в ТЗ, 1: использовать декартовы, чтобы избежать лишних преобразований)
;3	- AcqL (разрешение захвата на дальней дистанции)
;4	- AcqS (разрешение захвата на ближней дистанции)
;5	- Trk (разрешение перехода в сопровождение)
;6	- StereoEn (разрешение стереорежима)
;7	- MDDen (разрешение использования МДД при переходе на ближнюю дистанцию)
;8	- UseCov (нужно ли вычислять ковариационную матрицу шума измерений)
;9:15	- Reserved0 (пока не используется)
FT_shortRange	dw	0x780	;с какой дальности перейти на МБД, по умолчанию с 15 метров (чтобы получить метры, нужно число здесь поделить на 2^7 = 128)
FT_VelLW	dw	35	;размер окна (в отсчётах) для определения скорости сближения на дальней дистанции
FT_VelSW	dw	5	;размер окна (в отсчётах) для определения скорости сближения на ближней дистанции
FT_VelThr	dw	0x2300;на какой дистанции переходить от большого окна к маленькому окну (чтобы получить метры, нужно поделить на 2^7 = 128)
FT_alpha	dw	?	;угловой размер пикселя. Наиболее удобную единицу измерения выберем, когда будем писать алгоритм сопровождения
FT_ShipExp	dw	0	;вектор перехода из системы координат ВИПС в систему координат корабля, общая экспонента
FT_ShipX	dw	0	;вектор перехода в СК корабля, X
FT_ShipY	dw	0	;вектор перехода в СК корабля, Y
FT_ShipZ	dw	0	;вектор перехода в СК корабля, Z
FT_ShipQuat0	Int16	-32768	;кватернион перехода в СК корабля, скаляр
FT_ShipQuatX	Int16	0		;кватернион перехода в СК корабля, X
FT_ShipQuatY	Int16	0		;кватернион перехода в СК корабля, Y
FT_ShipQuatZ	Int16	0		;кватернион перехода в СК корабля, Z
FT_StereoVecExp	dw	0	;вектор "стереобазы", общая экспонента
FT_StereoVecX	Int16	0	;вектор "стереобазы", X
FT_StereoVecY	Int16	0	;вектор "стереобазы", Y
FT_StereoVecZ	Int16	0	;вектор "стереобазы", Z
FT_StereoQuat0	Int16	-32768	;кватернион "стереобазы", скаляр
FT_StereoQuatX	Int16	0		;кватернион "стереобазы", X
FT_StereoQuatY	Int16	0		;кватернион "стереобазы", Y
FT_StereoQuatZ	Int16	0		;кватернион "стереобазы", Z
FT_CRC		dw	?	;сюда запишется контрольная сумма полётного задания, если передача была успешной


Не факт, что всё это будет задействоваться - я постарался придумать всё, что может нам быть полезным, и сразу прописать в протоколе. Тут и "стереорежим", когда два прибора действуют не как "основной и резервный", а как левый и правый глаз, благодаря чему лучше "воспринимается объём", и всё-таки ДВЕ системы координат. Одна связана с нашим объективом и фотоприёмной матрицей, а вторая, в которой мы и будем выдавать данные, может быть сдвинута и повёрнута относительно первой.

И заказчики в самый последний момент включили поле FT_cnt, циклический счётчик сообщений "полётное задание", теперь не очень ясно, а что с ним вообще делать? Это, получается, они все сообщения, отправленные нам, нумеруют? Ну допустим, мы получили полётное задание, и там единичка. И что?? Если бы информации было прорва, так что за один раз не передашь - был бы смысл. Под номером "0" идёт первая часть, под номером "1" вторая, и так далее. А так - что получили, тому и рады.

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

Ну да ладно, определённая сермяжная правда в этом есть. Можно тогда не заморачиваться с собственным подсчётом, а как-то наладить переброс этого FT_cnt в телеметрию. И дать значение по умолчанию "-1", означающее "это вообще значения, зашитые в ПЗУ, ничего взамен нам не поступало!", и ещё одно, выражающее ошибку - "уже перезаписали новые данные, но CRC не сошёлся". Но подозреваю, протокольный контроллер придётся ещё немного доработать, чтобы он это автоматом делал...

Много "костылей" с измерением скорости сближения. На большой дальности шумит сильно, надо усреднять, иначе в требования ТЗ не уложимся. А на малой дальности, по моей модели, как раз нельзя усреднять - манёвры начинаются, а усреднение ведёт к запаздыванию. Я вообще считаю, что скорость должен уже "борт" считать, используя и наши показания дальности, и данные с акселерометров. Эти два прибора дополнили бы друг друга: прелесть ВИПС в том, что он не накапливает ошибок, "что видит ровно сейчас - то и измеряет", но измерения дальности у него "шумят". У акселерометра всё наоборот: на малых интервалах он очень точно приращение скорости найдёт, но с течением времени дрейфует. Но пока всё-таки готовимся выполнять требования ТЗ. Не мудрствуя лукаво, оно было "скопировано" с ТЗ на радиосистему "Курс", который скорость меряет по Допплеровскому смещению, это отдельный канал, не связанный с измерением дальности. Мы же скорость берём как производную дальности. Ну да ладно.

Подадрес 0x12 (адреса в памяти 0x40..0x5F), "параметры МДД", то есть Мишени Дальней Дистанции:
;подадрес 0x12 (адреса 0x040..0x05F) - параметры МДД
ORG 0x040
LT_header	dw	0xCCCC	;заголовок массива
LT_cnt		dw	0xFFFF	;циклический счётчик сообщений "параметры МДД"

LT_MBDz		Int16	0		;мишень ближней дистанции (воспринимаемая как одно пятно)
LT_MBDy		Int16	0		;диапазон значений от -4 до +4 метра (на всякий случай)
LT_MBDx		Int16	-2376

LT_MDD2z	Int16	4743	
LT_MDD2y	Int16	9339	
LT_MDD2x	Int16	0

LT_MDD1z	Int16	410	
LT_MDD1y	Int16	-6832	
LT_MDD1x	Int16	0	

LT_MDD3z	Int16	15794	
LT_MDD3y	Int16	-5554	
LT_MDD3x	Int16	0	

;заранее посчитанная матрица для нахождения коэф. аффинного преобразования
;МДД3-МДД1-МДД2-МБД
LongRangeAffine:
LT_AfMat00	Int16	0x0102		
LT_AfMat01	Int16	0x1BFE
LT_AfMat02	Int16	0x1464

LT_AfMat10	Int16	0xC77C
LT_AfMat11	Int16 	0xF88E
LT_AfMat12	Int16 	0x2213

LT_AfMat20	Int16	0x2797		
LT_AfMat21	Int16	0x22D6
LT_AfMat22	Int16	0x0F30

LT_AfMat30	Int16	0x0FEB
LT_AfMat31	Int16	0xC89E
LT_AfMat32	Int16	0x2696
LT_CRC		dw	?


Что-то я с координатами перемудрил, решив их "с конца" записывать, по-моему рассчитывал, что QuatCore сам займётся информационным обменом, поступающие данные начнёт распихивать в цикле, а циклы у нас удобнее всего "до нуля".

Был ещё очень хитрый план расположить сначала все отдельные отражатели мишени ближней дистанции (8 штук), затем 3 мишени дальней дистанции, а под самый конец опять мишень ближней, "как единое целое", чтобы в зависимости от текущей дальности и от настроек, можно было бы либо работать по дальней дистанции (последние 4), либо исключительно по ближней (первые 8), либо и по ближней, и по дальней (первые 11). Но что-то не складывается: сейчас между ними всё равно "прорехи" образуются в памяти, а даже если и сложилось бы - у них цена деления сейчас разная! Мишеням дальней дистанции после долгих раздумий дал диапазон аж -4..+4 метра, а мишеням ближней: -0,25..+0,25 метра. В одном масштабе чуть-чуть не хватало точности... Для дальней дистанции можно было ужаться максимум до -2..+2 метра, что давало цену младшего разряда 61 мкм и ошибку порядка 30 мкм, казалось бы, ДОСТАТОЧНО! Ещё попробуй с такой точностью всё изготовь и смонтируй. Но всё-таки в процессе вычислений накапливается ошибка где-то в 1-2 единицы младшего разряда, т.е 0,12 мм. А когда пытаешься измерить смещение центральной точки, вынесенной на 7 см, там ошибка на 0,12 мм ТУПО В ИСХОДНЫХ ДАННЫХ приводит к ошибке по углу на 0,1°, а это и есть точность, требуемая по ТЗ. Вот только кроме ошибки в исходных данных будет ещё ошибка измерения яркостных центров, и ошибка вычислений, так что требования заведомо выполняться не будут!

Возможно, у меня вектора будут превращены в кватернионы, где в скалярной части расположится общая "экспонента". И снова хочется, чтобы протокольный контроллер немного "поумнел" и сам её заполнил. "Аппетит приходит во время еды".

Подадрес 0x03 (адреса в памяти 0x060..0x07F) - параметры МБД
То бишь, Мишени Ближней Дистанции. Тут я из 5 мая 2021 года страшно недоволен собой из 19 апреля, подписавшего протокол, где эти мишени размещены в каком-то чудовищном порядке. Буду мыслить положительно, в смысле, положу на это болт, и здесь напишу так, как положено:

;подадрес 0x03 (адреса 0x060..0x07F) - параметры МБД
ORG 0x060
ST_header	dw	0x6666	;заголовок массива
ST_cnt		dw	0xFFFF	;циклический счётчик сообщений "параметры МБД"

;координаты отдельных отражателей мишени ближней дистанции, в увеличенном масштабе
;единица измерения - метры,
;диапазон представимых значений - от -0,125 до примерно +0,125 метров
;пользуемся тем фактом, что ракурс не более чем на 30 градусов, поэтому 
;"есть где развернуться"

ST_MBD8x	Int16	0
ST_MBD8y	Int16	11048
ST_MBD8z	Int16	-6730	

ST_MBD6x	Int16	0		
ST_MBD6y	Int16	20752	
ST_MBD6z	Int16	1128

ST_MBD7x	Int16	0
ST_MBD7y	Int16	9095
ST_MBD7z	Int16	5602

ST_MBD3x	Int16	0
ST_MBD3y	Int16	-9095
ST_MBD3z	Int16	5602

ST_MBD5x	Int16	0
ST_MBD5y	Int16	-20752
ST_MBD5z	Int16	1128

ST_MBD4x	Int16	0
ST_MBD4y	Int16	-11048
ST_MBD4z	Int16	-6730

ST_MBD1x	Int16	7864
ST_MBD1y	Int16	-13631
ST_MBD1z	Int16	0

ST_MBD2x	Int16	7864
ST_MBD2y	Int16	13631
ST_MBD2z	Int16	0

ST_CRC		dw	?


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

Подадрес 0x14 (адреса в памяти 0x80..0x9F) - аффинная матрица МБД
Да, вот и она, отдельным сообщением.

Тоже придётся чуть редактировать протокол, надеюсь, возражений это не вызовет. Поэтому вот так:

;подадрес 0x14 (адреса 0x080..0x09F) - аффинная матрица МБД
ORG	0x080
Af_header	dw	0xF0F0	;заголовок массива
Af_cnt		dw	0xFFFF	;циклический счётчик сообщений "аффинная матрица МБД"

;заранее посчитанная матрица для ближней дистанции
;МБД 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

Af_CRC		dw	?


Подадрес 0x05 (адреса в памяти 0x0A0..0x0BF) - "сырые данные" от соседнего комплекта

А пока сюда запихну сырые данные наши собственные, чтобы можно было алгоритм испытать на огромном количестве различных сценариев, посылая данные с компьютера (взамен тех, которые должны получиться после камеры), благо, по формату они должны быть очень похожи.

Впрочем, и тут я до конца не определился. Когда мы выходим на сопровождение, логично будет, что у нас на каждую мишень будет выделено своё место. Если какие-то уже выходят за пределы поля зрения, в их X-координату (к примеру) заносим 0x8000 (-32768), это будет признак "отсутствия данных". На этапе захвата у нас просто неупорядоченный набор точек, и можно ещё, чтоб не мучаться, вывести "количество обнаруженных точек". Если их слишком много или слишком мало - сразу отказываемся что-либо делать.

А ещё с максимальным количеством пятен непонятно. Издали, понятно, четыре пятна (МДД1-3 и МБД как единое целое). Вблизи: восемь пятен (МБД1-МБД8). А вот где-то посерединке, на 5..15 метрах, уже и МБД разрешается на отдельные пятна, но и МДД также предстают отдельными точками, по 7 в каждой на нынешней "итерации". Выходит, "по наихудшему случаю" их 29 штук, и на каждую точку надо по 2 слова (координаты X и Y), это уже два сообщения отправлять надо!

В режиме сопровождения такой фигни быть не должно: там, уже идентифицировав точки, мы будем "заказывать" мишени дальней дистанции как "единое целое", пофиг, что каждое предстаёт в виде 7 пятен прижатых друг к дружке. Так что волевым решением объявим, ДО 11 ТОЧЕК, и выйдет такое описание:

;подадрес 0x05 (адреса 0x0A0..0x0BF) - сырые данные с соседнего комплекта для стереорежима
ORG	0x0A0
RR_header	dw	0x5A5A	;заголовок массива

;тестовые данные: дистанция 300 метров, все углы нулевые, получили "вне времени"
RR_PRF_NO	dw	0xFFFF	;метка времени (корректные значения от 0x0000 до 0xC800)

RR_Count	dw	4		;количество обнаруженных точек
Points2D:
RR_x0		Int16	53	
RR_y0		Int16	-7
RR_x1		Int16	480	
RR_y1		Int16	-30
RR_x2		Int16	-539	
RR_y2		Int16	-302
RR_x3		Int16	400	
RR_y3		Int16	-997
RR_x4		Int16	-32768
RR_y4		Int16	?
RR_x5		Int16	-32768
RR_y5		Int16	?
RR_x6		Int16	-32768
RR_y6		Int16	?
RR_x7		Int16	-32768
RR_y7		Int16	?
RR_x8		Int16	-32768
RR_y8		Int16	?
RR_x9		Int16	-32768
RR_y9		Int16	?
RR_x10		Int16	-32768
RR_y10		Int16	?
RR_CRC		dw	?		;контрольная сумма


Префикс RR означает Raw Received, т.е сырые данные, ПРИНЯТЫЕ с соседнего комплекта.

Фух, с даннными на приём разобрались. Теперь на передачу.

Подадрес 0x06 (адреса в памяти 0xC0..0xDF) - целевая информация

Ради этих данных всё и затевалось, остальное так, "обвязка". Тут, увы, тоже всё ОЧЕНЬ весело. С одной стороны, нужно было безусловно выполнить требования ТЗ (сказано измерять углы - значит будем выдавать углы), но с другой, у меня были свои мысли, какую именно информацию на самом деле надо выдавать, чтобы избежать бессмысленных преобразований взад-вперёд, и вообще выжать из полученного кадра полный максимум, хотя бы "в перспективе". Приведу описание, потом обсудим:

;подадрес 0x06 (адреса 0xC0..0xDF) - целевая информация
ORG	0xC0
DA_header	dw	0x3C3C	;заголовок массива
DA_flags	dw	?		;всевозможные флажки
;0:1 - Mode (00-поиск, 01-захват, 10-сопровождение, 11-не используется)
;2   - Stereo (1-стереорежим активен)
;3   - DistOK, признак достоверности выдаваемой дистанции, активных углов и крена, (1-достоверны)
;4   - VelOK, признак достоверности выдаваемой скорости, (1-достоверна)
;5   - QuatOK, признак достоверности выдаваемого кватерниона (1-кватернион достоверен)
;6   - CovarN, какая часть ковариационной матрицы шума измерений передаётся в этом сообщении.
;              (0 - первые 11 значений, 1 - последнии 10 значений)
;7:15 - reserved (не используются)
DA_PRF_NO	dw	0xFFFF	;метка времени (правильные значения от 0 до 0xC800, сейчас у нас "вне времени")
DA_vel		Int16	?		;скорость сближения, диапазон -2..+2 м/с

DA_Vexp:	;при UseCartes=1: 
DA_dist	dw	?		;При UseCartes=0 (по умолчанию): дальность, диапазон 0..512 метра 
DA_Vx:	;При UseCartes=1:
DA_yaw		Int16	?		;При UseCartes=0 (по умолчанию): угол активного курса, диапазон -4..+4 радиан
DA_Vy:	;При UseCartes=1:
DA_pitch	Int16	?		;При UseCartes=0 (по умолчанию): угол активного тангажа, диапазон -4..+4 радиан
DA_Vz:	;При UseCartes=1:
DA_roll		Int16	?		;При UseCartes=0 (по умолчанию): угол взаимного крена, диапазон -4..+4 радиан
DA_Quat0	Int16	?	;кватернион взаимной ориентации, скалярная часть
DA_QuatX	Int16	?	;диапазон -1..+1 для каждой компоненты
DA_QuatY	Int16	?
DA_QuatZ	Int16	?

Matrix:
DA_M41:	;когда CovarN=1. А ещё Txx
DA_M00	Int16	?
DA_M42:	;когда CovarN=1. А ещё Txy
DA_M10	Int16	?
DA_M43:	;когда CovarN=1. А ещё Tyx
DA_M11	Int16	?
DA_M44:	;когда CovarN=1. А ещё Tyy
DA_M20	Int16	?
DA_M50:	;когда CovarN=1
Rx:
DA_M21	Int16	?
DA_51:	;когда CovarN=1
Ry:
DA_M22	Int16	?
DA_M52:	;когда CovarN=1
DA_M30	Int16	?
DA_M53:	;когда CovarN=1
DA_M31	Int16	?
DA_M54:	;когда CovarN=1
DA_M32	Int16	?
DA_M55:	;когда CovarN=1
DA_M33	Int16	?
DA_M40	Int16	?


4 поля зависят от того, выбрано ли UseCartes=0 в полётном задании или UseCartes=1 (Cartesian=декартовы, Des Cartes если что). В первом случае выдаётся дальность (в некотором роде "радиус-вектор"), углы курса и тангажа (как бы два сферических угла) и ещё отдельно взаимный крен. Вообще-то он и так "сидит" внутри кватерниона, но здесь тоже нюансы. Мы будем до дальности в 30 метров утверждать, что кватернион недостоверен, т.к требуемая точность ещё не получена, пассивные углы очень шумят (попробуй пойми, как к тебе эта штука повёрнута, с дальности 300 метров, да ещё и с точностью в 0,3°), хотя именно крен меряется хорошо. И раз уж он меряется хорошо - неплохо бы его скомпенсировать в самом-самом начале. Вот и вывели его на всякий случай отдельным полем, хотя я давал ссылку на свой "ликбез", разложение кватерниона на повороты.

Если же UseCartes=1, то вместо дальности и 3 углов будем выдавать вектор "параллельного переноса", с "общей экспонентой". Только так на любой дальности получается точная запись углов. Декартово представление удобно по двум причинам:
- именно так "внутри" это представлено в алгоритме сопровождения,
- в декартовом представлении легко можно переместить начало координат, просто прибавив вектор, тогда как в сферическом это огромный геморрой.

Впрочем, поскольку мы в полётном задании предусмотрели вектор параллельного переноса и кватернион для перехода в систему координат космического корабля, то можем и сами всё преобразовать и дать "на борт" данные в наиболее удобном для них виде. Может, углы и дальность, приведённые в ПРАВИЛЬНОЙ системе координат (где начало координат лежит хотя бы на оси корабля, а может и в центре масс) даже лучше.

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

Поначалу у меня вообще всё умещалось в одно сообщение в 32 слова: матрица (21 слово), кватернион (4 слова), дальность с углами (4 слова), скорость (1 слово), флажки (1 слово), метка времени (1 слово), итого ровно 32. Но потом от нас потребовали заголовок массива и CRC, и САМУЮ МАЛОСТЬ не хватило... Ладно, взаимный крен можно было убрать (он из кватерниона "вытаскивается"). Но ещё одно слово надо было как-то "выцарапать", и вот тут уже грусть. Может, стоило сообразить, ковариация между какими 2 параметрами наиболее "бесполезна" (заведомо околонулевая) - и выкинуть её нафиг. Поэтому пришлось матрицу разбить на две части, которые должны будут передаваться попеременно. Компьютерное моделирование показало, что от кадра к кадру эта матрица меняется очень слабо, разве что когда одна из мишеней уходит за поле зрения, либо в мишени ближней дистанции начинаем видеть отдельные отражатели - там скачок очень резкий.

Обратите внимание: здесь CRC я не объявляю в конце сообщения! В сообщениях "на приём" пока что объявил, потому что в нынешнем виде протокольный контроллер может полученный в сообщении CRC записать в память на это самое место, и очень важно, чтобы ничего ценного при этом не затёр. А тут, на передачу, запрос в память будет на последнее слово в сообщении, но "в ход" оно не пойдёт, вместо него к выходу напрямую присоединится модуль CRC.

Это очень важная особенность, поскольку здесь мы можем "продолжить" ковариационную матрицу шума измерений, должна же она где-то лежать целиком! Тогда единственное дополнительное действие - на каждом втором кадре переносить "нижние" 10 значений наверх. Объявим их здесь:
a41		Int16	?
a42		Int16	?
a43		Int16	?
a44		Int16	?
a50		Int16	?
a51		Int16	?
a52		Int16	?
a53		Int16	?
a54		Int16	?
a55		Int16	?


Впрочем, последнее значение всё равно "вылезает" на следующий подадрес, т.е на адрес 0xE0. Но это не страшно: как раз этот подадрес, 0x17, у нас свободен. Дело в том, что младшие 4 бита подадреса: 0111. К ним добавляем единицу в старшем разряде, чтобы сумма битов была чётной: 10111. Это число всего одним битом отличается от 11111 - подадреса "режима управления", поэтому его пока не используем. Соответственно, и память эту можем использовать "в своих собственных целях". Пока что сунем сюда вектор параллельного переноса:

AfTransf:		;когда начинаем вектор находить, матрица уже не нужна, зато лишний раз регистр переиначивать не надо!
Exp	dw	?	;отдельно экспонента.	Когда это аф матрица, то Txx
Tx	dw	?	;Tx отдельно.		Когда это аф матрица, то Tyx
Ty	Int16	?	;другие значения.	Когда это аф матрица, то Txy
Tz	Int16	?	;			Когда это аф матрица, то Tyy


Если UseCartes=1, то ровно эти 4 значения будут копироваться в DA_Vec0 ..DA_vecZ, но при UseCartes=0 они будут преобразованы "в сферические координаты".

Не сказать, что другое "укромное место" не мог найти - были "прорехи" и в предыдущих сообщениях (7, 5, 5, 11 и 6 слов соответственно, и на 1 больше каждое число, если запретить протокольному контроллеру записывать в память значение CRC), но почему бы и нет... Здесь чуточку безопаснее: сейчас есть возможность записать все 32 слова на каждый разрешённый для записи подадрес!

Подадрес 0x18 (адреса в памяти 0x100..0x11F) - телеметрия

Пока с телеметрией шибко много придумать не удалось, всего лишь 9 слов (не считая заголовка и CRC):

;подадрес 0x18 (адреса в памяти 0x100..0x11F) - телеметрия
ORG 0x100
TM_header	dw	0xFF00
TM_CmosTemp	Int16	?	;температура фотоприёмной матрицы (с встроенного в неё датчика), в градусах Цельсия
TM_AmbTemp	Int16	?	;температура на плате, в градусах Цельсия
TM_Exposure	dw	?	;время экспозиции фотоприёмной матрицы, дискретное значение от 0 до 14
TM_ILED	dw	?	;заданный ток светодиодов, значение ШИМ
TM_FT_CNT	dw	?	;последнее значение циклического счётчика FT полученное от БВС
TM_LT_CNT	dw	?	;последнее значение циклического счётчика LT полученное от БВС
TM_ST_CNT	dw	?	;последнее значение циклического счётчика ST полученное от БВС
TM_AF_CNT	dw	?	;последнее значение циклического счётчика AF полученное от БВС
TM_RA_CNT	dw	?	;последнее значение циклического счётчика RA ("сырые данные")


Может, потом ещё чего-то полезное в голову взбредёт, а пока вот так.

Подадрес 0x09 (адреса в памяти 0x120..0x13F) - изображение
Штука своеобразная. Сейчас в макете у меня есть мегабайт статической памяти, где я сохраняю один кадр, после чего могу медленно и печально его передать на компьютер, убедиться, что алгоритм обнаружения "в железе" работает ровно также, как он же в модели на компьютере.

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

Впрочем, идея использовать для этого буфер на 32 слова - не самая удачная. Таким макаром, нам понадобится 16384 кадра, чтобы наконец-то передать всё изображение целиком. При 25 кадрах в секунду, мы почти 11 минут будем ждать получения изображения, это как-то совсем грустно.

Видимо, придётся-таки ввести отладочный режим работы, "получение изображения", когда вся имеющаяся оперативная память освобождается под хранение фрагмента изображения. Если этой памяти будет килобайт, то за раз сможем сохранять аж по строке изображения, т.е за 1024 кадра завершим передачу. Это 41 секунда, уже чуточку лучше :)

Давайте посчитаем, а за сколько времени, минимум, позволяет это сделать сам протокол МКО (Mil-Std 1553). Командное слово: 20 мкс. За ним должна последовать пауза 2 мкс, затем ответное слово, 20 мкс, и уже после этого слова данных, 32 раза по 20 мкс. Затем желательно ещё выдержать паузу в 2 мкс, прежде чем посылать следующее командное слово. Итого, мы тратим 684 мкс на передачу 32 слов данных, вот только там ещё идёт CRC, заголовок массива и номер пакета, итого полезных слов 29, или 58 пикселей! Если они передаются за 684 мкс, то 1024*1024 пикселя будут переданы за 12,4 секунды. Так, навскидку, нам бы 4 килобайта оперативы - и примерно такую скорость мы наконец-то получим :)

Ладно, а пока заполним этот подадрес как положено, а что с этим делать - задумаемся позже...
;подадрес 0x09 (адреса в памяти 0x120..0x13F) - изображение
ORG 0x120
IM_header	dw	0x55AA
IM_count	dw	?		;номер передаваемого фрагмента
IM_data		dw	29 DUP(?)
IM_CRC		dw	?


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

Подадрес 0x0A (адреса в памяти 0x140..0x15F) - дамп памяти
Тоже сообщение чисто отладочное. Вообще, есть идея возложить исполнение этого дампа непосредственно на протокольный контроллер, у него "всё необходимое" для этого есть. В смысле, он может обращаться к памяти по любому адресу, вот пусть и обращается, и получает. Другой вариант - при получении этого сообщения вызывать некое прерывание, чтобы процессор занялся одним-единственным делом - приготовлением этого самого дампа. Может, вообще по любому из прерываний он сначала записывает некую информацию, помогающую понять, как мы дошли до жизни такой - а потом только и занимается тем, что в адреса 0x140..015F запихивает попеременно всё содержимое памяти, кусочек за кусочком (точно так же, как и с изображением - один прочитали - следующий берём). Так что на всякий случай и здесь всё подготовим:

;подадрес 0x0A (адреса в памяти 0x140..0x15F) - дамп памяти
ORG 0x140
DU_header	dw	0x33CC
DU_count	dw	0		;номер передаваемого фрагмента
DU_data		dw	29 DUP(?)
DU_CRC		dw	?


Подадрес 0x1B (адреса в памяти 0x160..0x17F) - НЕ ИСПОЛЬЗУЕТСЯ, т.к в двоичной записи это 11011, всего одним битом отличается от 11111 - подадреса "режима управления". Что сюда "набить" - пока не придумали, но было б место - а что туда положить, всегда найдём. Так-то нам ещё связанный список обнаруженных пятен необходим, он весьма прожорливый. По самым минимальным оценкам, нужно иметь место под 32 пятна, на каждое по 4..5 слов, эдак мы сразу 4-5 неиспользуемых "подадресов" займём.

Или давайте, раз уж тут "изолированный" закуток в 32 слова, запишем сюда 32 последних показания дальности, которые будут использоваться для вычисления скорости сближения:
;например, всунем сюда последние 32 отсчёта дальности, для вычисления скорости сближения
ORG	0x160
DistA	dw	32 DUP(?)


Как именно это должно происходить - пока не шибко думал, может самое время поразмыслить...

Подадрес 0x0C (адреса в памяти 0x180..0x19F) - сырые данные для ПЕРЕДАЧИ на соседний прибор в стереорежиме
Просто повторим описание сырых данных на ПРИЁМ, только префикс заменим с RR (Raw Receive) на RT (Raw Transmit):
;подадрес 0x0C (адреса 0x180..0x19F) - сырые данные для соседнего комплекта, для стереорежима
ORG	0x180
RT_header	dw	0x5A5A	;заголовок массива

;тестовые данные: дистанция 300 метров, все углы нулевые, получили "вне времени"
RT_PRF_NO	dw	0xFFFF	;метка времени (корректные значения от 0x0000 до 0xC800)

RT_Count	dw	4		;количество обнаруженных точек
RawPoints2D:
RT_x0		Int16	53	
RT_y0		Int16	-7
RT_x1		Int16	480	
RT_y1		Int16	-30
RT_x2		Int16	-539	
RT_y2		Int16	-302
RT_x3		Int16	400	
RT_y3		Int16	-997
RT_x4		Int16	-32768
RT_y4		Int16	?
RT_x5		Int16	-32768
RT_y5		Int16	?
RT_x6		Int16	-32768
RT_y6		Int16	?
RT_x7		Int16	-32768
RT_y7		Int16	?
RT_x8		Int16	-32768
RT_y8		Int16	?
RT_x9		Int16	-32768
RT_y9		Int16	?
RT_x10		Int16	-32768
RT_y10		Int16	?


Подадреса 0x1D, 0x1E, 0x1F (адреса 0x1A0 .. 0x1FF) - НЕ ИСПОЛЬЗУЮТСЯ
Два из них отличаются от подадреса режима управления одним битом,а третий - ЭТО И ЕСТЬ подадрес режима управления! Итого, здесь у нас прорва незадействованной памяти. По сути, 4 неиспользуемых подадреса подряд, т.к двинешься дальше 0x1FF - попадёшь в 0x000, а там тоже память у нас свободна была.

Хотя я так сходу и не скажу, для каких целей нам нужна свободная память "одним куском" :) Ну, для стека разве что, хотя до сих пор мы использовали стек очень экономно - у нас и вложенных вызовов процедур раз два и обчёлся (и ни разу не игрались с рекурсией), и временные данные на стеке храним по 1-2 значения, ну максимум 4 при умножении кватернионов. Ладно, впишем стек и пока успокоимся:

Stack dw	?,?,?


ВСЁ! Память, требуемую для информационного обмена, разметили!

Программа хотя бы компилится и выдаёт такую вот простыню листинга памяти. Хотел его привести, но козёл Фрэнк не даёт, "запись слишком большая".



На удивление долго память размечал. Вроде бы должен был "на автомате" практически сделать, но всё сомнения одолевали. Маленький милый поросёнок как-то внезапно превратился в ОГРОМНОГО КАБАНА! Мне по душе задачки поменьше, где можно как-то хитро извернуться и красиво решить, где всё очерчено хорошо. Но что делать, когда всё это соединяешь вместе, хочешь-не хочешь, получится толсто.

Теперь ещё есть желание переделать вход IN с UART, чтобы это не UART был как таковой, а "отмашка" от протокольного контроллера, сигнал к началу работы. OUT вообще "выкинуть" - и попробовать всё это запустить на железе, вдруг прям возьмёт и заработает? Это будет очень подозрительно...
Tags: ПЛИС, зверушки, программки, работа, странные девайсы
Subscribe

Recent Posts from This Journal

  • Тестируем 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 

  • 3 comments