Appendix A: Digital controller implementation - sample code example

;====================================================================
	.include "Eperipheral_registers.h"
	.include "PWM Driver Macro.asm"
	.include "ADC Driver Macro.asm"
	.include "Control Law macro.asm"
;*****************************************************************************
; Controller Coefficients
;*****************************************************************************	
	.sect   "CNTL_coeff"
			
; Controller coefficients are in Q26 format. Controller output is in Q24 format. 
VCNTL_COEFF1:
	.long        816043786     ; B2
 	.long       -1805899530   ; B1
                                                 .long	        997908808     ; B0
                                                 .long	       -31749204      ; A2
                                                 .long	        98851357       ; A1
                                                 .long 	       0x00FFFFFF   ; MAX Q24
                                                 .long 	       0xFF000000    ; MIN Q24		
;*****************************************************************************
; Declare Public functions for External Reference
;*****************************************************************************
.def 	_ISR_Init
.def 	_ISR_Run
;*****************************************************************************
; Variable declaration
;*****************************************************************************	
ZeroNet	.usect "Net_terminals", 8, 1, 1
DummyNet	 .usect "Net_terminals", 8, 1, 1
			.text
;****************************************************************************
_ISR_Init:	
;****************************************************************************
HRBUCK_DRV_INIT 1
ADC2CH_DRV_INIT
CNTL_2P2Z_INIT  1, VCNTL_COEFF1
LRETR

;***************************************************************************
_ISR_Run:	
;***************************************************************************
CONTEXT_SAVE		; Call macro. Save context.	
ADC2CH_DRV		; Call macro. Read ADC result for output voltage
CNTL_2P2Z  1		; Call macro. Implement digital volt mode controller
HRBUCK_DRV  1		; Call macro. Generate High Resolution PWM.
			
; Interrupt management before exit. This is the case for ISR triggered by ADC 

MOVW  DP, #ADCST>>6
MOV  @ADCST, #0x010		; Clear INT SEQ1 Int flag
MOVW  DP, #PIEACK>>6		; Acknowledge PIE interrupt Group 1
MOV  @PIEACK, #PIEACK_GROUP1
CONTEXT_REST		; Call macro. Restore context.
IRET						
;====================================================================
;	CONTROLLER Macro module - 2 Pole, 2 Zero compensation
;====================================================================
; Implements a second order difference equation of the form:
;
;                        B2 z^2 + B1 z + B0
; U(z)/E(z) = ---------------------------------------
;                       -A2 z^2 - A1 z + 1	
;
; U(n)= A1*U(n-1)+A2*U(n-2)+B0*E(n)+B1*E(n-1)+B2*E(n-2)
;
; where,  U(n-1) => 1 sample old output , U(n-2) => 2 sample old output
; E(n)   => present error, E(n-1) => 1 sample old error, E(n-2) => 2 sample old error
;
;Controller data buffer format
;                                                               |---------|
;	| u(n-1) |0
;                                                               |---------|
;	| u(n-2) |2
;                                                               |---------|
;                                                               |  e(n)    |4
;                                                               |---------|
;	| e(n-1) |6
;                                                               |---------|
;	| e(n-2) |8
;                                                               |---------|	
;====================================================================
CNTL_2P2Z_INIT	.macro x, y
;====================================================================
; Variable Declarations
_CNTL_2P2Z_Ref:x:	.usect "Net_terminals",2,1,1	; Input Terminal 1
_CNTL_2P2Z_Fdbk:x:	.usect "Net_terminals",2 	; Input Terminal 2
_CNTL_2P2Z_Out:x:	.usect "Net_terminals",2 	; Output Terminal
CNTL_2P2Z_DBUFF:x: 	.usect "Net_terminals",10	; "Local" memory (history) 
CNTL_2P2Z_temp:x:	.usect "Net_terminals",2	; "Local" memory
CNTL_2P2Z_COEFF:x:	.usect "Net_terminals",14	; "Local" memory (constants)



; Publish Terminal Pointers for access from the C environment (optional)
;====================================================================
.def  	_CNTL_2P2Z_Ref:x:
.def 	_CNTL_2P2Z_Fdbk:x:
.def 	_CNTL_2P2Z_Out:x:

; Actual macro code starts here. Set terminal to point to ZeroNet

MOVL  XAR2, #ZeroNet
MOVW  DP, #_CNTL_2P2Z_Ref:x:
MOVL  @_CNTL_2P2Z_Ref:x:, XAR2
MOVL  @_CNTL_2P2Z_Fdbk:x:, XAR2
MOVL  XAR2, #DummyNet
MOVL  @_CNTL_2P2Z_Out:x:, XAR2		; Zero the Data Buffer
MOVL  XAR2,#CNTL_2P2Z_DBUFF:x:
RPT #4 		; 5 times
||MOV  *XAR2++, #0		; Load Coefficients & Saturation limits
MOVL   XAR2, #:y:
MOVL  XAR3, #CNTL_2P2Z_COEFF:x:
MOV  @AR0, #13 		;loop 14 times
CNTL_2P2Z_LOOP:x:
MOV ACC, *XAR2++
MOV  *XAR3++, AL
BANZ  CNTL_2P2Z_LOOP:x:, AR0--

.endm

;=================================================================
CNTL_2P2Z  .macro x
;=================================================================
MOVW DP, #_CNTL_2P2Z_Ref:x:
MOVL  XAR4, @_CNTL_2P2Z_Ref:x:		; Net pointer to reference voltage (XAR4)
MOVL  XAR5, @_CNTL_2P2Z_Fdbk:x:		; Net pointer to feedback (XAR5)
MOVL  XAR6, @_CNTL_2P2Z_Out:x:		; Net pointer to controller output (XAR6)
MOVL    XAR7, #CNTL_2P2Z_COEFF:x:		; Local coefficient pointer (XAR7)
	.
SETC    SXM,  OVM 
MOV  ACC, *XAR4		;Q15
SUB  ACC, *XAR5		;Q15
LSL  ACC, #16		;Q31, ACC=e(n)

; Implement the difference equation

MOVL  @CNTL_2P2Z_DBUFF:x:+4, ACC  	; Save e(n) in the DBUFF
MOVL  XT, @CNTL_2P2Z_DBUFF:x:+8		; XT=e(n-2),Q31
QMPYL  ACC, XT, *XAR7++		; b2*e(n-2),Q26*Q31(64-bit result)
 MOVDL   XT, @CNTL_2P2Z_DBUFF:x:+6    	; XT=e(n-1), e(n-2)=e(n-1)
 QMPYL  P, XT, *XAR7++ 		; ACC=b2*e(n-2) P=b1*e(n-1),Q26*Q31
 ADDL  ACC, P		; 64-bit result in Q57, So ACC is in Q25
MOVDL  XT, @CNTL_2P2Z_DBUFF:x:+4	; XT=e(n), e(n-1)=e(n)
QMPYL  P, XT, *XAR7++		; ACC=b2*e(n-2)+b1*e(n-1), 
	                                                                   ;P=b0*e(n),Q26*Q31, 64-bit result, ACC in Q25
ADDL  ACC, P		; ACC=b2*e(n-2)+b1*e(n-1)+b0*e(n), Q25	
SFR  ACC,#1
MOVL  @CNTL_2P2Z_temp:x:, ACC		; Q24
MOVL  XT, @CNTL_2P2Z_DBUFF:x:+2		; XT=u(n-2),Q24
QMPYL  P, XT, *XAR7++		; P=a2*u(n-2), Q26*Q24
 MOVDL   XT,@CNTL_2P2Z_DBUFF:x:     	; XT=u(n-1), u(n-2)=u(n-1),Q24
 QMPYL  ACC, XT, *XAR7++    		; ACC=a2*u(n-2)
 		; 64-bit result in Q50, So ACC is in Q18
 ADDL  ACC, P		; ACC=a1*u(n-1)+a2*u(n-2),ACC in Q18
 LSL  ACC,#5		; Q23
ADDL  ACC, ACC		; Q24
 ADDL  ACC, @CNTL_2P2Z_temp:x:  		; Q24,
	                                                ;ACC=a1*u(n-1)+a2*u(n-2)+b2*e(n-2)+b1*e(n-1)+b0*e(n)
 MOVL @CNTL_2P2Z_DBUFF:x:, ACC		; ACC=u(n)(Q24)

; Saturate the result [0, 1]
MINL  ACC,*XAR7++	
MAXL  ACC,*XAR7++
		
; Move result to Uout as Q15.
LSL ACC, #7
MOV  *XAR6, AH
		
.endm






	
