www.pudn.com > dsPIC33EP512MU810.zip > pi.s, change:2011-03-28,size:6013b


; ****************************************************************************** 
;  2009 Microchip Technology Inc. 
;  
; SOFTWARE LICENSE AGREEMENT: 
; Microchip Technology Incorporated ("Microchip") retains all ownership and  
; intellectual property rights in the code accompanying this message and in all  
; derivatives hereto.  You may use this code, and any derivatives created by  
; any person or entity by or on your behalf, exclusively with Microchip's 
; proprietary products.  Your acceptance and/or use of this code constitutes  
; agreement to the terms and conditions of this notice. 
; 
; CODE ACCOMPANYING THIS MESSAGE IS SUPPLIED BY MICROCHIP "AS IS".  NO  
; WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED  
; TO, IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY AND FITNESS FOR A  
; PARTICULAR PURPOSE APPLY TO THIS CODE, ITS INTERACTION WITH MICROCHIP'S  
; PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION.  
; 
; YOU ACKNOWLEDGE AND AGREE THAT, IN NO EVENT, SHALL MICROCHIP BE LIABLE, WHETHER  
; IN CONTRACT, WARRANTY, TORT (INCLUDING NEGLIGENCE OR BREACH OF STATUTORY DUTY),  
; STRICT LIABILITY, INDEMNITY, CONTRIBUTION, OR OTHERWISE, FOR ANY INDIRECT, SPECIAL,  
; PUNITIVE, EXEMPLARY, INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, FOR COST OR EXPENSE OF  
; ANY KIND WHATSOEVER RELATED TO THE CODE, HOWSOEVER CAUSED, EVEN IF MICROCHIP HAS BEEN  
; ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE.  TO THE FULLEST EXTENT  
; ALLOWABLE BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO  
; THIS CODE, SHALL NOT EXCEED THE PRICE YOU PAID DIRECTLY TO MICROCHIP SPECIFICALLY TO  
; HAVE THIS CODE DEVELOPED. 
; 
; You agree that you are solely responsible for testing the code and  
; determining its suitability.  Microchip has no obligation to modify, test,  
; certify, or support the code. 
; 
; ****************************************************************************** 
;******************************************************************* 
; PI 
;   
;Description:  Calculate PI correction. 
; 
;void CalcPI( tPIParm *pParm) 
;{ 
;    Err  = InRef - InMeas 
;    U  = Sum + Kp * Err 
;    if( U > Outmax ) 
;        Out = Outmax 
;    else if( U < Outmin ) 
;        Out = Outmin 
;    else         
;        Out = U  
;    Exc = U - Out 
;    Sum = Sum + Ki * Err - Kc * Exc 
;} 
; 
; 
;void InitPI( tPIParm *pParm) 
;{ 
;    Sum = 0 
;    Out = 0 
;} 
; 
;---------------------------- 
; Representation of PI constants: 
; The constant Kp is scaled so it can be represented in 1.15 format by  
; adjusting the constant by a power of 2 which is removed when the 
; calculation is completed. 
; 
; Kp is scaled Kp = qKp * 2^NKo  
; 
; Ki & Kc are scaled Ki = qKi, Kc = qKc 
; 
; 
;Functional prototype: 
;  
; void InitPI( tPIParm *pParm) 
; void CalcPI( tPIParm *pParm) 
; 
;On Entry:   PIParm structure must contain qKp,qKi,qKc,qOutMax,qOutMin, 
;                                           InRef,InMeas 
;On Exit:    PIParm will contain qOut 
; 
;Parameters:  
; Input arguments: tPIParm *pParm 
; 
; Return: 
;   Void 
; 
; SFR Settings required: 
;         CORCON.SATA  = 0 
;         CORCON.IF    = 0 
; 
; Support routines required: None 
; 
; Local Stack usage: 0 
; 
; Registers modified: w0-w6,AccA 
; 
; Timing: 
;  31 instruction cycles max, 28 cycles min 
; 
;******************************************************************* 
; 
 
; External references 
          .include "PI.inc" 
 
; Register usage 
 
          .equ BaseW0,   w0  ; Base of parm structure 
 
          .equ OutW1,    w1  ; Output 
          .equ SumLW2,   w2  ; Integral sum 
          .equ SumHW3,   w3  ; Integral sum 
 
          .equ ErrW4,    w4  ; Error term: InRef-InMeas 
          .equ WorkW5,   w5  ; Working register 
          .equ UnlimitW6,w6  ; U: unlimited output  
          .equ WorkW7,   w7  ; Working register 
 
 
;=================== CODE ===================== 
 
          .section  .text 
 
          .global   _InitPI 
          .global   InitPI 
_InitPI: 
InitPI: 
          mov.w     w1,[BaseW0+PI_qOut] 
          return 
 
 
          .global   _CalcPI 
          .global   CalcPI 
 
_CalcPI: 
CalcPI: 
     ;; Err  = InRef - InMeas 
 
          mov.w     [BaseW0+PI_qInRef],WorkW7 
          mov.w     [BaseW0+PI_qInMeas],WorkW5 
          sub.w     WorkW7,WorkW5,ErrW4 
           
     ;; U  = Sum + Kp * Err * 2^NKo 
          lac       [++BaseW0],B               ; AccB = Sum           
          mov.w     [--BaseW0],WorkW5 
          mov.w     WorkW5,ACCBL 
 
          mov.w     [BaseW0+PI_qKp],WorkW5 
          mpy       ErrW4*WorkW5,A 
          sftac     A,#-NKo                  ; AccA = Kp*Err*2^NKo      
          add       A                        ; Sum = Sum + Kp*Err*2^NKo 
          sac       A,UnlimitW6              ; store U before tests 
 
     ;; if( U > Outmax ) 
     ;;     Out = Outmax 
     ;; else if( U < Outmin ) 
     ;;     Out = Outmin 
     ;; else         
     ;;     Out = U  
 
          mov.w     [BaseW0+PI_qOutMax],OutW1 
          cp        UnlimitW6,OutW1 
          bra       GT,jPI5            ; U > Outmax; OutW1 = Outmax 
 
          mov.w     [BaseW0+PI_qOutMin],OutW1 
          cp        UnlimitW6,OutW1 
          bra       LE,jPI5            ; U < Outmin; OutW1 = Outmin 
 
          mov.w     UnlimitW6,OutW1         ; OutW1 = U 
jPI5: 
          mov.w     OutW1,[BaseW0+PI_qOut] 
 
     ;; Ki * Err 
          mov.w     [BaseW0+PI_qKi],WorkW5 
          mpy       ErrW4*WorkW5,A 
 
     ;; Exc = U - Out 
          sub.w     UnlimitW6,OutW1,UnlimitW6 
 
     ;; Ki * Err - Kc * Exc  
          mov.w     [BaseW0+PI_qKc],WorkW5 
          msc       WorkW5*UnlimitW6,A 
 
     ;; Sum = Sum + Ki * Err - Kc * Exc  
          add       A 
           
          sac       A,[++BaseW0]             ; store Sum  
          mov.w     ACCAL,WorkW5 
          mov.w     WorkW5,[--BaseW0] 
          return 
 
          .end