Форум РадиоКот

Код: Выделить всё

; 3. для частоты 300 Гц кол-во выборок: 38400 / 300 = 128
; используемые регистры
;.def mulres_lo	= R0
;.def mulres_hi	= R1
;.def Lm2		= R15
;.def Lm1		= R16
;.def Lm0		= R17
;.def Re2		= R18
;.def Re1		= R19
;.def Re0		= R20
;.def adcv		= R21
;.def sin		= R22
;.def cos		= R23
;.def i_lo		= R24
;.def i_hi		= R25
;.def sqr_lo		= R16
;.def sqr_hi		= R17
; возвращаемый результат
; .def retval	= R20 
furie_func_300Hz:
	clr Re2							; Re = 0 (1 т)
	clr Re1							; (1 т)
	clr Re0							; (1 т)
	clr Lm2							; Lm = 0 (1 т)
	clr Lm1							; (1 т)
	clr Lm0							; (1 т)
	ldi i_lo, 3						; (1 т)
	ldi R26, LOW(adc_buf)			; x = adc_buf; (1 т)
	ldi R27, HIGH(adc_buf)			; (1 т)
	furie_func_300Hz_loop_i_lo:	; brpl - переход по положит. зн. (N == 0)
		ldi R30, LOW(sin_cos_128)		; z = cos128 (1 т)
		ldi R31, HIGH(sin_cos_128)		; (1 т)
		ldi i_hi, 127					; (1 т)
		furie_func_300Hz_loop_i_hi:
			ld adcv, X+					; adcv = *x++; (2 т)
			lpm sin, Z+					; sin = *z++; (3 т)
			lpm cos, Z+					; cos = *z++; (3 т)

			muls adcv, sin				; mulres = adcv * sin (2 т)
			; здесь надо приобразовать 2х байтный результат умножения в 3х байтный с учетом знака
			; знак в старшем байте (если он есть), проверяем
			clr mulres_3 ; в R2 - 3й байт результата умножения (1 т)
			tst mulres_hi ; если число положительное, то пропускаем (1 т)
			brpl furie_func_300Hz_loop_negative_test_1_skip ; (2 т)
				; иначе нам надо изменить 3й байт результата умножения,
				; записав в него 0xFF
				ser mulres_3 ; (1 т)
			furie_func_300Hz_loop_negative_test_1_skip:
			; теперь просто складываем 2 24х битных числа
			add Lm0, mulres_lo		; Lm += mulres (1 т)
			adc Lm1, mulres_hi		; (1 т)
			adc Lm2, mulres_3				; (1 т)

			muls adcv, cos			; mulres = adcv * cos (2 т)
			; здесь надо приобразовать 2х байтный результат умножения в 3х байтный с учетом знака
			; знак в старшем байте (если он есть), проверяем
			clr mulres_3 ; в R2 - 3й байт результата умножения (1 т)
			tst mulres_hi ; если число положительное, то пропускаем (1 т)
			brpl furie_func_300Hz_loop_negative_test_2_skip ; (2 т)
				; иначе нам надо изменить 3й байт результата умножения,
				; записав в него 0xFF
				ser mulres_3 ; (1 т)
			furie_func_300Hz_loop_negative_test_2_skip:
			; теперь просто складываем 2 24х битных числа
			add Re0, mulres_lo		; Lm += mulres (1 т)
			adc Re1, mulres_hi		; (1 т)
			adc Re2, mulres_3				; (1 т)

			dec i_hi				; (1 т)
			brpl furie_func_300Hz_loop_i_hi	; (2 т)
			; предъидущий цикл 128*31=3968 тактов
		dec i_lo					; (1 т)
		brpl furie_func_300Hz_loop_i_lo	; (2 т)
		;  предъидущий цикл 4*(3968+3+3)=15896 тактов
	
	;здесь надо Lm и Re cделать положительными
	clr i_lo	; (1 т)
	ldi i_hi, 1	; (1 т)
	tst Lm2		; (1 т)
	brpl furie_func_300Hz_negative_test_1_skip	; (2 т)
		com Lm0			; (1 т)
		com Lm1			; (1 т)
		com Lm2			; (1 т)
		add Lm0, i_hi	; (1 т)
		adc Lm1, i_lo	; (1 т)
		adc Lm2, i_lo	; (1 т)
	furie_func_300Hz_negative_test_1_skip:

	tst Re2	; (1 т)
	brpl furie_func_300Hz_negative_test_2_skip	; (2 т)
		com Re0			; (1 т)
		com Re1			; (1 т)
		com Re2			; (1 т)
		add Re0, i_hi	; (1 т)
		adc Re1, i_lo	; (1 т)
		adc Re2, i_lo	; (1 т)
	furie_func_300Hz_negative_test_2_skip:

	; Lm >>= 16; Re >>= 16;
	ldi i_lo, 16					; i = 16 (1 т)
	furie_func_300Hz_loop2:			; 
		lsr Lm2						; Lm >>= 1 (1 т)
		ror Lm1						; (1 т)
		ror Lm0						; (1 т)

		lsr Re2						; Re >>= 1 (1 т)
		ror Re1						; (1 т)
		ror Re0						; (1 т)

		dec i_lo					; i-- (1 т)
		brne furie_func_300Hz_loop2	; while (i) (2 т)
	; предъидущий цикл 1+16*9=145 тактов
	; sqr = Lm * Lm + Re * Re
	muls Lm0, Lm0					; mulres = Lm * Lm (2 т)
	movw sqr_lo, mulres_lo			; sqr = mulres (1 т)
	muls Re0, Re0					; mulres = Re * Re (2 т)
	add sqr_lo, mulres_lo			; sqr += mulres (1 т)
	adc sqr_hi, mulres_hi			; (1 т)
	; ret = qsqr(sqr);
	rcall qsqrt						; 3+344 такта
	; результат в R20
	ret								; 4 такта
; максимальное количество тактов функции 9+15896+20+145+7+3+344+4=16428 тактов
; время выполнения ~1027 мкс

Код: Выделить всё

			muls adcv, sin				; mulres = adcv * sin (2 т)
			; здесь надо приобразовать 2х байтный результат умножения в 3х байтный с учетом знака
			; знак в старшем байте (если он есть), проверяем
			clr mulres_3 ; в R2 - 3й байт результата умножения (1 т)
			tst mulres_hi ; если число положительное, то пропускаем (1 т)
			brpl furie_func_300Hz_loop_negative_test_1_skip ; (2 т)
				; иначе нам надо изменить 3й байт результата умножения,
				; записав в него 0xFF
				ser mulres_3 ; (1 т)                                     ; <----------------------ВОТ ЗДЕСЬ-------------------
			furie_func_300Hz_loop_negative_test_1_skip:
			; теперь просто складываем 2 24х битных числа
			add Lm0, mulres_lo		; Lm += mulres (1 т)
			adc Lm1, mulres_hi		; (1 т)
			adc Lm2, mulres_3				; (1 т)