nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Испытываем новую команду "S"

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

Теперь надо попробовать ею воспользоваться, и для начала я хочу переписать кусочек кода в процедуре FindRoll (найти крен). Там мы нашли синус и косинус угла крена, сокращённо si и co, и хотим превратить их в кватернион поворота по крену, по сути в синус и косинус половинного угла, не прибегая к хитрючей тригонометрии. К счастью, соответствующий "рецепт" уже был у нас в ликбезе:

Если co>0, то


в противном случае



и затем кватернион нормируется. Длина ненормированного кватерниона будет от 0,707 (один на корень из двух) до 1, так что итераций для нормировки нужно не шибко много, хватит 4..5, чтобы выйти на полную точность, доступную в 16 битах.

Значение co у нас лежало в [Z], si в [Z+1], компоненты кватерниона мы хотим запихать в [X] и [X+1], для чего применяли такой вот упоротый код:

	;i=j=k=Inv=0				
	ABS	[Z+k]			
	DIV2	Acc
	ADD	16384
	;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
	JGE	@@skip
	i	1	;выходит i=S
@@skip:	X	DA_Quat0
	[X+i]	Acc
	j	1
	DIV2	[Z+1]
	[X+i^j]	Acc	;по сути, Y+(~S)


Нужно его немножко упростить!


Ровно на одну строчку. Мы очень уж хитро присваивали i=S, через условный прыжок, пропускающий строку "i 1". Взамен напишем так:

ABS	[Z+k]			
DIV2	Acc
ADD	16384
;флаг S (sign) сейчас показывает знак co, что для нас очень полезно
i	S
X	DA_Quat0
[X+i]	Acc
j	1
DIV2	[Z+1]
[X+i^j]	Acc			;по сути, X+(~S)


заодно подправили комментарий к нижней строчки, там вместо X указывался Y.

Программа скомпилировалась и стала занимать на 1 слово меньше, чем раньше: 309 вместо 310. Это алгоритмы захвата на ближней и дальней дистанции, процедура SetClock, выбор между ближней и дальней дистанцией в зависимости от количества точек, формирование признака "захват", и теперь ещё в придачу процедура atan1.

Но проверить хочется не только наш "примитивный частный случай", когда крен нулевой, но и когда картинка развёрнута на 180 градусов. Давайте для этого в кои-то веки сформируем сообщение "сырые данные". Сначала сообразим, какие слова данных нужны:
1. заголовок массива, 0x5A5A,
2. метка времени, пофиг пока, пусть 0x0000,
3. количество обнаруженных точек, 0x0004, но с компьютера надо будет "вбить" 0x0400 (классическая подлянка с Endian)
4. сами точки. Когда-то я этот "сценарий" описывал непосредственно в ассемблерном файле:
;дистанция 300 метров, поворот по крену 180 градусов
Fx0		Int16	-393
Fy0		Int16	994
Fx1		Int16	547
Fy1		Int16	299
Fx2		Int16	-474
Fy2		Int16	26
Fx3		Int16	-45
Fy3		Int16	0


В Hex это получится 0xFE77, 0x03E2, 0x0223, 0x012B, 0xFE26, 0x001A, 0xFFD3, 0x0000.

Но не забываем все старшие и младшие байты поменять местами при передаче с компьютера через RS485, выходит 0x77FE E203 2302 2B01 26FE 1A00 D3FF 0000.

5. контрольная сумма от всего этого безобразия, 0xA2C5.

Всего слов данных получается 12 (заголовок+метка+количество+8 координат+CRC) = 0x0C. Поэтому командное слово: 0x30AC (адрес 6, передача КШ-ОУ, подадрес 5, количество СД: 12), которое мы по RS485 должны передать как 0xAC30.

Итого, в Termite вдалбливается такая строка: 0xAC305A5A0000040077FEE20323022B0126FE1A00D3FF0000A2C5.

Попробуем проверить..
Синтез заканчивается с Critical Warning - Timing requirements not met, предельная частота 24,81 МГц. Но при комнатной температуре и номинальном напряжении питания должно работать...

Рискнём всё же запустить:


По самым первым тестовым данным (300 метров, крен 0, корабли строго друг напротив друга, хотя наш приборчик чуть в стороне, поэтому в точности нулевые координаты получаться и не должны) получаем те же координаты, что ранее, а вот кватернион криво вышел. Там, где мы ожидали a = 0x7FFF, x = 0, вышло всё наоборот: a = 0, x = 0x7FFF, что означает разворот на 180 градусов.

Нашу длиннющую посылку прибор принял (ответил 0x3000, что значит "всё хорошо"), и после обработки получил a = 0xFFFF = -1, x = 0x7FFF = 32767. Практически то же самое, разворот на 180 градусов, но тут это правильно.

Что ж, надо разобраться, что пошло не так. Запустим на симуляции и посмотрим содержимое памяти в конце работы:


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

Раз уж сделали симуляцию, надо посмотреть непосредственный момент применения этой команды. Но сначала ещё одно наблюдение. Посмотрим вызов NormSiCo как раз для нормировки этих значений, синуса и косинуса от угла крена:


Исходные значения: 0 и 0x1B6 = 438, при диапазоне -32768..+32767. Т.е значения совсем маленькие, это и ясно - эти числа выражают также масштаб, а он сейчас минимальный, мы находимся в 300 метрах от объекта. Меньше уже не ожидается.

И теперь смотрим нашу злосчастную команду:


Листинг данного участка:
06F  F3B8  CALL    NormSiCo
070  84E8  ABS     [Z+k]           
071  8C80  DIV2    Acc
072  8228  ADD     16384
073  A081  i       S
074  CD29  X       DA_Quat0
075  C480  [X+i]   Acc
076  A104  j       1
077  8CE0  DIV2    [Z+1]
078  CC80  [X+i^j] Acc         ;по сути, X+(~S)


Первая проблема: значения 438 и 0 должны были отнормироваться в 32767=0x7FFF и 0. Мы же на входе, сразу после нормировки, получаем 0x7
FCE = 32718. То есть, буквально одной итерации не хватило, чтобы отнормировать до конца. Да, об этом я из прошлого тоже предупреждал себя сегодняшнего. Что ж, в NormSiCo строку
j   12


заменяем на
j   13


Такая неточная нормировка приводила к неверному определению дальности, т.к эти si/co мы использовали, чтобы "убрать" крен из матрицы аффинного преобразования.

Возвращаемся к нашему главному блюду, команде "S". Код команды: 0x81 на SrcAddr, мы обвели её синим. Значение подаётся на шину данных DataBus на следующий такт, его мы тоже выделили цветом: 0x0001. НЕПРАВИЛЬНО! У нас флаг знака нулевой. Причём, учитывая, что в ситуации с реальным поворотом на 180° ответ был правильным, команда S попросту всегда выдаёт 0x0001...

Что ж, ещё раз посмотрим код, отвечающий за младший разряд:

//младший бит
wire mode0lsb = (SrcAddr[1:0] == 2'b00)? Acc[15] & ~Acc[16]:
		(SrcAddr[1:0] == 2'b01)? S:	//непосредственно флаг знака сюда засунется
		(SrcAddr[1:0] == 2'b10)? 1'b0:
					 1'b1;

wire [1:0] LsbMode = {mode[1], mode0lsb};

assign Q[0] = 	(MsbMode == 2'b00)? 1'b0 :
		(MsbMode == 2'b01)? 1'b1 :
		(MsbMode == 2'b10)? Acc[0] :
				C[0];


СЕМЁН СЕМЁНЫЧ! Сформировал сигнал LsbMode, а применил в итоге MsbMode, который для старшего разряда... Дурацкая ошибка. Исправляем:

//младший бит
wire mode0lsb = (SrcAddr[1:0] == 2'b00)? Acc[15] & ~Acc[16]:
		(SrcAddr[1:0] == 2'b01)? S:	//непосредственно флаг знака сюда засунется
		(SrcAddr[1:0] == 2'b10)? 1'b0:
					 1'b1;

wire [1:0] LsbMode = {mode[1], mode0lsb};

assign Q[0] = 	(LsbMode == 2'b00)? 1'b0 :
		(LsbMode == 2'b01)? 1'b1 :
		(LsbMode == 2'b10)? Acc[0] :
				C[0];


Ещё раз глянем симуляцию:


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

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


Да, всё правильно.

И теперь повторно компилим программу (чтобы добавилась одна итерация в NormSiCo) и запускаем на макете:




Уже лучше. Первый раз кватернион 32767 + 0i, т.е кватернион нулевого поворота. Второй раз -1+32767i, это практически поворот на 180 градусов, как и было в этом "сценарии".

Ещё показания дальности изменились после добавления одной итерации в NormSiCo, теперь "мантисса" 0x953A = 38202, а дальность 38202/32768 * 29-1 = 298,45 метров. Логично: у нас до этого нормировка до конца не доходила, поэтому масштаб выходил меньше и соответствовал большей дальности. Бывает и так: я координаты точек когда-то "сочинял", пытаясь учесть все проблемы, которые могут возникнуть с фотоприёмной матрицей: неравномерность чувствительности, "мёртвые зоны" и пр., может при правильных вычислениях и должно было получиться так...

В следующий раз громко думаем над знаками для FMPM, после чего тестируем арктангенс.
Tags: ПЛИС, программки, работа, странные девайсы
Subscribe

  • Лестница для самых жадных

    В эти выходные побывал на даче, после 3-недельной "самоизоляции". Забавно, как будто зима началась! Особенно грязные галоши остались на улице, в…

  • Возвращаемся к макету

    Очень давно макетом видеоизмерителя параметров сближения не занимался: сначала "громко думал" по поводу измерения его положения на аппарате, а потом…

  • Минутка живописи

    В процессе разгребания содержимого квартиры (после нескольких ремонтов) дошёл, наконец, и до картин. В кои-то веки их повесил. Куда их вешать -…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments