.comm iXB10,2,1 ;speicher fuer 1. Bandpass Q14 .comm iXB11,2,1 .comm iXB12,2,1 .comm iYB10,2,1 .comm iYB11,2,1 .comm iYB12,2,1 .comm iBA10,2,1 ;Faktoren fuer 1. Bandpass Q14 .comm iBA11,2,1 .comm iBA12,2,1 .comm iBB10,2,1 .comm iBB11,2,1 .comm iBB12,2,1 .comm iXB20,2,1 ;speicher fuer 1. Bandpass Q14 .comm iXB21,2,1 .comm iXB22,2,1 .comm iYB20,2,1 .comm iYB21,2,1 .comm iYB22,2,1 .comm iBA20,2,1 ;Faktoren fuer 2. Bandpass Q14 .comm iBA21,2,1 .comm iBA22,2,1 .comm iBB20,2,1 .comm iBB21,2,1 .comm iBB22,2,1 ;************************************************************************************************* ; ; STOMAC MAC Speichern ; ;************************************************************************************************* .macro STOMAC Reg1 sts \Reg1,R14 sts \Reg1+1,R15 .endm ;************************************************************************************************* ; ; CLRMAC MAC loeschen ; ;************************************************************************************************* .macro CLRMAC clr R16 mov R14,R16 mov R15,R16 .endm ;************************************************************************************************* ; ; MOVDATA Reg1,Reg2 kopiert eine Int von Reg2 nach Reg1 ; ;************************************************************************************************* .macro MOVDATA Reg1 Reg2 lds r16,\Reg2 sts \Reg1,r16 lds r16,\Reg2+1 sts \Reg1+1,r16 .endm ;************************************************************************************************* ; ; MAC14 Reg1,Fac1 ; multipliziert das Register mit dem Factor und addiert es auf dem Akku (R14,R15) ; ;************************************************************************************************* .MACRO MAC14 Reg1 Fac1 ;Multiply and Add lds R22,\Reg1 ;Load Value lds R23,\Reg1+1 lds R20,\Fac1 ;load Constant lds R21,\Fac1+1 rcall Q14Mul ;Multiply add R14,R16 ;Addition adc R15,R17 .endm ;************************************************************************************************* ; ; MSC14 Reg1,Fac1 ; multipliziert das Register mit dem Factor und subtraiert es von dem Akku (R14,R15) ; ;************************************************************************************************* .MACRO MSC14 Reg1 Fac1 ;Multiply and Add lds R22,\Reg1 ;Load Value lds R23,\Reg1+1 lds R20,\Fac1 ;load Constant lds R21,\Fac1+1 rcall Q14Mul ;Multiply sub R14,R16 ;subtraction sbc R15,R17 .endm ;************************************************************************************************* ; ; READADC ADC auslesen und 2.er Komplement bilden und *4 nehmen fuer Q14 Format ; Q14 = Sign Vorkomma 14 Nachkommastellen ; ;************************************************************************************************* .macro READADC Reg1 lds R16,ADCL lds R17,ADCH subi R16,lo8(512) sbci R17,hi8(512) lsl R16 rol R17 lsl R16 rol R17 lsl R16 rol R17 lsl R16 rol R17 sts \Reg1,R16 sts \Reg1+1,R17 .endm ;****************************************************************************************************** ; ; BandPass ; Bandpass 4. Ordnung berechnen ; ;****************************************************************************************************** BandPass: MOVDATA iXB12,iXB11 ;x[2] = x[1] MOVDATA iXB11,iXB10 ;x[1] = x[0] MOVDATA iYB12,iYB11 ;y[2] = y[1] MOVDATA iYB11,iYB10 ;y[1] = y[0] MOVDATA iXB10,iFILTERIN ;Neuer Eingangswert CLRMAC ;Mac loeschen MAC14 iXB10,iBA10 ;y[0] += Q14Mul(x[0],a0) MAC14 iXB12,iBA12 ;y[0] += Q14Mul(x[2],a2) MSC14 iYB11,iBB11 ;y[0] -= Q14Mul(y[1],b1) MSC14 iYB12,iBB12 ;y[0] -= Q14Mul(y[2],b2) STOMAC iYB10 ;Wert abspeichern ;************** 2. Teil Bandpass berechnen *********************************************************** MOVDATA iXB22,iXB21 MOVDATA iXB21,iXB20 MOVDATA iYB22,iYB21 MOVDATA iYB21,iYB20 MOVDATA iXB20,iYB10 CLRMAC ;Mac loeschen MAC14 iXB20,iBA20 ;y[0] += Q14Mul(x[0],a0) MAC14 iXB22,iBA22 ;y[0] += Q14Mul(x[2],a2) MSC14 iYB21,iBB21 ;y[0] -= Q14Mul(y[1],b1) MSC14 iYB22,iBB22 ;y[0] -= Q14Mul(y[2],b2) STOMAC iYB20 ;Wert abspeichern STOMAC iFILTEROUT ;Filter wert liegt jetzt in FILTEROUT ret ;und fertig ;****************************************************************************************************** ; ; Q14Mul ; Fractional Multiplikation ; r17:r16 = r23:r22 * r21:r20 ; Hi Lo Hi Lo Hi Lo ; (R23:R22 * R21:R20) >> 14 ; Aufbau der Zahlen ; SV.NNNNNNNNNNNNNN ; ^^ ^^^^^^^^^^^^^^ ; || -------------- Nachkommateil ; |---------------- Vorkommateil ; ----------------- Vorzeichen ; Bereich: (-2 .. +2] ; ;****************************************************************************************************** Q14Mul: rcall muls16x16_32 ;Multiplikation ausfuehren lsl r17 ;ergebnis um 14 bit nach rechts schieben rol r18 rol r19 lsl r17 rol r18 rol r19 mov r16,r18 mov r17,r19 ret ;******************************************************************************************************** ;* ;* FUNCTION ;* muls16x16_32 ;* DECRIPTION ;* Signed multiply of two 16bits numbers with 32bits result. ;* USAGE ;* r19:r18:r17:r16 = r23:r22 * r21:r20 ;* Hi Lo Hi Lo Hi Lo ;* STATISTICS ;* Cycles : 19 + ret ;* Words : 15 + ret ;* Register usage: r0 to r2 and r16 to r23 (11 registers) ;* NOTE ;* The routine is non-destructive to the operands. ;* ;******************************************************************************************************** muls16x16_32: clr r2 muls r23, r21 ; (signed)ah * (signed)bh mov r19,r1 mov r18,r0 mul r22, r20 ; al * bl mov r17,r1 mov r16,r0 mulsu r23, r20 ; (signed)ah * bl sbc r19, r2 add r17, r0 adc r18, r1 adc r19, r2 mulsu r21, r22 ; (signed)bh * al sbc r19, r2 add r17, r0 adc r18, r1 adc r19, r2 ret