Bar Logo 3.8/7.6 kw Totem pole Demonstration Application (Part-No. (not specified))
 
Content
      
Loading...
Searching...
No Matches
smps_2p2z_dspic.s
Go to the documentation of this file.
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; Local inclusions.
33 .nolist
34 .list
35
36; Define data structure address offsets
37
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_Controller2P2ZUpdate:
57; Prototype:
58; void SMPS_Controller2P2ZUpdate(SMPS_2P2Z_T* controllerData,
59; volatile uint16_t* controllerInputRegister,int16_t reference,
60; volatile uint16_t* controllerOutputRegister);
61;
62; Operation:
63; Wcp0 ( S + Wcz1 )
64; Calculate the transfer function Hc(s) = ---- X ------------------------
65; S ( S + Wcp1 )( S + Wcp2 )
66; Error Normalization
67; ------------
68; | prsft |
69; ------------
70;Reference | e(n) e(n-1) e(n-2)
71;Input | -------- --------
72; - - - | | | |
73; o----|-|-|x|-|x|----o->| z^(-1) |-o->| z^(-1) |-o Post
74; - - - | | | | | | | Scaling
75; | NORM | -------- | -------- |
76; | | | | --------
77;Measured| --- --- --- | | - - - - - - - - - -
78;Input | B0=>| X | B1=>| X | B2=>| X | |posft | | |
79; | --- --- --- |postScalar| | P O W E R |
80; | | | | ---------- | P L A N T |
81; | ------------------------------------- | | |
82; | | | U[n] --- | |
83; | | A C C U M U L A T O R |--o--| X |------O Duty Ratio=>PWM |
84; | | | | --- | |
85; | ------------------------------------- | | Voltage/Current |
86; | | | | | Feedback |
87; | --- --- | | O |
88; | A2=>| X | A1=>| X | | | | |
89; | --- --- | - - - - - - - - - -
90; | | | | |
91; | | -------- | -------- | | A/D
92; | | | | | | | | | Input
93; | o--| z^(-1) |<-o-| z^(-1) |<----------- |
94; | | | | | |
95; | -------- -------- |
96; | u[n-2] u[n-1] |
97; | |
98; | |
99; ---------------------------------------------------------------------------
100;
101; controlHistory[n] = errorHistory[n] * bCoefficients[0]
102; + errorHistory[n-1] * bCoefficients[1]
103; + errorHistory[n-2] * bCoefficients[2]
104; + controlHistory[n-1] * aCoefficients[0]
105; + controlHistory[n-2] * aCoefficients[1]
106;
107; where:
108;
109; aCoefficients[0] = A1
110; aCoefficients[1] = A2
111; bCoefficients[0] = B0
112; bCoefficients[1] = B1
113; bCoefficients[2] = B2
114; controlHistory[0] = u(n)
115; controlHistory[1] = u(n-1)
116; errorHistory[0] = e(n) => most recent error {(controlReference) - (sourceRegister)}
117; errorHistory[1] = e(n-1)
118; errorHistory[2] = e(n-2)
119; preShift = prsft => pre scaling factor for Q15 normalization purposes
120; postShift = posft => post scaling factor for normalization purposes
121; postScaler = postScaler => post scaling coefficient for normalization purpose
122;
123; Function call inputs:
124; sourceRegister = ADBUFx => measured feedback input
125; controlReference = V_ref => reference voltage
126; targetRegister = PDCx => output duty cycle
127;
128; where:
129;
130; aCoefficients, bCoefficients, controlHistory, errorHistory,
131; preShift and postShift are all members of the data structure SMPS_2P2Z_T.
132;
133; Input:
134; w0 = Address of SMPS_2P2Z_T data structure
135; w1 = Address of the Source Register (Input)
136; w2 = Control Reference
137; w3 = Address of the Target Register (Output)
138; Return:
139; w0 = Address of SMPS_2P2Z_T data structure
140; w1 = Address of the Source Register (Input)
141; w2 = Control Reference
142; w3 = Address of the Target Register (Output)
143;
144; System resources usage:
145; {w4..w5} saved, used, restored
146; {w8,w10} saved, used, restored
147; AccA used, not restored
148; CORCON saved, used, restored
149;
150; Note
151; This function requires CORCON register to be setup in a certain state
152; in order to operate correctly. Due to this requirement, this function
153; will save the CORCON register on the stack in the beginning of the
154; function and restore it before the function return.
155; After saving the CORCON register, this function writes to all bits of
156; the CORCON register. Thus, for the brief duration when this function is
157; executing, the state of CORCON register may be different from its state
158; as set by the function caller. This may change the CPU core behaviour with
159; respect to exception processing latency, DO loop termination, CPU interrupt
160; priority level and DSP-engine behaviour.
161;............................................................................
162
163 .global _SMPS_Controller2P2ZUpdate ; provide global scope to routine
164_SMPS_Controller2P2ZUpdate:
165
166 ; Save working registers.
167
168 push w4 ; Save register W4
169 push w5 ; Save register W5
170 push w8 ; Save register W8
171 push w10 ; Save register W10
172 push CORCON ; Save CORCON as it will be modified for fractional computation
173
174 ; Set up DSP core for signed fractional operation, saturation on accumulator A
175 ; and for write-back instructions from ACCx to WREGx,
176 ; accumulator saturation mode is 1.31
177
178 mov #0x00A0, w4 ; Load literal value 0x00A0 CORCON register: 0b 0000 0000 1010 0000
179 mov w4, _CORCON
180
181 ; Initialize source input, reference, error history and normalization variables
182
183 mov [w0 + #offsetErrorHistory], w10 ; w10 = Base Address of _ErrorHistory array (e[n], e[n-1], e[n-2], e[n-3])
184 mov [w0 + #offsetPreShift], w8 ; Load error amplifier normalization bit-shift step
185
186 ; Update the error history
187
188 mov [w10 + #2], w5 ; W5 = ErrorHistory[n-1]
189 mov w5, [w10 + #4] ; ErrorHistory[n-2] = W5
190 mov [w10 + #0], w5 ; W5 = ErrorHistory[n]
191 mov w5, [w10 + #2] ; ErrorHistory[n-1] = W5
192
193 ; Calculate most recent error with normalization,
194
195 sub w2, [w1], w5 ; w5 = Reference - inputSource
196 sl w5, w8, w5 ; shift error by PRE_SHIFT bit to the left (Q15 scaling)
197
198 ; Store most recent error to error history array
199 mov w5, [w10] ; controller2P2Z.errorHistory[n] = w5
200
201 ; Initialize B coefficients array
202
203 mov [w0 + #offsetBCoefficients], w8 ; w8 = Base Address of _BCoefficients array (B0, B1, B2, B3)
204
205 ; Calculate first section of 2P2Z controller -> Error History
206 clr a, [w8]+=2, w4, [w10]+=2, w5 ; w4 = B0
207 ; w5 = e[0]
208 mac w4*w5, a, [w8]+=2, w4, [w10]+=2, w5 ; ACCA = B0 * e[n]
209 ; w4 = B1, w5 = e[n-1]
210 mac w4*w5, a, [w8], w4, [w10], w5 ; ACCA = B0 * e[n] + B1 * e[n-1]
211 ; w4 = B2, w5 = e[n-2]
212 mac w4*w5, a ; ACCA = B0 * e[n] + B1 * e[n-1] + B2 * e[n-2]
213
214 ; Initialize A coefficients and control history arrays
215
216 mov [w0 + #offsetACoefficients], w8 ; w8 = Base Address of _ACoefficients array (A1, A2, A3)
217 mov [w0 + #offsetControlHistory], w10 ; w10 = Base Address of _ControlHistory array (y[n-1], y[n-2], y[n-3])
218
219 ; Calculate second section of 2P2Z controller - Controller History
220
221 movsac a, [w8]+=2, w4, [w10]+=2, w5 ; w4 = A1, w5 = y[n-1]
222 mac w4*w5, a, [w8], w4, [w10], w5 ; ACCA = A1 * y[n-1] + B0 * e[n] + B1 * e[n-1] + B2 * e[n-2]
223 ; w4 = A2, w5 = y[n-2]
224 mac w4*w5, a ; ACCA = A1 * y[n-1] + A2 * y[n-2] + B0 * e[n] + B1 * e[n-1] + B2 * e[n-2]
225
226 ; Initialize Scale-factor and multiply
227 sac.r a, w4 ; w4 = Sat(Rnd(ACCAH))
228 mov [w0 + #offsetPostScaler], w5 ; w5 = postScaler
229 mpy w4*w5, a ; Multiply control output and scale-factor
230
231
232 ; Backwards normalization & write back
233 mov [w0 + #offsetPostShift], w5 ; w5 = Normalization Shift to compensate coefficient scaling
234 sftac a, w5 ; Backward normalization to compensate coefficient scaling
235 sac.r a, w4 ; w4 = Sat(Rnd(ACCAH))
236
237
238
239
240
241
242 ; Clamp controller output to min/max values if needed
243 mov [w0 + #offsetMinOutput], w5
244 cpsgt w4, w5
245 mov.w w5, w4
246 mov [w0 + #offsetMaxOutput], w5
247 cpslt w4, w5
248 mov.w w5, w4
249
250
251
252
253
254
255
256 ; Update the controller output history on the delay line
257
258 mov [w10 + #-2], w5 ; W5 = ControlHistory[n-1]
259 mov w5, [w10] ; ControlHistory[n-2] = W5
260 mov w4, [w10 + #-2] ; ControlHistory[n] = w2
261
262; ; Clamp controller output to min/max values if needed
263; mov [w0 + #offsetMinOutput], w5
264; cpsgt w4, w5
265; mov.w w5, w4
266; mov [w0 + #offsetMaxOutput], w5
267; cpslt w4, w5
268; mov.w w5, w4
269
270 ; Controller write back into target register
271 mov w4, [w3] ; write result into target register
272
273 ; restore registers
274
275 pop CORCON ; restore CORCON.
276 pop w10 ; restore working register W10
277 pop w8 ; restore working register W8
278 pop w5 ; restore working register W5
279 pop w4 ; restore working register W4
280 pop.s ; restore working registers W0...W3
281
282
283 return
284
285;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
286
287;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
288; _SMPS_Controller2P2ZInitialize:
289;
290; Prototype:
291; void SMPS_Controller2P2ZInitialize( SMPS_2P2Z_T *controllerData )
292;
293; Operation: This routine clears the delay line elements in the arrays
294; _ControlHistory and _ErrorHistory.
295;
296; Input:
297; w0 = Address of data structure SMPS_2P2Z_T (type defined in smps_control.h)
298;
299; Return:
300; (void)
301;
302; System resources usage:
303; w0 saved, used, restored
304;
305;............................................................................
306
307 .global _SMPS_Controller2P2ZInitialize ; provide global scope to routine
308
309_SMPS_Controller2P2ZInitialize:
310
311 ; Clear controlHistory variables
312 push w0 ; Save w0
313 mov [w0 + #offsetControlHistory], w0 ; Set up pointer to the base of
314 ; controlHistory variables within SMPS_2P2Z_T
315 clr [w0++] ; ControlHistory[n-1] = 0
316 clr [w0] ; ControlHistory[n-2] = 0
317 pop w0 ; Restore pointer to base of SMPS_2P2Z_T
318
319 ; Clean up errorHistory variables
320 push w0 ; Save w0
321 mov [w0 + #offsetErrorHistory], w0 ; w0 = Address of _ErrorHistory array (e[n], e[n-1], e[n-2], e[n-3])
322
323 clr [w0++] ; ErrorHistory[n] = 0
324 clr [w0++] ; ErrorHistory[n-1] = 0
325 clr [w0] ; ErrorHistory[n-2] = 0
326 pop w0 ; Restore pointer to base of SMPS_2P2Z_T
327
328
329 return
330;
331;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
332
333 .end
334
335;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
336; EOF
337