Other Parts Discussed in Thread: CONTROLSUITE
;=====================================================================================
; 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.00
;================================================================================
; 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) */
; （1） 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
;----------------------------------------------------------------------------------
（2） 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)
; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
B UPDATE_REG3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SAT_MAX_REG3 ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
ADRK #1 ; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
LACC * ; ACC = pid_out_max (Q15)
; ARP=AR2, AR0->FR0, AR2->pid_out_max
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SACL * ; pid_out_reg3 = pid_out_max (Q15)
; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
B UPDATE_REG3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SAT_MIN_REG3 ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
ADRK #2 ; ARP=AR2, AR0->FR0, AR2->pid_out_min
;----------------------------------------------------------------------------------
LACC *+ ; ACC = pid_out_min (Q15)
; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SACL * ; pid_out_reg3 = pid_out_min (Q15)
; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
UPDATE_REG3 ; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
LACC *,15 ; ACC = pid_out_reg3 (Q30)
; ARP=AR2, AR0->FR0, AR2->pid_out_reg3
;----------------------------------------------------------------------------------
SBRK #3 ; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
SUB *,16 ; ACC = pid_out_reg3 - uprsat_reg3 (Q30)
; ARP=AR2, AR0->FR0, AR2->uprsat_reg3
;----------------------------------------------------------------------------------
ADRK #4 ; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
SACH *+ ; saterr_reg3 = pid_out_reg3 - uprsat_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->Ki_reg3
;----------------------------------------------------------------------------------
SPM 3 ; Set right shifted 6 bit
; ARP=AR2, AR0->FR0, AR2->Ki_reg3
;----------------------------------------------------------------------------------
（3） LT * ; TREG = Ki (Q31-16 bit)
; ARP=AR2, AR0->FR0, AR2->Ki_reg3
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
（4）MPY * ; PREG = Ki*up (Q38)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
PAC ; ACC = Ki*up (Q32)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
SFR ; ACC = Ki*up (Q31)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
SFR ; ACC = Ki*up (Q30)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
SPM 1 ; Set left shifted 1 bit
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
ADRK #11 ; ARP=AR2, AR0->FR0, AR2->Kc_reg3
;----------------------------------------------------------------------------------
LT * ; TREG = Kc (Q15)
; ARP=AR2, AR0->FR0, AR2->Kc_reg3
;----------------------------------------------------------------------------------
SBRK #2 ; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
MPY * ; PREG = Kc*saterr_reg3 (Q29)
; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
APAC ; ACC = Ki*up + Kc*(pid_out_reg3-uprsat_reg3) (Q30)
; ARP=AR2, AR0->FR0, AR2->saterr_reg3
;----------------------------------------------------------------------------------
SBRK #7 ; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3
;----------------------------------------------------------------------------------
ADDS *- ; ACC = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3) (Q30)
; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3
;----------------------------------------------------------------------------------
ADDH *+ ; ACC = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3) (Q30)
; ARP=AR2, AR0->FR0, AR2->ui_lo_reg3
;----------------------------------------------------------------------------------
SACL *- ; ui = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3) (Q30)
; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3
;----------------------------------------------------------------------------------
SACH * ; ui = ui + Ki*up + Kc*(pid_out_reg3-uprsat_reg3) (Q30)
; ARP=AR2, AR0->FR0, AR2->ui_hi_reg3
;----------------------------------------------------------------------------------
ADRK #11 ; ARP=AR2, AR0->FR0, AR2->Kd_reg3
;----------------------------------------------------------------------------------
LT * ; TREG = Kd (Q14)
; ARP=AR2, AR0->FR0, AR2->Kd_reg3
;----------------------------------------------------------------------------------
SBRK #12 ; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
MPY * ; PREG = Kd*up (Q28)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
PAC ; ACC = Kd*up (Q29)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
ADRK #13 ; ARP=AR2, AR0->FR0, AR2->up1_reg3
;----------------------------------------------------------------------------------
MPY * ; PREG = Kd*up1 (Q28)
; ARP=AR2, AR0->FR0, AR2->up1_reg3
;----------------------------------------------------------------------------------
SPAC ; ACC = Kd*up - Kd*up1 (Q29)
; ARP=AR2, AR0->FR0, AR2->up1_reg3
;----------------------------------------------------------------------------------
SBRK #10 ; ARP=AR2, AR0->FR0, AR2->ud_lo_reg3
;----------------------------------------------------------------------------------
SACL *+,1 ; ud_lo_reg3 = Kd*up - Kd*up1 (Q30)
; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3
;----------------------------------------------------------------------------------
SACH *,1 ; ud_hi_reg3 = Kd*up - Kd*up1 (Q30)
; ARP=AR2, AR0->FR0, AR2->ud_hi_reg3
;----------------------------------------------------------------------------------
SBRK #4 ; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
LACC * ; ACC = up_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->up_reg3
;----------------------------------------------------------------------------------
ADRK #13 ; ARP=AR2, AR0->FR0, AR2->up1_reg3
;----------------------------------------------------------------------------------
SACL *,AR1 ; up1_reg3 = up_reg3 (Q14)
; ARP=AR2, AR0->FR0, AR2->up1_reg3, ARP=AR1
;----------------------------------------------------------------------------------
_pid_reg3_calc_exit:
;; MAR *,AR1 ; can be removed if this condition is met on
; every path to this code. (i.e., ARP=AR1 here)
SPM 0
CLRC OVM
CLRC SXM
SBRK #(__pid_reg3_calc_framesize+1)
LAR AR0,*-
PSHD *
RET
MY question ：
The code above is obtained from TMS320C2000™ Microcontroller Digital Motor Control Libraries.and the PID_reg3.asm is a part of the library which is implented by ASM language.my qustion is focused on the yellow colour parts.
1. in yellow part (1),my question is :how does a 16-bit data express Q31 format date
2. in yellow part (2)(3)(4),my question is :（2）and（3）show Ki is a Q31 format date and up(or up_reg3) is a Q14 format date;if they multiply,then the product should be a Q45 (Q31+Q14) format date.but the yellow part (4) show the product is a Q38 format date. why?
Looking forward to your answers.
Thanks for your help.
