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