1 ; **********************************************************************************
2 ; SDK Version: PowerSmart™ Digital Control Library Designer v0.9.14.678
3 ; CGS Version: Code Generator Script v3.0.8 (03/12/2021)
5 ; Date/Time: 04/25/2021 13:39:05
6 ; **********************************************************************************
7 ; 2P2Z Control Library File (Fast Floating Point Coefficient Scaling Mode)
8 ; **********************************************************************************
10 ;------------------------------------------------------------------------------
12 .nolist ; (no external dependencies)
13 .list ; list of all external dependencies
15 ;------------------------------------------------------------------------------
17 .section .data ; place constant data in the data section
19 ;------------------------------------------------------------------------------
20 ;include NPNZ16b_s data structure and global constants.
21 .include "./sources/power_control/drivers/npnz16b.inc" ; include NPNZ16b_s object data structure value offsets and status flag labels
23 ;------------------------------------------------------------------------------
25 .section .text ; place code in the text section
27 ;------------------------------------------------------------------------------
28 ; Global function declaration
29 ; This function calls the z-domain controller processing the latest data point input
30 ;------------------------------------------------------------------------------
32 .global _v_loop_Update ; provide global scope to routine
33 _v_loop_Update: ; local function label
35 ;------------------------------------------------------------------------------
36 ; Check status word for Enable/Disable flag and bypass computation, if disabled
37 btss [w0], #NPNZ16_STATUS_ENABLED ; check ENABLED bit state, skip (do not execute) next instruction if set
38 bra V_LOOP_LOOP_BYPASS ; if ENABLED bit is cleared, jump to end of control code
40 ;------------------------------------------------------------------------------
41 ; Setup pointer to first element of error history array
42 mov [w0 + #ptrErrorHistory], w10 ; load pointer address into working register
44 ;------------------------------------------------------------------------------
45 ; Update error history (move error one tick down the delay line)
46 mov [w10 + #2], w6 ; move entry (n-2) into buffer
47 mov w6, [w10 + #4] ; move buffered value one tick down the delay line
48 mov [w10 + #0], w6 ; move entry (n-1) into buffer
49 mov w6, [w10 + #2] ; move buffered value one tick down the delay line
51 ;------------------------------------------------------------------------------
52 ; Read data from input source
53 mov [w0 + #ptrSourceRegister], w2 ; load pointer to input source register
54 mov [w2], w1 ; move value from input source into working register
55 mov [w0 + #ptrDProvControlInputComp], w2 ; load pointer address of target buffer of most recent, compensated controller input from data structure
56 mov w1, [w2] ; copy most recent controller input value to given data buffer target
58 ;------------------------------------------------------------------------------
59 ; Load reference and calculate error input to transfer function
60 mov [w0 + #ptrControlReference], w2 ; move pointer to control reference into working register
61 subr w1, [w2], w1 ; calculate error (=reference - input)
62 mov [w0 + #normPreShift], w2 ; move error input scaler into working register
63 sl w1, w2, w1 ; normalize error result to fractional number format
65 ;------------------------------------------------------------------------------
66 ; Setup pointers to B-Term data arrays
67 mov [w0 + #ptrBCoefficients], w8 ; load pointer to first index of B coefficients array
68 mov w1, [w10] ; add most recent error input to history array
70 ;------------------------------------------------------------------------------
71 ; Compute compensation filter B-term
72 clr b, [w8]+=2, w5 ; clear accumulator B and prefetch first error operand
73 clr a, [w8]+=4, w4, [w10]+=2, w6 ; clear accumulator A and prefetch first coefficient operand including number scaler
74 mpy w4*w6, a, [w8]+=4, w4, [w10]+=2, w6 ; multiply first control output of the delay line with first coefficient
75 sftac a, w5 ; shift accumulator to post-scale floating number
76 add b ; add accumulator a to accumulator b
77 mov [w8 - #6], w5 ; load scaler into working register
78 mpy w4*w6, a, [w8]+=4, w4, [w10]+=2, w6 ; multiply control output (n-1) from the delay line with coefficient B1
79 sftac a, w5 ; shift accumulator to post-scale floating number
80 add b ; add accumulator a to accumulator b
81 mov [w8 - #6], w5 ; load scaler into working register
82 mpy w4*w6, a ; multiply & accumulate last control output with coefficient of the delay line (no more prefetch)
83 sftac a, w5 ; shift accumulator to post-scale floating number
84 add b ; add accumulator a to accumulator b
86 ;------------------------------------------------------------------------------
87 ; Setup pointers to A-Term data arrays
88 mov [w0 + #ptrACoefficients], w8 ; load pointer to first index of A coefficients array
90 ;------------------------------------------------------------------------------
91 ; Load pointer to first element of control history array
92 mov [w0 + #ptrControlHistory], w10 ; load pointer address into working register
94 ;------------------------------------------------------------------------------
95 ; Compute compensation filter A-term
96 movsac b, [w8]+=2, w5 ; leave contents of accumulator B unchanged
97 clr a, [w8]+=4, w4, [w10]+=2, w6 ; clear accumulator A and prefetch first coefficient operand including number scaler
98 mpy w4*w6, a, [w8]+=4, w4, [w10]+=2, w6 ; multiply first control output of the delay line with first coefficient
99 sftac a, w5 ; shift accumulator to post-scale floating number
100 add b ; add accumulator a to accumulator b
101 mov [w8 - #6], w5 ; load scaler into working register
102 mpy w4*w6, a ; multiply & accumulate last control output with coefficient of the delay line (no more prefetch)
103 sftac a, w5 ; shift accumulator to post-scale floating number
104 add b ; add accumulator a to accumulator b
105 sac.r b, w4 ; store most recent accumulator result in working register
107 ;------------------------------------------------------------------------------
108 ; Controller Anti-Windup (control output value clamping)
110 ; Check for lower limit violation
111 mov [w0 + #MinOutput], w6 ; load lower limit value
112 cpsgt w4, w6 ; compare values and skip next instruction if control output is within operating range (control output > lower limit)
113 mov w6, w4 ; override most recent controller output
114 V_LOOP_CLAMP_MIN_EXIT:
116 ; Check for upper limit violation
117 mov [w0 + #MaxOutput], w6 ; load upper limit value
118 cpslt w4, w6 ; compare values and skip next instruction if control output is within operating range (control output < upper limit)
119 mov w6, w4 ; override most recent controller output
120 V_LOOP_CLAMP_MAX_EXIT:
122 ;------------------------------------------------------------------------------
123 ; Call user function before the most recent controller result is written to target
124 mov [w0 + #ptrExtHookPreTargetWriteFunction], w12 ; load function pointer to extern user function
125 call w12 ; call function
127 ;------------------------------------------------------------------------------
128 ; Write control output value to target
129 mov [w0 + #ptrTargetRegister], w8 ; capture pointer to target in working register
130 mov w4, [w8] ; move control output to target address
131 mov [w0 + #ptrDProvControlOutput], w2 ; load pointer address of target buffer of most recent controller output value from data structure
132 mov w4, [w2] ; copy most recent controller output value to given data buffer target
134 ;------------------------------------------------------------------------------
135 ; Load pointer to first element of control history array
136 mov [w0 + #ptrControlHistory], w10 ; load pointer address into working register
138 ;------------------------------------------------------------------------------
139 ; Update control output history (move entries one tick down the delay line)
140 mov [w10 + #0], w6 ; move entry (n-1) one tick down the delay line
142 mov w4, [w10] ; add most recent control output to history
144 ;------------------------------------------------------------------------------
145 ; Enable/Disable bypass branch target with dummy read of source buffer
146 goto V_LOOP_LOOP_EXIT ; when enabled, step over dummy read and go straight to EXIT
147 V_LOOP_LOOP_BYPASS: ; Enable/Disable bypass branch target to perform dummy read of source to clear the source buffer
148 mov [w0 + #ptrSourceRegister], w2 ; load pointer to input source register
149 mov [w2], w1 ; move value from input source into working register
150 mov [w0 + #ptrDProvControlInputComp], w2 ; load pointer address of target buffer of most recent, compensated controller input from data structure
151 mov w1, [w2] ; copy most recent controller input value to given data buffer target
152 mov [w0 + #ptrDProvControlOutput], w2 ; load pointer address of target buffer of most recent controller output value from data structure
153 clr [w2] ; copy most recent controller output value to given data buffer target
154 V_LOOP_LOOP_EXIT: ; Exit control loop branch target
156 ;------------------------------------------------------------------------------
158 return ; end of function; return to caller
160 ;------------------------------------------------------------------------------
163 ;------------------------------------------------------------------------------
164 ; Global function declaration v_loop_Reset
165 ; This function clears control and error histories enforcing a reset
166 ;------------------------------------------------------------------------------
168 .global _v_loop_Reset ; provide global scope to routine
169 _v_loop_Reset: ; local function label
171 ;------------------------------------------------------------------------------
172 ; Clear control history array
173 push w0 ; save contents of working register WREG0
174 mov [w0 + #ptrControlHistory], w0 ; set pointer to the base address of control history array
175 clr [w0++] ; clear next address of control history array
176 clr [w0] ; clear last address of control history array
177 pop w0 ; restore contents of working register WREG0
179 ;------------------------------------------------------------------------------
180 ; Clear error history array
181 push w0 ; save contents of working register WREG0
182 mov [w0 + #ptrErrorHistory], w0 ; set pointer to the base address of error history array
183 clr [w0++] ; Clear next address of error history array
184 clr [w0++] ; Clear next address of error history array
185 clr [w0] ; clear last address of error history array
186 pop w0 ; restore contents of working register WREG0
188 ;------------------------------------------------------------------------------
190 return ; end of function; return to caller
192 ;------------------------------------------------------------------------------
195 ;------------------------------------------------------------------------------
196 ; Global function declaration v_loop_Precharge
197 ; This function loads user-defined default values into control and error histories
198 ;------------------------------------------------------------------------------
200 .global _v_loop_Precharge ; provide global scope to routine
201 _v_loop_Precharge: ; local function label
203 ;------------------------------------------------------------------------------
204 ; Charge error history array with defined value
205 push w0 ; save contents of working register WREG0
206 push w1 ; save contents of working register WREG1
207 mov [w0 + #ptrErrorHistory], w0 ; set pointer to the base address of error history array
208 mov w1, [w0++] ; Load user value into next address of error history array
209 mov w1, [w0++] ; Load user value into next address of error history array
210 mov w1, [w0] ; load user value into last address of error history array
211 pop w1 ; restore contents of working register WREG1
212 pop w0 ; restore contents of working register WREG0
214 ;------------------------------------------------------------------------------
215 ; Charge control history array with defined value
216 push w0 ; save contents of working register WREG0
217 push w2 ; save contents of working register WREG2
218 mov [w0 + #ptrControlHistory], w0 ; set pointer to the base address of control history array
219 mov w2, [w0++] ; Load user value into next address of control history array
220 mov w2, [w0] ; Load user value into last address of control history array
221 pop w2 ; restore contents of working register WREG2
222 pop w0 ; restore contents of working register WREG0
224 ;------------------------------------------------------------------------------
226 return ; end of function; return to caller
228 ;------------------------------------------------------------------------------
231 ;------------------------------------------------------------------------------
232 ; Global function declaration v_loop_PTermUpdate
233 ; This function executes a P-Term based control loop used for plant measurements only.
234 ; THIS LOOP IS NOT SUITED FOR STABLE OPERATION
235 ;------------------------------------------------------------------------------
237 .global _v_loop_PTermUpdate ; provide global scope to routine
238 _v_loop_PTermUpdate: ; local function label
240 ;------------------------------------------------------------------------------
241 ; Check status word for Enable/Disable flag and bypass computation when disabled
242 btss [w0], #NPNZ16_STATUS_ENABLED ; check ENABLED bit state, skip (do not execute) next instruction if set
243 bra V_LOOP_PTERM_LOOP_BYPASS ; if ENABLED bit is cleared, jump to end of control code
245 ;------------------------------------------------------------------------------
246 ; Read data from input source
247 mov [w0 + #ptrSourceRegister], w2 ; load pointer to input source register
248 mov [w2], w1 ; move value from input source into working register
249 mov [w0 + #ptrDProvControlInputComp], w2 ; load pointer address of target buffer of most recent, compensated controller input from data structure
250 mov w1, [w2] ; copy most recent controller input value to given data buffer target
252 ;------------------------------------------------------------------------------
253 ; Load reference and calculate error input to transfer function
254 mov [w0 + #ptrControlReference], w2 ; move pointer to control reference into working register
255 subr w1, [w2], w1 ; calculate error (=reference - input)
256 mov [w0 + #normPreShift], w2 ; move error input scaler into working register
257 sl w1, w2, w1 ; normalize error result to fractional number format
259 ;------------------------------------------------------------------------------
260 ; Load P-gain factor from data structure
261 mov [w0 + #PTermFactor], w6 ; move P-coefficient fractional into working register
262 mov [w0 + #PTermScaler], w2 ; move P-coefficient scaler into working register
263 mov w1, w4 ; move error to MPY working register
264 ; calculate P-control result
265 mpy w4*w6, a ; multiply most recent error with P-coefficient
266 sftac a, w2 ; shift accumulator to post-scale floating number
267 sac.r a, w4 ; store accumulator result to working register
269 ;------------------------------------------------------------------------------
270 ; Controller Anti-Windup (control output value clamping)
272 ; Check for lower limit violation
273 mov [w0 + #MinOutput], w6 ; load lower limit value
274 cpsgt w4, w6 ; compare values and skip next instruction if control output is within operating range (control output > lower limit)
275 mov w6, w4 ; override most recent controller output
276 V_LOOP_PTERM_CLAMP_MIN_EXIT:
278 ; Check for upper limit violation
279 mov [w0 + #MaxOutput], w6 ; load upper limit value
280 cpslt w4, w6 ; compare values and skip next instruction if control output is within operating range (control output < upper limit)
281 mov w6, w4 ; override most recent controller output
282 V_LOOP_PTERM_CLAMP_MAX_EXIT:
284 ;------------------------------------------------------------------------------
285 ; Call user function before the most recent controller result is written to target
286 mov [w0 + #ptrExtHookPreTargetWriteFunction], w12 ; load function pointer to extern user function
287 mov [w0 + #ExtHookPreTargetWriteFunctionParam], w13 ; load single parameter or pointer to parameter data structure
288 call w12 ; call function
290 ;------------------------------------------------------------------------------
291 ; Write control output value to target
292 mov [w0 + #ptrTargetRegister], w8 ; capture pointer to target in working register
293 mov w4, [w8] ; move control output to target address
294 mov [w0 + #ptrDProvControlOutput], w2 ; load pointer address of target buffer of most recent controller output value from data structure
295 mov w4, [w2] ; copy most recent controller output value to given data buffer target
297 ;------------------------------------------------------------------------------
298 ; Enable/Disable bypass branch target with dummy read of source buffer
299 goto V_LOOP_PTERM_LOOP_EXIT ; when enabled, step over dummy read and go straight to EXIT
300 V_LOOP_PTERM_LOOP_BYPASS: ; Enable/Disable bypass branch target to perform dummy read of source to clear the source buffer
301 mov [w0 + #ptrSourceRegister], w2 ; load pointer to input source register
302 mov [w2], w1 ; move value from input source into working register
303 mov [w0 + #ptrDProvControlInputComp], w2 ; load pointer address of target buffer of most recent, compensated controller input from data structure
304 mov w1, [w2] ; copy most recent controller input value to given data buffer target
305 mov [w0 + #ptrDProvControlOutput], w2 ; load pointer address of target buffer of most recent controller output value from data structure
306 clr [w2] ; copy most recent controller output value to given data buffer target
307 V_LOOP_PTERM_LOOP_EXIT: ; Exit P-Term control loop branch target
309 ;------------------------------------------------------------------------------
311 return ; end of function; return to caller
313 ;------------------------------------------------------------------------------
316 ;------------------------------------------------------------------------------
318 .end ; end of file v_loop_asm.s
320 ;------------------------------------------------------------------------------
323 ; **********************************************************************************
324 ; Download latest version of this tool here: https://microchip-pic-avr-tools.github.io/powersmart-dcld/
325 ; **********************************************************************************
