1 /******************************************************************************
2 * @file controller_functions.h
3 * @brief Public header file for CMSIS DSP Library
4 * @version V1.10.0
5 * @date 08 July 2021
6 * Target Processor: Cortex-M and Cortex-A cores
7 ******************************************************************************/
8 /*
9 * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
10 *
11 * SPDX-License-Identifier: Apache-2.0
12 *
13 * Licensed under the Apache License, Version 2.0 (the License); you may
14 * not use this file except in compliance with the License.
15 * You may obtain a copy of the License at
16 *
17 * www.apache.org/licenses/LICENSE-2.0
18 *
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
21 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
24 */
25
26
27 #ifndef _CONTROLLER_FUNCTIONS_H_
28 #define _CONTROLLER_FUNCTIONS_H_
29
30 #include "arm_math_types.h"
31 #include "arm_math_memory.h"
32
33 #include "dsp/none.h"
34 #include "dsp/utils.h"
35
36 #ifdef __cplusplus
37 extern "C"
38 {
39 #endif
40
41 /**
42 * @brief Macros required for SINE and COSINE Controller functions
43 */
44
45 #define CONTROLLER_Q31_SHIFT (32 - 9)
46 /* 1.31(q31) Fixed value of 2/360 */
47 /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
48 #define INPUT_SPACING 0xB60B61
49
50 /**
51 * @defgroup groupController Controller Functions
52 */
53
54
55 /**
56 @ingroup groupController
57 */
58
59 /**
60 @defgroup SinCos Sine Cosine
61
62 Computes the trigonometric sine and cosine values using a combination of table lookup
63 and linear interpolation.
64 There are separate functions for Q31 and floating-point data types.
65 The input to the floating-point version is in degrees while the
66 fixed-point Q31 have a scaled input with the range
67 [-1 0.9999] mapping to [-180 +180] degrees.
68
69 The floating point function also allows values that are out of the usual range. When this happens, the function will
70 take extra time to adjust the input value to the range of [-180 180].
71
72 The result is accurate to 5 digits after the decimal point.
73
74 The implementation is based on table lookup using 360 values together with linear interpolation.
75 The steps used are:
76 -# Calculation of the nearest integer table index.
77 -# Compute the fractional portion (fract) of the input.
78 -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.
79 -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.
80 -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.
81 -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.
82 */
83
84 /**
85 * @brief Floating-point sin_cos function.
86 * @param[in] theta input value in degrees
87 * @param[out] pSinVal points to the processed sine output.
88 * @param[out] pCosVal points to the processed cos output.
89 */
90 void arm_sin_cos_f32(
91 float32_t theta,
92 float32_t * pSinVal,
93 float32_t * pCosVal);
94
95
96 /**
97 * @brief Q31 sin_cos function.
98 * @param[in] theta scaled input value in degrees
99 * @param[out] pSinVal points to the processed sine output.
100 * @param[out] pCosVal points to the processed cosine output.
101 */
102 void arm_sin_cos_q31(
103 q31_t theta,
104 q31_t * pSinVal,
105 q31_t * pCosVal);
106
107
108 /**
109 @ingroup groupController
110 */
111
112 /**
113 * @defgroup PID PID Motor Control
114 *
115 * A Proportional Integral Derivative (PID) controller is a generic feedback control
116 * loop mechanism widely used in industrial control systems.
117 * A PID controller is the most commonly used type of feedback controller.
118 *
119 * This set of functions implements (PID) controllers
120 * for Q15, Q31, and floating-point data types. The functions operate on a single sample
121 * of data and each call to the function returns a single processed value.
122 * <code>S</code> points to an instance of the PID control data structure. <code>in</code>
123 * is the input sample value. The functions return the output value.
124 *
125 * \par Algorithm:
126 * <pre>
127 * y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
128 * A0 = Kp + Ki + Kd
129 * A1 = (-Kp ) - (2 * Kd )
130 * A2 = Kd
131 * </pre>
132 *
133 * \par
134 * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
135 *
136 * \par
137 * \image html PID.gif "Proportional Integral Derivative Controller"
138 *
139 * \par
140 * The PID controller calculates an "error" value as the difference between
141 * the measured output and the reference input.
142 * The controller attempts to minimize the error by adjusting the process control inputs.
143 * The proportional value determines the reaction to the current error,
144 * the integral value determines the reaction based on the sum of recent errors,
145 * and the derivative value determines the reaction based on the rate at which the error has been changing.
146 *
147 * \par Instance Structure
148 * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
149 * A separate instance structure must be defined for each PID Controller.
150 * There are separate instance structure declarations for each of the 3 supported data types.
151 *
152 * \par Reset Functions
153 * There is also an associated reset function for each data type which clears the state array.
154 *
155 * \par Initialization Functions
156 * There is also an associated initialization function for each data type.
157 * The initialization function performs the following operations:
158 * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
159 * - Zeros out the values in the state buffer.
160 *
161 * \par
162 * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
163 *
164 * \par Fixed-Point Behavior
165 * Care must be taken when using the fixed-point versions of the PID Controller functions.
166 * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
167 * Refer to the function specific documentation below for usage guidelines.
168 */
169
170
171 /**
172 * @ingroup PID
173 * @brief Instance structure for the Q15 PID Control.
174 */
175 typedef struct
176 {
177 q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
178 #if !defined (ARM_MATH_DSP)
179 q15_t A1; /**< The derived gain A1 = -Kp - 2Kd */
180 q15_t A2; /**< The derived gain A1 = Kd. */
181 #else
182 q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
183 #endif
184 q15_t state[3]; /**< The state array of length 3. */
185 q15_t Kp; /**< The proportional gain. */
186 q15_t Ki; /**< The integral gain. */
187 q15_t Kd; /**< The derivative gain. */
188 } arm_pid_instance_q15;
189
190 /**
191 * @ingroup PID
192 * @brief Instance structure for the Q31 PID Control.
193 */
194 typedef struct
195 {
196 q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
197 q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
198 q31_t A2; /**< The derived gain, A2 = Kd . */
199 q31_t state[3]; /**< The state array of length 3. */
200 q31_t Kp; /**< The proportional gain. */
201 q31_t Ki; /**< The integral gain. */
202 q31_t Kd; /**< The derivative gain. */
203 } arm_pid_instance_q31;
204
205 /**
206 * @ingroup PID
207 * @brief Instance structure for the floating-point PID Control.
208 */
209 typedef struct
210 {
211 float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
212 float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
213 float32_t A2; /**< The derived gain, A2 = Kd . */
214 float32_t state[3]; /**< The state array of length 3. */
215 float32_t Kp; /**< The proportional gain. */
216 float32_t Ki; /**< The integral gain. */
217 float32_t Kd; /**< The derivative gain. */
218 } arm_pid_instance_f32;
219
220
221
222 /**
223 * @brief Initialization function for the floating-point PID Control.
224 * @param[in,out] S points to an instance of the PID structure.
225 * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
226 */
227 void arm_pid_init_f32(
228 arm_pid_instance_f32 * S,
229 int32_t resetStateFlag);
230
231
232 /**
233 * @brief Reset function for the floating-point PID Control.
234 * @param[in,out] S is an instance of the floating-point PID Control structure
235 */
236 void arm_pid_reset_f32(
237 arm_pid_instance_f32 * S);
238
239
240 /**
241 * @brief Initialization function for the Q31 PID Control.
242 * @param[in,out] S points to an instance of the Q15 PID structure.
243 * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
244 */
245 void arm_pid_init_q31(
246 arm_pid_instance_q31 * S,
247 int32_t resetStateFlag);
248
249
250 /**
251 * @brief Reset function for the Q31 PID Control.
252 * @param[in,out] S points to an instance of the Q31 PID Control structure
253 */
254
255 void arm_pid_reset_q31(
256 arm_pid_instance_q31 * S);
257
258
259 /**
260 * @brief Initialization function for the Q15 PID Control.
261 * @param[in,out] S points to an instance of the Q15 PID structure.
262 * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
263 */
264 void arm_pid_init_q15(
265 arm_pid_instance_q15 * S,
266 int32_t resetStateFlag);
267
268
269 /**
270 * @brief Reset function for the Q15 PID Control.
271 * @param[in,out] S points to an instance of the q15 PID Control structure
272 */
273 void arm_pid_reset_q15(
274 arm_pid_instance_q15 * S);
275
276
277
278
279
280 /**
281 * @ingroup PID
282 * @brief Process function for the floating-point PID Control.
283 * @param[in,out] S is an instance of the floating-point PID Control structure
284 * @param[in] in input sample to process
285 * @return processed output sample.
286 */
arm_pid_f32(arm_pid_instance_f32 * S,float32_t in)287 __STATIC_FORCEINLINE float32_t arm_pid_f32(
288 arm_pid_instance_f32 * S,
289 float32_t in)
290 {
291 float32_t out;
292
293 /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
294 out = (S->A0 * in) +
295 (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
296
297 /* Update state */
298 S->state[1] = S->state[0];
299 S->state[0] = in;
300 S->state[2] = out;
301
302 /* return to application */
303 return (out);
304
305 }
306
307 /**
308 @ingroup PID
309 @brief Process function for the Q31 PID Control.
310 @param[in,out] S points to an instance of the Q31 PID Control structure
311 @param[in] in input sample to process
312 @return processed output sample.
313
314 \par Scaling and Overflow Behavior
315 The function is implemented using an internal 64-bit accumulator.
316 The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
317 Thus, if the accumulator result overflows it wraps around rather than clip.
318 In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.
319 After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
320 */
arm_pid_q31(arm_pid_instance_q31 * S,q31_t in)321 __STATIC_FORCEINLINE q31_t arm_pid_q31(
322 arm_pid_instance_q31 * S,
323 q31_t in)
324 {
325 q63_t acc;
326 q31_t out;
327
328 /* acc = A0 * x[n] */
329 acc = (q63_t) S->A0 * in;
330
331 /* acc += A1 * x[n-1] */
332 acc += (q63_t) S->A1 * S->state[0];
333
334 /* acc += A2 * x[n-2] */
335 acc += (q63_t) S->A2 * S->state[1];
336
337 /* convert output to 1.31 format to add y[n-1] */
338 out = (q31_t) (acc >> 31U);
339
340 /* out += y[n-1] */
341 out += S->state[2];
342
343 /* Update state */
344 S->state[1] = S->state[0];
345 S->state[0] = in;
346 S->state[2] = out;
347
348 /* return to application */
349 return (out);
350 }
351
352
353 /**
354 @ingroup PID
355 @brief Process function for the Q15 PID Control.
356 @param[in,out] S points to an instance of the Q15 PID Control structure
357 @param[in] in input sample to process
358 @return processed output sample.
359
360 \par Scaling and Overflow Behavior
361 The function is implemented using a 64-bit internal accumulator.
362 Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
363 The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
364 There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
365 After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
366 Lastly, the accumulator is saturated to yield a result in 1.15 format.
367 */
arm_pid_q15(arm_pid_instance_q15 * S,q15_t in)368 __STATIC_FORCEINLINE q15_t arm_pid_q15(
369 arm_pid_instance_q15 * S,
370 q15_t in)
371 {
372 q63_t acc;
373 q15_t out;
374
375 #if defined (ARM_MATH_DSP)
376 /* Implementation of PID controller */
377
378 /* acc = A0 * x[n] */
379 acc = (q31_t) __SMUAD((uint32_t)S->A0, (uint32_t)in);
380
381 /* acc += A1 * x[n-1] + A2 * x[n-2] */
382 acc = (q63_t)__SMLALD((uint32_t)S->A1, (uint32_t)read_q15x2 (S->state), (uint64_t)acc);
383 #else
384 /* acc = A0 * x[n] */
385 acc = ((q31_t) S->A0) * in;
386
387 /* acc += A1 * x[n-1] + A2 * x[n-2] */
388 acc += (q31_t) S->A1 * S->state[0];
389 acc += (q31_t) S->A2 * S->state[1];
390 #endif
391
392 /* acc += y[n-1] */
393 acc += (q31_t) S->state[2] << 15;
394
395 /* saturate the output */
396 out = (q15_t) (__SSAT((q31_t)(acc >> 15), 16));
397
398 /* Update state */
399 S->state[1] = S->state[0];
400 S->state[0] = in;
401 S->state[2] = out;
402
403 /* return to application */
404 return (out);
405 }
406
407
408
409 /**
410 * @ingroup groupController
411 */
412
413 /**
414 * @defgroup park Vector Park Transform
415 *
416 * Forward Park transform converts the input two-coordinate vector to flux and torque components.
417 * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
418 * from the stationary to the moving reference frame and control the spatial relationship between
419 * the stator vector current and rotor flux vector.
420 * If we consider the d axis aligned with the rotor flux, the diagram below shows the
421 * current vector and the relationship from the two reference frames:
422 * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
423 *
424 * The function operates on a single sample of data and each call to the function returns the processed output.
425 * The library provides separate functions for Q31 and floating-point data types.
426 * \par Algorithm
427 * \image html parkFormula.gif
428 * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
429 * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
430 * cosine and sine values of theta (rotor flux position).
431 * \par Fixed-Point Behavior
432 * Care must be taken when using the Q31 version of the Park transform.
433 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
434 * Refer to the function specific documentation below for usage guidelines.
435 */
436
437
438
439 /**
440 * @ingroup park
441 * @brief Floating-point Park transform
442 * @param[in] Ialpha input two-phase vector coordinate alpha
443 * @param[in] Ibeta input two-phase vector coordinate beta
444 * @param[out] pId points to output rotor reference frame d
445 * @param[out] pIq points to output rotor reference frame q
446 * @param[in] sinVal sine value of rotation angle theta
447 * @param[in] cosVal cosine value of rotation angle theta
448 *
449 * The function implements the forward Park transform.
450 *
451 */
arm_park_f32(float32_t Ialpha,float32_t Ibeta,float32_t * pId,float32_t * pIq,float32_t sinVal,float32_t cosVal)452 __STATIC_FORCEINLINE void arm_park_f32(
453 float32_t Ialpha,
454 float32_t Ibeta,
455 float32_t * pId,
456 float32_t * pIq,
457 float32_t sinVal,
458 float32_t cosVal)
459 {
460 /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
461 *pId = Ialpha * cosVal + Ibeta * sinVal;
462
463 /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
464 *pIq = -Ialpha * sinVal + Ibeta * cosVal;
465 }
466
467
468 /**
469 @ingroup park
470 @brief Park transform for Q31 version
471 @param[in] Ialpha input two-phase vector coordinate alpha
472 @param[in] Ibeta input two-phase vector coordinate beta
473 @param[out] pId points to output rotor reference frame d
474 @param[out] pIq points to output rotor reference frame q
475 @param[in] sinVal sine value of rotation angle theta
476 @param[in] cosVal cosine value of rotation angle theta
477
478 \par Scaling and Overflow Behavior
479 The function is implemented using an internal 32-bit accumulator.
480 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
481 There is saturation on the addition and subtraction, hence there is no risk of overflow.
482 */
arm_park_q31(q31_t Ialpha,q31_t Ibeta,q31_t * pId,q31_t * pIq,q31_t sinVal,q31_t cosVal)483 __STATIC_FORCEINLINE void arm_park_q31(
484 q31_t Ialpha,
485 q31_t Ibeta,
486 q31_t * pId,
487 q31_t * pIq,
488 q31_t sinVal,
489 q31_t cosVal)
490 {
491 q31_t product1, product2; /* Temporary variables used to store intermediate results */
492 q31_t product3, product4; /* Temporary variables used to store intermediate results */
493
494 /* Intermediate product is calculated by (Ialpha * cosVal) */
495 product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
496
497 /* Intermediate product is calculated by (Ibeta * sinVal) */
498 product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
499
500
501 /* Intermediate product is calculated by (Ialpha * sinVal) */
502 product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
503
504 /* Intermediate product is calculated by (Ibeta * cosVal) */
505 product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
506
507 /* Calculate pId by adding the two intermediate products 1 and 2 */
508 *pId = __QADD(product1, product2);
509
510 /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
511 *pIq = __QSUB(product4, product3);
512 }
513
514
515
516 /**
517 * @ingroup groupController
518 */
519
520 /**
521 * @defgroup inv_park Vector Inverse Park transform
522 * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
523 *
524 * The function operates on a single sample of data and each call to the function returns the processed output.
525 * The library provides separate functions for Q31 and floating-point data types.
526 * \par Algorithm
527 * \image html parkInvFormula.gif
528 * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
529 * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
530 * cosine and sine values of theta (rotor flux position).
531 * \par Fixed-Point Behavior
532 * Care must be taken when using the Q31 version of the Park transform.
533 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
534 * Refer to the function specific documentation below for usage guidelines.
535 */
536
537
538
539 /**
540 * @ingroup inv_park
541 * @brief Floating-point Inverse Park transform
542 * @param[in] Id input coordinate of rotor reference frame d
543 * @param[in] Iq input coordinate of rotor reference frame q
544 * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
545 * @param[out] pIbeta points to output two-phase orthogonal vector axis beta
546 * @param[in] sinVal sine value of rotation angle theta
547 * @param[in] cosVal cosine value of rotation angle theta
548 */
arm_inv_park_f32(float32_t Id,float32_t Iq,float32_t * pIalpha,float32_t * pIbeta,float32_t sinVal,float32_t cosVal)549 __STATIC_FORCEINLINE void arm_inv_park_f32(
550 float32_t Id,
551 float32_t Iq,
552 float32_t * pIalpha,
553 float32_t * pIbeta,
554 float32_t sinVal,
555 float32_t cosVal)
556 {
557 /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
558 *pIalpha = Id * cosVal - Iq * sinVal;
559
560 /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
561 *pIbeta = Id * sinVal + Iq * cosVal;
562 }
563
564
565 /**
566 @ingroup inv_park
567 @brief Inverse Park transform for Q31 version
568 @param[in] Id input coordinate of rotor reference frame d
569 @param[in] Iq input coordinate of rotor reference frame q
570 @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
571 @param[out] pIbeta points to output two-phase orthogonal vector axis beta
572 @param[in] sinVal sine value of rotation angle theta
573 @param[in] cosVal cosine value of rotation angle theta
574
575 @par Scaling and Overflow Behavior
576 The function is implemented using an internal 32-bit accumulator.
577 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
578 There is saturation on the addition, hence there is no risk of overflow.
579 */
arm_inv_park_q31(q31_t Id,q31_t Iq,q31_t * pIalpha,q31_t * pIbeta,q31_t sinVal,q31_t cosVal)580 __STATIC_FORCEINLINE void arm_inv_park_q31(
581 q31_t Id,
582 q31_t Iq,
583 q31_t * pIalpha,
584 q31_t * pIbeta,
585 q31_t sinVal,
586 q31_t cosVal)
587 {
588 q31_t product1, product2; /* Temporary variables used to store intermediate results */
589 q31_t product3, product4; /* Temporary variables used to store intermediate results */
590
591 /* Intermediate product is calculated by (Id * cosVal) */
592 product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
593
594 /* Intermediate product is calculated by (Iq * sinVal) */
595 product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
596
597
598 /* Intermediate product is calculated by (Id * sinVal) */
599 product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
600
601 /* Intermediate product is calculated by (Iq * cosVal) */
602 product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
603
604 /* Calculate pIalpha by using the two intermediate products 1 and 2 */
605 *pIalpha = __QSUB(product1, product2);
606
607 /* Calculate pIbeta by using the two intermediate products 3 and 4 */
608 *pIbeta = __QADD(product4, product3);
609 }
610
611
612 /**
613 * @ingroup groupController
614 */
615
616 /**
617 * @defgroup clarke Vector Clarke Transform
618 * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
619 * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
620 * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
621 * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
622 * \image html clarke.gif Stator current space vector and its components in (a,b).
623 * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
624 * can be calculated using only <code>Ia</code> and <code>Ib</code>.
625 *
626 * The function operates on a single sample of data and each call to the function returns the processed output.
627 * The library provides separate functions for Q31 and floating-point data types.
628 * \par Algorithm
629 * \image html clarkeFormula.gif
630 * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
631 * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
632 * \par Fixed-Point Behavior
633 * Care must be taken when using the Q31 version of the Clarke transform.
634 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
635 * Refer to the function specific documentation below for usage guidelines.
636 */
637
638
639 /**
640 *
641 * @ingroup clarke
642 * @brief Floating-point Clarke transform
643 * @param[in] Ia input three-phase coordinate <code>a</code>
644 * @param[in] Ib input three-phase coordinate <code>b</code>
645 * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
646 * @param[out] pIbeta points to output two-phase orthogonal vector axis beta
647 */
arm_clarke_f32(float32_t Ia,float32_t Ib,float32_t * pIalpha,float32_t * pIbeta)648 __STATIC_FORCEINLINE void arm_clarke_f32(
649 float32_t Ia,
650 float32_t Ib,
651 float32_t * pIalpha,
652 float32_t * pIbeta)
653 {
654 /* Calculate pIalpha using the equation, pIalpha = Ia */
655 *pIalpha = Ia;
656
657 /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
658 *pIbeta = (0.57735026919f * Ia + 1.15470053838f * Ib);
659 }
660
661
662 /**
663 @ingroup clarke
664 @brief Clarke transform for Q31 version
665 @param[in] Ia input three-phase coordinate <code>a</code>
666 @param[in] Ib input three-phase coordinate <code>b</code>
667 @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
668 @param[out] pIbeta points to output two-phase orthogonal vector axis beta
669
670 \par Scaling and Overflow Behavior
671 The function is implemented using an internal 32-bit accumulator.
672 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
673 There is saturation on the addition, hence there is no risk of overflow.
674 */
arm_clarke_q31(q31_t Ia,q31_t Ib,q31_t * pIalpha,q31_t * pIbeta)675 __STATIC_FORCEINLINE void arm_clarke_q31(
676 q31_t Ia,
677 q31_t Ib,
678 q31_t * pIalpha,
679 q31_t * pIbeta)
680 {
681 q31_t product1, product2; /* Temporary variables used to store intermediate results */
682
683 /* Calculating pIalpha from Ia by equation pIalpha = Ia */
684 *pIalpha = Ia;
685
686 /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
687 product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
688
689 /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
690 product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
691
692 /* pIbeta is calculated by adding the intermediate products */
693 *pIbeta = __QADD(product1, product2);
694 }
695
696
697
698 /**
699 * @ingroup groupController
700 */
701
702 /**
703 * @defgroup inv_clarke Vector Inverse Clarke Transform
704 * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
705 *
706 * The function operates on a single sample of data and each call to the function returns the processed output.
707 * The library provides separate functions for Q31 and floating-point data types.
708 * \par Algorithm
709 * \image html clarkeInvFormula.gif
710 * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
711 * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
712 * \par Fixed-Point Behavior
713 * Care must be taken when using the Q31 version of the Clarke transform.
714 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
715 * Refer to the function specific documentation below for usage guidelines.
716 */
717
718
719
720 /**
721 * @ingroup inv_clarke
722 * @brief Floating-point Inverse Clarke transform
723 * @param[in] Ialpha input two-phase orthogonal vector axis alpha
724 * @param[in] Ibeta input two-phase orthogonal vector axis beta
725 * @param[out] pIa points to output three-phase coordinate <code>a</code>
726 * @param[out] pIb points to output three-phase coordinate <code>b</code>
727 */
arm_inv_clarke_f32(float32_t Ialpha,float32_t Ibeta,float32_t * pIa,float32_t * pIb)728 __STATIC_FORCEINLINE void arm_inv_clarke_f32(
729 float32_t Ialpha,
730 float32_t Ibeta,
731 float32_t * pIa,
732 float32_t * pIb)
733 {
734 /* Calculating pIa from Ialpha by equation pIa = Ialpha */
735 *pIa = Ialpha;
736
737 /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
738 *pIb = -0.5f * Ialpha + 0.8660254039f * Ibeta;
739 }
740
741
742 /**
743 @ingroup inv_clarke
744 @brief Inverse Clarke transform for Q31 version
745 @param[in] Ialpha input two-phase orthogonal vector axis alpha
746 @param[in] Ibeta input two-phase orthogonal vector axis beta
747 @param[out] pIa points to output three-phase coordinate <code>a</code>
748 @param[out] pIb points to output three-phase coordinate <code>b</code>
749
750 \par Scaling and Overflow Behavior
751 The function is implemented using an internal 32-bit accumulator.
752 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
753 There is saturation on the subtraction, hence there is no risk of overflow.
754 */
arm_inv_clarke_q31(q31_t Ialpha,q31_t Ibeta,q31_t * pIa,q31_t * pIb)755 __STATIC_FORCEINLINE void arm_inv_clarke_q31(
756 q31_t Ialpha,
757 q31_t Ibeta,
758 q31_t * pIa,
759 q31_t * pIb)
760 {
761 q31_t product1, product2; /* Temporary variables used to store intermediate results */
762
763 /* Calculating pIa from Ialpha by equation pIa = Ialpha */
764 *pIa = Ialpha;
765
766 /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
767 product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
768
769 /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
770 product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
771
772 /* pIb is calculated by subtracting the products */
773 *pIb = __QSUB(product2, product1);
774 }
775
776
777
778
779
780
781 #ifdef __cplusplus
782 }
783 #endif
784
785 #endif /* ifndef _CONTROLLER_FUNCTIONS_H_ */
786
