sub_iirt_filter.s

该文件包含30F的电机控制程序

;START_HEADER
;
; dsPIC30F6014 Demo Source File
; (c) Copyright 2005 Microchip Technology, All rights reserved
;
; --------------------------------------------------------------------------
; File Revision History:
; --------------------------------------------------------------------------
;
; $Log: sub_iirt_filter.s,v $
; Revision 1.2  2005/04/04 23:38:49  VasukiH
; Updated comments in header
;
; Revision 1.1.1.1  2003/08/23 00:38:33  VasukiH
; First import of demo source into CVS Repository
;
;
;
; --------------------------------------------------------------------------
;
; Software and Development Tools Info:
; --------------------------------------------------------------------------
; Tool                          Version
; --------------------------------------------------------------------------
; MPLAB IDE                     7.0
; MPLAB C30 Toolsuite           1.30
; dsPICDEM(TM) Processor Board  1.10
; --------------------------------------------------------------------------
;
; File Notes:
;
; Block Cascaded Transpose IIR Implementation (cascade form I)
; This file should be assembled and linked against filter coefficients
; generated by dsPicFD -- filter design software by Momentum Data Systems.
;
; Module Re-entrancy:
; Module re-entrancy is not supported
;
; Input:  for routine   _sub_iirt_filter
;
;               w0 = pointer to filter structure
;               w1 = pointer to input  sample buffer
;               w2 = pointer to output sample buffer
;               w3 = number of output samples to generate
;
;
; System Resource usage
;       W0                      used but value on exit is same as entry
;       W1, W2, W3              used not restored
;       W4, W5, W6, W7          used not restored
;       w8, W9, w10,W11         used, saved and restored
;       accumulators a,b        used not restored
;       CORCON                  used, saved and restored
;       PSVPAG                  used, saved and restored
;
;
; Input: for _IIRTransposeFilterInit
;               w0 = pointer to filter structure
;
; System Resource usage
;       w0, w1, W2              used not restored
;
; do and repeat instruction usage
;     2 level do instruction
;     repeat instruction is not used
;
; Module Cycle Count
;     _sub_iirt_filter: 27 + N*(11 + S*11)      (+2 if PSV)
;     _IIRTransposeFilterInit:  8 + S*2
; with N: number of samples per block, S: number of sections in filter.
;
;
;END_HEADER

; offsets into IIRTransposeFilter structure
; (entries are word wide -- hence +=2)
.equ oNumSectionsLess1, 0                       ; number of second order sections - 1
.equ oCoefs,            2                       ; pointer to coefficients
                                                ; number of coefficients is
                                                ; 5 * number of second order sections
.equ oPSVpage,          4                       ; page number in program memory if
                                                ; coefficients are in program memory
                                                ; 0xFF00 if not
.equ oStates1,          6                       ; pointer to state variable 1
                                                ; one word for every section
.equ oStates2,          8                       ; pointer to state variable 1
                                                ; one word for every section
.equ oFinalShift,      10                       ; final left shift count
                                                ; restores filter gain to 0 dB
                                                ; shift count may be zero
                                                ; if not zero, it is the number of bits
                                                ; to shift the output to the left


.section .text
.global _sub_iirt_filter

_sub_iirt_filter:
         push  w8                               ; save context of w8
         push  w9                               ; save context of w9
         push  w10                              ; save context of W10
         push  w11                              ; save context of w11
         push  CORCON                           ; save context of CORCON
         push  PSVPAG                           ; save context of PSVPAG

; Check if filter coefficients are stored in Program Space or Data Space and enable
; PSV and set up PSVPAG accordingly

         mov   [w0+oPSVpage], w10
         mov   #0xFF00, w8
         cp    w8, w10                          ; perform w8-w10 (if w10 = FF00 then do not enable PSV)
         bra   z, no_psv                        ; branch if compare true (coefficients are in data space)
         mov   #0x00F4,w8                       ; Enable enable Program Space Visibility,
                                                ; Accumulator A Saturation and Data Space write Saturation
                                                ;    as bits 2, 5 and 7 are set in CORCON
                                                ; Also note bits 2, 5 and 7 are in low order byte which is
                                                ;   the first byte of a 2 byte word in a little endian
                                                ;   processor such as the dsPIC30
                                                ; also, enable unbiased (convergent) rounding mode,
                                                ; 1.39 saturation enabled, fractional mult.
         mov   w10, PSVPAG                      ; PSVPAG = Program Space page containing filter taps
         bra   SetupPointers

no_psv:
         mov   #0x00F0,w8                       ; Enable Accumulator A Saturation and
                                                ; Data Space write Saturation
                                                ;    as bits 5 and 7 are set in CORCON

; Setup pointers

SetupPointers:
         mov    w8, CORCON                      ; set PSV and saturation options

         mov    [w0+oNumSectionsLess1], w4      ; w4 = number of sections - 1
         mov    [w0+oFinalShift], w9            ; w9 = final shift count
         dec    w3, w3                          ; w3 = number of output samples -1

         do     w3, transposeBlockLoop          ; loop on the number of input samples

         mov    [w0+oCoefs], w8                 ; w8 = base address of coefs
         mov    [w1++], w6                      ; w6 = next input sample

         mov    [w0+oStates1], w10              ; w10 = base address of states1 buffer
         mov    [w0+oStates2], w11              ; w11 = base address of states2 buffer
         mov    [w8++], w5                      ; fetch first coefficient
         lac    [w10], #1, a                    ; fetch filter state

         do     w4, transposeSectionLoop        ; loop on the number of second order sections

         mac    w5*w6, a, [w8]+=2, w5
         lac    [w11], #1, b
         sac.r  a, #-1, w7
         mac    w5*w6, b, [w8]+=2, w5
         mac    w5*w7, b, [w8]+=2, w5
         sac.r  b, #-1, [w10++]
         mpy    w5*w6, b, [w8]+=2, w5
         sac.r  a, #-1, w6
         lac    [w10], #1, a
         mac    w5*w7, b, [w8]+=2, w5
transposeSectionLoop:
         sac.r  b, #-1, [w11++]

         lac    w6, a
         sftac  a, w9                           ; perform arithmetic shift

         sac.r  a, [w2]

transposeBlockLoop:                             ; round and store result in output buffer
         asr    [w2], [w2++]

         pop    PSVPAG                          ; restore context of PSVPAG
         pop    CORCON                          ; restore context of CORCON
         pop    w11                             ; restore context of w11
         pop    w10                             ; restore context of W10
         pop    w9                              ; restore context of w9
         pop    w8                              ; restore context of w8


         return                                 ; exit from _sub_iirt_filter:

;Subroutine: Filter State initialization
; Input:
;               w0 = pointer to filter structure

.section .text
.global _IIRTransposeFilterInit

_IIRTransposeFilterInit:
         mov    [w0+oStates1], w1               ; w1 = base address of states1 buffer
         mov    [w0+oStates2], w2               ; w2 = base address of states2 buffer
         mov    [w0+oNumSectionsLess1], w0      ; w0 = number of sections - 1

; initialize state buffers (i.e. fill with zeros)
         do     w0, transposeInitLoop
         clr    [w1++]
transposeInitLoop:
         clr    [w2++]

         return                                 ; exit from _IIRTransposeFilterInit:



.end