Bar Logo 4kW dsPIC33C PSFB DC-DC DA (Part-No. )
 
Content
      
Loading...
Searching...
No Matches
smps_3p3z_dspic.s
1;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
2; (c) 2013 Microchip Technology Inc.
3;
4; MICROCHIP SOFTWARE NOTICE AND DISCLAIMER: You may use this software, and any
5; derivatives created by any person or entity by or on your behalf, exclusively with
6; Microchip?s products. Microchip and its licensors retain all ownership and intellectual
7; property rights in the accompanying software and in all derivatives here to.
8;
9; This software and any accompanying information is for suggestion only. It does not
10; modify Microchip?s standard warranty for its products. You agree that you are solely
11; responsible for testing the software and determining its suitability. Microchip has
12; no obligation to modify, test, certify, or support the software.
13;
14; THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
15; OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF NON-INFRINGEMENT,
16; MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE, ITS INTERACTION
17; WITH MICROCHIP?S PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION.
18;
19; IN NO EVENT, WILL MICROCHIP BE LIABLE, WHETHER IN CONTRACT, WARRANTY, TORT
20; (INCLUDING NEGLIGENCE OR BREACH OF STATUTORY DUTY), STRICT LIABILITY, INDEMNITY,
21; CONTRIBUTION, OR OTHERWISE, FOR ANY INDIRECT, SPECIAL, PUNITIVE, EXEMPLARY, INCIDENTAL
22; OR CONSEQUENTIAL LOSS, DAMAGE, FOR COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE
23; SOFTWARE, HOWSOEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE POSSIBILITY OR
24; THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT ALLOWABLE BY LAW, MICROCHIP'S TOTAL
25; LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES,
26; IF ANY, THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE.
27;
28; MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE TERMS.
29;
30;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
31
32
33; Local inclusions.
34 .nolist
35 .list
36
37; Define data structure address offsets
38 .equ offsetACoefficients, 0
39 .equ offsetBCoefficients, 2
40 .equ offsetControlHistory, 4
41 .equ offsetErrorHistory, 6
42 .equ offsetPreShift, 8
43 .equ offsetPostShift, 10
44 .equ offsetPostScaler, 12
45 .equ offsetMinOutput, 14
46 .equ offsetMaxOutput, 16
47
48
49;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
50
51 ; .section .libdsp, code ; use this section type when used as hidden library file
52 .section .text ; use this section type for debugging
53
54;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
55;
56; _SMPS_Controller3P3ZUpdate:
57; Prototype:
58; void SMPS_Controller3P3ZUpdate(SMPS_3P3Z_T* controllerData,
59; volatile uint16_t* controllerInputRegister,
60; int16_t reference,
61; volatile uint16_t* controllerOutputRegister);
62;
63; Operation:
64; Wcp0 ( S + Wcz1 )( S + Wcz2 )
65; Calculate the transfer function Hc(s) = ---- X ------------------------
66; S ( S + Wcp1 )( S + Wcp2 )
67; Error Normalization
68; ------------
69; | prsft |
70; ------------
71;Reference | e(n) e(n-1) e(n-2) e(n-3)
72;Input | -------- -------- --------
73; - - - | | | | | |
74; o----|-|-|x|-|x|----o->| z^(-1) |-o->| z^(-1) |-o->| z^(-1) |--o Post
75; - - - | | | | | | | | | | Scaling
76; | NORM | -------- | -------- | -------- |
77; | | | | | --------
78;Measured| --- --- --- --- | | - - - - - - - - - -
79;Input | B0=>| X | B1=>| X | B2=>| X | B3=>| X | |posft | | |
80; | --- --- --- --- |postScalar | P O W E R |
81; | | | | | -------- | P L A N T |
82; | ---------------------------------------------- | | |
83; | | | U[n] --- | |
84; | | A C C U M U L A T O R |--o--| X |------O Duty Ratio=>PWM |
85; | | | | --- | |
86; | ---------------------------------------------- | | Voltage/Current |
87; | | | | | | Feedback |
88; | --- --- --- | | O |
89; | A3=>| X | A2=>| X | A1=>| X | | | | |
90; | --- --- --- | - - - - - - - - - -
91; | | | | | |
92; | | -------- | -------- | -------- | | A/D
93; | | | | | | | | | | | | Input
94; | o--| z^(-1) |<-o-| z^(-1) |<-o-| z^(-1) |------- |
95; | | | | | | | |
96; | -------- -------- -------- |
97; | u[n-3] u[n-2] u[n-1] |
98; | |
99; | |
100; -------------------------------------------------------------------------------------
101;
102;
103; controlHistory[n] =
104; ( errorHistory[n] * bCoefficients[0]
105; + errorHistory[n-1] * bCoefficients[1]
106; + errorHistory[n-2] * bCoefficients[2]
107; + errorHistory[n-3] * bCoefficients[3]
108; + controlHistory[n-1] * aCoefficients[0]
109; + controlHistory[n-2] * aCoefficients[1]
110; + controlHistory[n-3] * aCoefficients[2]
111; )
112;
113;
114; where:
115;
116; aCoefficients[0] = A1
117; aCoefficients[1] = A2
118; aCoefficients[2] = A3
119; bCoefficients[0] = B0
120; bCoefficients[1] = B1
121; bCoefficients[2] = B2
122; bCoefficients[3] = B3
123; controlHistory[0] = u(n)
124; controlHistory[1] = u(n-1)
125; controlHistory[2] = u(n-2)
126; errorHistory[0] = e(n) => most recent error {(Reference) - (inputSource)}
127; errorHistory[1] = e(n-1)
128; errorHistory[2] = e(n-2)
129; errorHistory[3] = e(n-3)
130; preShift = prsft => pre scaling factor for Q15 normalization purposes
131; postShift = posft => post scaling factor for normalization purposes
132; postScaler = postScaler => post scaling coefficient for normalization purpose
133;
134; Function call inputs:
135; sourceRegister = ADBUFx => measured feedback input
136; controlReference = v_ref => reference voltage
137; targetRegister = PDCx => output duty cycle
138;
139; where:
140;
141; aCoefficients, bCoefficients, controlHistory, errorHistory,
142; preShift and postShift are all members of the data structure SMPS_3P3Z_T.
143;
144; Input:
145; w0 = Address of SMPS_3P3Z_T data structure
146; w1 = Address of the Source Register (Input)
147; w2 = Control Reference
148; w3 = Address of the Target Register (Output)
149; Return:
150; w0 = Address of SMPS_3P3Z_T data structure
151; w1 = Address of the Source Register (Input)
152; w2 = Control Reference
153; w3 = Address of the Target Register (Output)
154;
155; System resources usage:
156; {w4..w5} saved, used, restored
157; {w8,w10} saved, used, restored
158; AccA used, not restored
159; CORCON saved, used, restored
160;
161; Note
162; This function requires CORCON register to be setup in a certain state
163; in order to operate correctly. Due to this requirement, this function
164; will save the CORCON register on the stack in the beginning of the
165; function and restore it before the function return.
166; After saving the CORCON register, this function writes to all bits of
167; the CORCON register. Thus, for the brief duration when this function is
168; executing, the state of CORCON register may be different from its state
169; as set by the function caller. This may change the CPU core behaviour with
170; respect to exception processing latency, DO loop termination, CPU interrupt
171; priority level and DSP-engine behaviour.
172;............................................................................
173
174 .global _SMPS_Controller3P3ZUpdate ; provide global scope to routine
175_SMPS_Controller3P3ZUpdate:
176
177 ; Save working registers.
178 push w4 ; Save register W4
179 push w5 ; Save register W5
180 push w8 ; Save register W8
181 push w10 ; Save register W10
182 push CORCON ; Save CORCON as it will be modified for fractional computation
183
184 ; Set up DSP core for signed fractional operation, saturation on accumulator A
185 ; and for write-back instructions from ACCx to WREGx,
186 ; accumulator saturation mode is 1.31
187
188 mov #0x00A0, w4 ; Load literal value 0x00A0 to CORCON register: 0b 0000 0000 1010 0000
189 mov w4, _CORCON
190
191 ; Initialize source input, reference, error history and normalization variables
192
193 mov [w0 + #offsetErrorHistory], w10 ; w10 = Base Address of _ErrorHistory array (e[n], e[n-1], e[n-2], e[n-3])
194 mov [w0 + #offsetPreShift], w8 ; Load error amplifier normalization bit-shift step
195
196 ; Update the error history
197
198 mov [w10 + #4], w5 ; W5 = ErrorHistory[n-2]
199 mov w5, [w10 + #6] ; ErrorHistory[n-3] = W5
200 mov [w10 + #2], w5 ; W5 = ErrorHistory[n-1]
201 mov w5, [w10 + #4] ; ErrorHistory[n-2] = W5
202 mov [w10 + #0], w5 ; W5 = ErrorHistory[n]
203 mov w5, [w10 + #2] ; ErrorHistory[n-1] = W5
204
205 ; Calculate most recent error with normalization,
206
207 sub w2, [w1], w5 ; w5 = Reference - inputSource
208 sl w5, w8, w5 ; shift error by PRE_SHIFT bit to the left (Q15 scaling)
209
210 ; Store most recent error to error history array
211 mov w5, [w10] ; controller3P3Z.errorHistory[n] = w5
212
213 ; Initialize B coefficients array
214
215 mov [w0 + #offsetBCoefficients], w8 ; w8 = Base Address of _BCoefficients array (B0, B1, B2, B3)
216
217 ; Calculate first section of 2P2Z controller -> Error History
218 clr a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = EMPTY
219 ; w4 = B0, w5 = e[0]
220 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = B0 * e[n]
221 ; w4 = B1, w5 = e[n-1]
222 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = B0 * e[n] + B1 * e[n-1]
223 ; w4 = B2, w5 = e[n-2]
224 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = B0 * e[n] + B1 * e[n-1] + B2 * e[n-2]
225 ; w4 = B3, w5 = e[n-3]
226 mac w4*w5, a ; ACCA = B0 * e[n] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
227
228
229 ; Initialize A coefficients and control history arrays
230
231 mov [w0 + #offsetACoefficients], w8 ; w8 = Base Address of _ACoefficients array (A1, A2, A3)
232 mov [w0 + #offsetControlHistory], w10 ; w10 = Base Address of _ControlHistory array (y[n-1], y[n-2], y[n-3])
233
234
235 ; Calculate second section of 2P2Z controller - Controller History
236 movsac a, [w8]+=2, w4, [w10]+=2, w5 ; w4 = A1, w5 = y[n-1]
237 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = A1 * y[n-1] + B0 * e[n] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
238 ; w4 = A2, w5 = y[n-2]
239 mac w4*w5, a, [w8], w4, [w10], w5 ; ACCA = A1 * y[n-1] + A2 * y[n-2] + B0 * e[n] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
240 ; w4 = A3, w5 = y[n-3]
241 mac w4*w5, a ; ACCA = A1 * y[n-1] + A2 * y[n-2] + A3 * y[n-3] + B0 * e[n] + B1 * e[n-1] + B2 * e[n-2] + B3 * e[n-3]
242
243
244 ; Initialize Scale-factor and multiply
245
246 sac.r a, w4 ; w4 = Sat(Rnd(ACCAH))
247 mov [w0 + #offsetPostScaler], w5 ; w5 = postScaler
248 mpy w4*w5, a ; Multiply control output and scale-factor
249
250
251 ; Backwards normalization & write back
252 mov [w0 + #offsetPostShift], w5 ; w5 = Normalization Shift to compensate coefficient scaling
253 sftac a, w5 ; Backward normalization to compensate coefficient scaling
254 sac.r a, #0, w4 ; w4 = Sat(Rnd(ACCAH))
255
256
257
258 ; Clamp controller output to min/max values if needed
259 mov [w0 + #offsetMinOutput], w5
260 cpsgt w4, w5
261 mov.w w5, w4
262 mov [w0 + #offsetMaxOutput], w5
263 cpslt w4, w5
264 mov.w w5, w4
265
266
267
268 ; Update the controller output history on the delay line
269
270 mov [w10 + #-2], w5 ; W5 = ControlHistory[n-2]
271 mov w5, [w10] ; ControlHistory[n-3] = W5
272 mov [w10 + #-4], w5 ; W5 = ControlHistory[n-1]
273 mov w5, [w10 + #-2] ; ControlHistory[n-2] = W5
274 mov w4, [w10 + #-4] ; ControlHistory[n] = w2
275
276; ; Clamp controller output to min/max values if needed
277; mov [w0 + #offsetMinOutput], w5
278; cpsgt w4, w5
279; mov.w w5, w4
280; mov [w0 + #offsetMaxOutput], w5
281; cpslt w4, w5
282; mov.w w5, w4
283
284 ; Controller write back into target register
285 mov w4, [w3] ; write result into target register
286
287 ; restore registers
288
289 pop CORCON ; restore CORCON.
290 pop w10 ; restore working register W10
291 pop w8 ; restore working register W8
292 pop w5 ; restore working register W5
293 pop w4 ; restore working register W4
294
295
296 return
297
298;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
299
300;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
301; _SMPS_Controller3P3ZInitialize:
302;
303; Prototype:
304; void SMPS_Controller3P3ZInitialize ( SMPS_3P3Z_T *controller3P3ZStruct )
305;
306; Operation: This routine clears the delay line elements in the arrays
307; _ControlHistory and _ErrorHistory.
308;
309; Input:
310; w0 = Address of data structure SMPS_3P3Z_T (type defined in smps_control.h)
311;
312; Return:
313; (void)
314;
315; System resources usage:
316; w0 saved, used, restored
317;
318;............................................................................
319
320 .global _SMPS_Controller3P3ZInitialize ; provide global scope to routine
321
322_SMPS_Controller3P3ZInitialize:
323
324 ; Clean up controlHistory variables
325 push w0 ; Save w0
326 mov [w0 + #offsetControlHistory], w0 ; Set up pointer to the base of
327 ; controlHistory variables within SMPS_3P3Z_T
328 clr [w0++] ; ControlHistory[n-1] = 0
329 clr [w0++] ; ControlHistory[n-2] = 0
330 clr [w0] ; ControlHistory[n-3] = 0
331 pop w0 ; Restore pointer to base of SMPS_3P3Z_T
332
333 ; Clean up errorHistory variables
334 push w0 ; Save w0
335 mov [w0 + #offsetErrorHistory], w0 ; w0 = Address of _ErrorHistory array (e[n], e[n-1], e[n-2], e[n-3])
336
337 clr [w0++] ; ErrorHistory[n] = 0
338 clr [w0++] ; ErrorHistory[n-1] = 0
339 clr [w0++] ; ErrorHistory[n-2] = 0
340 clr [w0] ; ErrorHistory[n-3] = 0
341 pop w0 ; Restore pointer to base of SMPS_3P3Z_T
342
343 return
344
345
346;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
347
348 .end
349
350;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
351; EOF
352