环路代码调用的是Microchip的compensator Libraries,由于是VMC控制,采用的是3P3Z补偿器。
3P3Z的差分方程如下:
u[n] = B0*e[n] + B1*e[n-1] + B2*e[n-2] + B3*e[n-3] + A1*u[n-1] + A2*u[n-2] + A3*u[n-3]
系数定义如下:
; Array assignments:
;
; xABCoefficients[0] = B0
; xABCoefficients[1] = B1
; xABCoefficients[2] = B2
; xABCoefficients[3] = B3
; xABCoefficients[4] = A1
; xABCoefficients[5] = A2
; xABCoefficients[6] = A3
; xErrorControlHistory[0] = e[n-1]
; xErrorControlHistory[1] = e[n-2]
; xErrorControlHistory[2] = e[n-3]
; xErrorControlHistory[3] = u[n-1]
; xErrorControlHistory[4] = u[n-2]
; xErrorControlHistory[5] = u[n-3]
我这里使用了辅助工作寄存器,该工作寄存器能够节约数据保存,压栈等的时间,寄存器应用如下:
; w0 = Control Reference value
; w1 = Address of the Source Register (Input) - ADCBUFx
; w2 = Address of the Target Register (Output) - PDCx/CMPxDAC
; w3 = ACCAL ... and misc operands
; w4 = ACCAH ... and misc operands
; w5 = ACCAU ... and misc operands
; w6 = postScalar
; w7 = postShift
; w8 = Library options structure pointer
; w9 = ACoefficients/BCoefficients array base address { B0, B1, B2, B3, A1, A2, A3 }
; w10 = ErrorHistory/ControlHistory array base address { e[n-1], e[n-2], e[n-3], u[n-1], u[n-2], u[n-3] }
; w11 = minclamp
; w12 = maxClamp
; w13 = user defined, misc use
; w14 = user defined, misc use
.global _SMPS_Controller3P3ZUpdate_HW_Accel ; provide global scope to routine
_SMPS_Controller3P3ZUpdate_HW_Accel :
push CORCON ; Save CORCON as it will be modified for fractional computation
; 1.31 saturation
; Immediately load previous ACCUMULATOR results to free up Alt-W registers w3, w4, & w5 for misc. instructions
mov w3, ACCAL ; Load bits 15:0
mov w4, ACCAH ; Load bits 31:16
mov w5, ACCAU ; Load bits 39:32
mov #0xA0, w4 ; Load literal value 0x0080 CORCON register: 0b 0000 0000 1010 0000
mov w4, _CORCON
sub w0, [w1], w5 ; w5 = Control Reference - measured output ( w5 = w0 - [w1] )
mov w5, [w10] ; Store most recent error; becomes e[n-1] for next iteration
mov [w9++], w4 ; w4 = B0
mac w4*w5, a ; ACCA = B0 * e[n] + ACCA from previous iteration
sftac a, w7 ; w7 = Normalization shift value to compensate coefficient scaling
; ACC = ACC>>
ostShift or ACC = ACC<<
ostShift depending on sign of value
sac.r a, #0, w4 ; w4 = Rnd(ACCAH) - this is the control output value from previous
; line of code after having been shifted. Store bits 31:16 of the ACCA
; into w4 temp register
mpy w4*w6, a ; Multiply control output (after rounding) and postScalar factor
sac.r a, w4 ; w4 = Rnd(ACCAH) again after normalization
; Save control output to control history array prior to clamping
mov w4, [w10 + #0x6] ; w4 = u[n] ( becomes u[n-1] for next calculation )
; Clamp to minimum if needed
cpsgt w4, w11 ; Check if u[n] > minClamp. If true, the next instruction is discarded and a Nop() is executed
; If not true, execute next instruction
mov.w w11, w4 ; Update u[n] with minClamp value
; Clamp to maximum if needed
cpslt w4, w12 ; Check if u[n] < maxClamp. If true, the next instruction is discarded and a Nop() is executed
; If not true, execute next instruction
mov.w w12, w4 ; Update u[n] with maxClamp value
mov w4, [w2] ; Update the target register (Output): [w2] = PDCx/CMPxDAC
mov w4, w14
controlHistoryUpdate:
; Update control history:
clr a, [w9]+=2, w4, [w10]+=2, w5 ; ACCA = EMPTY
; w4 = B1, w5 = e[n-1]
mac w4*w5, a, [w9]+=2, w4, [w10]+=2, w5 ; ACCA = B1 * e[n-1]
; w4 = B2, w5 = e[n-2]
mac w4*w5, a, [w9]+=2, w4, [w10]+=2, w5 ; ACCA = B1 * e[n-1] + B2 * e[n-2]
; w4 = B3, w5 = e[n-3]
mac w4*w5, a, [w9]+=2, w4, [w10]+=2, w5 ; ACCA = B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
; w4 = A1, w5 = u[n-1]
; Calculate second section of 3P3Z controller -> (ACoefficients * controlHistory) and add results to accumulator 'A'
mac w4*w5, a, [w9]+=2, w4, [w10]+=2, w5 ; ACCA = A1 * u[n-1] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
; w4 = A2, w5 = u[n-2]
mac w4*w5, a, [w9], w4, [w10], w5 ; ACCA = A1 * u[n-1] + A2 + u[n-2] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
; w4 = A3, w5 = u[n-3]
mac w4*w5, a, [w10]-=6, w5 ; ACCA = A1 * u[n-1] + A2 * u[n-2] + A3 * u[n-3] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
; Decrement pointers to return to array base addresses
sub w9, #0x0C, w9 ; Decrement by 6 words (Reset coefficient array pointer)
; Update compensator errorControlHistory array elements
mov [--w10], w3 ; w3 = e[n-2]
mov w3, [w10 + #2] ; e[n-3] = w3
mov [--w10], w3 ; w3 = e[n-1]
mov w3, [w10 + #2] ; e[n-2] = w3
mov [w10 + #8], w3 ; w3 = u[n-2]
mov w3, [w10 + #10] ; u[n-3] = w3
mov [w10 + #6], w3 ; w3 = u[n-1]
mov w3, [w10 + #8] ; u[n-2] = w3
; Save 40-bit accumulator results for next iteration
mov ACCAL, w3 ; Load bits: 15:0
mov ACCAH, w4 ; Load bits: 31:16
mov ACCAU, w5 ; Load bits: 39:32
pop CORCON ; Restore CORCON
return
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;