?? pid_reg3.asm
字號:
;=====================================================================================
; File name: PID_REG3.ASM
;
; Originator: Digital Control Systems Group
; Texas Instruments
;
; Description: Digital PID controller with anti-windup correction
;
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 02-08-2001 Release Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx. (Peripheral Independent).
;================================================================================
; Routine Name: pid_reg3_calc Type: C Callable
;
; C prototype : void pid_reg3_calc(struct PIDREG3 *p);
;
; The struct object is defined in the header file "pid_reg3.h" as follows:
;
; typedef struct { int pid_ref_reg3; /* Input: Reference input (Q15) */
; int pid_fdb_reg3; /* Input: Feedback input (Q15) */
; int e_reg3; /* Variable: Error (Q14) */
; int Kp_reg3; /* Parameter: Proportional gain (Q15) */
; int up_reg3; /* Variable: Proportional output (Q14) */
; int ui_hi_reg3; /* Variable: Integral output (Q30) */
; int ui_lo_reg3; /* Variable: Integral output (Q30) */
; int ud_lo_reg3; /* Variable: Derivative output (Q30) */
; int ud_hi_reg3; /* Variable: Derivative output (Q30) */
; int uprsat_reg3; /* Variable: Pre-saturated output (Q14) */
; int pid_out_max; /* Parameter: Maximum output (Q15) */
; int pid_out_min; /* Parameter: Minimum output (Q15) */
; int pid_out_reg3; /* Output: PID output (Q15) */
; int saterr_reg3; /* Variable: Saturated difference (Q14) */
; int Ki_reg3; /* Parameter: Integral gain (Q31-16bit) */
; int Kc_reg3; /* Parameter: Integral correction gain (Q15) */
; int Kd_reg3; /* Parameter: Derivative gain (Q14) */
; int up1_reg3; /* Variable: Previous proportional output (Q14) */
; int (*calc)(); /* Pointer to calculation function */
; } PIDREG3;
;
; Frame Usage Details:
; step | a | b | c | d
;____________|_____________|______________|______________|_____________
; AR0 | | | |
;
;================================================================================
.def _pid_reg3_calc
;================================================================================
__pid_reg3_calc_framesize .set 0000h
;================================================================================
_pid_reg3_calc:
; Assume now ARP=AR1
POPD *+ ; Keep return address
SAR AR0,*+ ; Keep old frame pointer (FP)
SAR AR1,* ; Keep old stack pointer (SP)
LARK AR0,__pid_reg3_calc_framesize ; Load AR0 with frame size
LAR AR0,*0+,AR0 ; AR0->FP0 (new FP), ARP=AR0
;================================================================================
SBRK #3 ; ARP=AR0, AR0->FR0-3 (1st argument)
;----------------------------------------------------------------------------------
LAR AR2,* ; ARP=AR0, AR0->pid_ref_reg3, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR0, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
MAR *,AR2 ; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
SETC SXM ; Turn sign extension mode on
; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
SETC OVM ; Set overflow mode
; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
SPM 0 ; Reset product mode
; ARP=AR2, AR0->FR0, AR2->pid_ref_reg3
;----------------------------------------------------------------------------------
LACC *+,15 ; ACC = pid_ref_reg3 (Q30)
; ARP=AR2, AR0->FR0, AR2->pid_fdb_reg3
;----------------------------------------------------------------------------------
SUB *+,15 ; ACC = pid_ref_reg3 - pid_fdb_reg3 (Q30)
; ARP=AR2, AR0->FR0, AR2->e_reg3
;----------------------------------------------------------------------------------
SACH * ; e_reg3 = pid_ref_reg3 - pid_fdb_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->e_reg3
;----------------------------------------------------------------------------------
LT *+ ; TREG = e_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->Kp_reg3
;----------------------------------------------------------------------------------
MPY *+ ; PREG = Kp_reg3*e_reg3 (Q29)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
PAC ; ACC = Kp_reg3*e_reg3 (Q29)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
SACH *+,1 ; up_reg3 = Kp_reg3*e_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3
;----------------------------------------------------------------------------------
LACC *+,16 ; ACC = ui (Q30)
; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3
;----------------------------------------------------------------------------------
ADDS *+ ; ACC = ui (Q30)
; ARP=AR2, AR0->FR0, AR2->ud_lo_reg3
;----------------------------------------------------------------------------------
ADDS *+ ; ACC = ui + ud (Q30)
; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3
;----------------------------------------------------------------------------------
ADDH * ; ACC = ui + ud (Q30)
; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3
;----------------------------------------------------------------------------------
SBRK #4 ; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
ADDH * ; ACC = up + ui + ud (Q30)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
ADRK #5 ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
SACH * ; uprsat_reg3 = up + ui + ud (Q14)
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
LACC *+,16 ; ACC = uprsat_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
SUB *-,15 ; ACC = uprsat_reg3 - pid_out_max (Q14)
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
BCND SAT_MAX_REG3,GT ; Branch to SAT_MAX_REG3 if uprsat_reg3 > pid_out_max
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
LACC *,16 ; ACC = uprsat_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
SUB *,15 ; ACC = uprsat_reg3 - pid_out_min (Q14)
; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
BCND SAT_MIN_REG3,LT ; Branch to SAT_MIN_REG3 if uprsat_reg3 < pid_out_min
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
LACC *,16 ; ACC = uprsat_reg3 (Q30)
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
ADRK #3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SACH *,1 ; pid_out_reg3 = uprsat_reg3 (Q15)
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -