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 * @return none
449 *
450 * The function implements the forward Park transform.
451 *
452 */
arm_park_f32(float32_t Ialpha,float32_t Ibeta,float32_t * pId,float32_t * pIq,float32_t sinVal,float32_t cosVal)453 __STATIC_FORCEINLINE void arm_park_f32(
454 float32_t Ialpha,
455 float32_t Ibeta,
456 float32_t * pId,
457 float32_t * pIq,
458 float32_t sinVal,
459 float32_t cosVal)
460 {
461 /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
462 *pId = Ialpha * cosVal + Ibeta * sinVal;
463
464 /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
465 *pIq = -Ialpha * sinVal + Ibeta * cosVal;
466 }
467
468
469 /**
470 @ingroup park
471 @brief Park transform for Q31 version
472 @param[in] Ialpha input two-phase vector coordinate alpha
473 @param[in] Ibeta input two-phase vector coordinate beta
474 @param[out] pId points to output rotor reference frame d
475 @param[out] pIq points to output rotor reference frame q
476 @param[in] sinVal sine value of rotation angle theta
477 @param[in] cosVal cosine value of rotation angle theta
478 @return none
479
480 \par Scaling and Overflow Behavior
481 The function is implemented using an internal 32-bit accumulator.
482 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
483 There is saturation on the addition and subtraction, hence there is no risk of overflow.
484 */
arm_park_q31(q31_t Ialpha,q31_t Ibeta,q31_t * pId,q31_t * pIq,q31_t sinVal,q31_t cosVal)485 __STATIC_FORCEINLINE void arm_park_q31(
486 q31_t Ialpha,
487 q31_t Ibeta,
488 q31_t * pId,
489 q31_t * pIq,
490 q31_t sinVal,
491 q31_t cosVal)
492 {
493 q31_t product1, product2; /* Temporary variables used to store intermediate results */
494 q31_t product3, product4; /* Temporary variables used to store intermediate results */
495
496 /* Intermediate product is calculated by (Ialpha * cosVal) */
497 product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
498
499 /* Intermediate product is calculated by (Ibeta * sinVal) */
500 product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
501
502
503 /* Intermediate product is calculated by (Ialpha * sinVal) */
504 product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
505
506 /* Intermediate product is calculated by (Ibeta * cosVal) */
507 product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
508
509 /* Calculate pId by adding the two intermediate products 1 and 2 */
510 *pId = __QADD(product1, product2);
511
512 /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
513 *pIq = __QSUB(product4, product3);
514 }
515
516
517
518 /**
519 * @ingroup groupController
520 */
521
522 /**
523 * @defgroup inv_park Vector Inverse Park transform
524 * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
525 *
526 * The function operates on a single sample of data and each call to the function returns the processed output.
527 * The library provides separate functions for Q31 and floating-point data types.
528 * \par Algorithm
529 * \image html parkInvFormula.gif
530 * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
531 * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
532 * cosine and sine values of theta (rotor flux position).
533 * \par Fixed-Point Behavior
534 * Care must be taken when using the Q31 version of the Park transform.
535 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
536 * Refer to the function specific documentation below for usage guidelines.
537 */
538
539
540
541 /**
542 * @ingroup inv_park
543 * @brief Floating-point Inverse Park transform
544 * @param[in] Id input coordinate of rotor reference frame d
545 * @param[in] Iq input coordinate of rotor reference frame q
546 * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
547 * @param[out] pIbeta points to output two-phase orthogonal vector axis beta
548 * @param[in] sinVal sine value of rotation angle theta
549 * @param[in] cosVal cosine value of rotation angle theta
550 * @return none
551 */
arm_inv_park_f32(float32_t Id,float32_t Iq,float32_t * pIalpha,float32_t * pIbeta,float32_t sinVal,float32_t cosVal)552 __STATIC_FORCEINLINE void arm_inv_park_f32(
553 float32_t Id,
554 float32_t Iq,
555 float32_t * pIalpha,
556 float32_t * pIbeta,
557 float32_t sinVal,
558 float32_t cosVal)
559 {
560 /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
561 *pIalpha = Id * cosVal - Iq * sinVal;
562
563 /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
564 *pIbeta = Id * sinVal + Iq * cosVal;
565 }
566
567
568 /**
569 @ingroup inv_park
570 @brief Inverse Park transform for Q31 version
571 @param[in] Id input coordinate of rotor reference frame d
572 @param[in] Iq input coordinate of rotor reference frame q
573 @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
574 @param[out] pIbeta points to output two-phase orthogonal vector axis beta
575 @param[in] sinVal sine value of rotation angle theta
576 @param[in] cosVal cosine value of rotation angle theta
577 @return none
578
579 @par Scaling and Overflow Behavior
580 The function is implemented using an internal 32-bit accumulator.
581 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
582 There is saturation on the addition, hence there is no risk of overflow.
583 */
arm_inv_park_q31(q31_t Id,q31_t Iq,q31_t * pIalpha,q31_t * pIbeta,q31_t sinVal,q31_t cosVal)584 __STATIC_FORCEINLINE void arm_inv_park_q31(
585 q31_t Id,
586 q31_t Iq,
587 q31_t * pIalpha,
588 q31_t * pIbeta,
589 q31_t sinVal,
590 q31_t cosVal)
591 {
592 q31_t product1, product2; /* Temporary variables used to store intermediate results */
593 q31_t product3, product4; /* Temporary variables used to store intermediate results */
594
595 /* Intermediate product is calculated by (Id * cosVal) */
596 product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
597
598 /* Intermediate product is calculated by (Iq * sinVal) */
599 product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
600
601
602 /* Intermediate product is calculated by (Id * sinVal) */
603 product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
604
605 /* Intermediate product is calculated by (Iq * cosVal) */
606 product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
607
608 /* Calculate pIalpha by using the two intermediate products 1 and 2 */
609 *pIalpha = __QSUB(product1, product2);
610
611 /* Calculate pIbeta by using the two intermediate products 3 and 4 */
612 *pIbeta = __QADD(product4, product3);
613 }
614
615
616 /**
617 * @ingroup groupController
618 */
619
620 /**
621 * @defgroup clarke Vector Clarke Transform
622 * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
623 * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
624 * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
625 * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
626 * \image html clarke.gif Stator current space vector and its components in (a,b).
627 * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
628 * can be calculated using only <code>Ia</code> and <code>Ib</code>.
629 *
630 * The function operates on a single sample of data and each call to the function returns the processed output.
631 * The library provides separate functions for Q31 and floating-point data types.
632 * \par Algorithm
633 * \image html clarkeFormula.gif
634 * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
635 * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
636 * \par Fixed-Point Behavior
637 * Care must be taken when using the Q31 version of the Clarke transform.
638 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
639 * Refer to the function specific documentation below for usage guidelines.
640 */
641
642
643 /**
644 *
645 * @ingroup clarke
646 * @brief Floating-point Clarke transform
647 * @param[in] Ia input three-phase coordinate <code>a</code>
648 * @param[in] Ib input three-phase coordinate <code>b</code>
649 * @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
650 * @param[out] pIbeta points to output two-phase orthogonal vector axis beta
651 * @return none
652 */
arm_clarke_f32(float32_t Ia,float32_t Ib,float32_t * pIalpha,float32_t * pIbeta)653 __STATIC_FORCEINLINE void arm_clarke_f32(
654 float32_t Ia,
655 float32_t Ib,
656 float32_t * pIalpha,
657 float32_t * pIbeta)
658 {
659 /* Calculate pIalpha using the equation, pIalpha = Ia */
660 *pIalpha = Ia;
661
662 /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
663 *pIbeta = (0.57735026919f * Ia + 1.15470053838f * Ib);
664 }
665
666
667 /**
668 @ingroup clarke
669 @brief Clarke transform for Q31 version
670 @param[in] Ia input three-phase coordinate <code>a</code>
671 @param[in] Ib input three-phase coordinate <code>b</code>
672 @param[out] pIalpha points to output two-phase orthogonal vector axis alpha
673 @param[out] pIbeta points to output two-phase orthogonal vector axis beta
674 @return none
675
676 \par Scaling and Overflow Behavior
677 The function is implemented using an internal 32-bit accumulator.
678 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
679 There is saturation on the addition, hence there is no risk of overflow.
680 */
arm_clarke_q31(q31_t Ia,q31_t Ib,q31_t * pIalpha,q31_t * pIbeta)681 __STATIC_FORCEINLINE void arm_clarke_q31(
682 q31_t Ia,
683 q31_t Ib,
684 q31_t * pIalpha,
685 q31_t * pIbeta)
686 {
687 q31_t product1, product2; /* Temporary variables used to store intermediate results */
688
689 /* Calculating pIalpha from Ia by equation pIalpha = Ia */
690 *pIalpha = Ia;
691
692 /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
693 product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
694
695 /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
696 product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
697
698 /* pIbeta is calculated by adding the intermediate products */
699 *pIbeta = __QADD(product1, product2);
700 }
701
702
703
704 /**
705 * @ingroup groupController
706 */
707
708 /**
709 * @defgroup inv_clarke Vector Inverse Clarke Transform
710 * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
711 *
712 * The function operates on a single sample of data and each call to the function returns the processed output.
713 * The library provides separate functions for Q31 and floating-point data types.
714 * \par Algorithm
715 * \image html clarkeInvFormula.gif
716 * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
717 * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
718 * \par Fixed-Point Behavior
719 * Care must be taken when using the Q31 version of the Clarke transform.
720 * In particular, the overflow and saturation behavior of the accumulator used must be considered.
721 * Refer to the function specific documentation below for usage guidelines.
722 */
723
724
725
726 /**
727 * @ingroup inv_clarke
728 * @brief Floating-point Inverse Clarke transform
729 * @param[in] Ialpha input two-phase orthogonal vector axis alpha
730 * @param[in] Ibeta input two-phase orthogonal vector axis beta
731 * @param[out] pIa points to output three-phase coordinate <code>a</code>
732 * @param[out] pIb points to output three-phase coordinate <code>b</code>
733 * @return none
734 */
arm_inv_clarke_f32(float32_t Ialpha,float32_t Ibeta,float32_t * pIa,float32_t * pIb)735 __STATIC_FORCEINLINE void arm_inv_clarke_f32(
736 float32_t Ialpha,
737 float32_t Ibeta,
738 float32_t * pIa,
739 float32_t * pIb)
740 {
741 /* Calculating pIa from Ialpha by equation pIa = Ialpha */
742 *pIa = Ialpha;
743
744 /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
745 *pIb = -0.5f * Ialpha + 0.8660254039f * Ibeta;
746 }
747
748
749 /**
750 @ingroup inv_clarke
751 @brief Inverse Clarke transform for Q31 version
752 @param[in] Ialpha input two-phase orthogonal vector axis alpha
753 @param[in] Ibeta input two-phase orthogonal vector axis beta
754 @param[out] pIa points to output three-phase coordinate <code>a</code>
755 @param[out] pIb points to output three-phase coordinate <code>b</code>
756 @return none
757
758 \par Scaling and Overflow Behavior
759 The function is implemented using an internal 32-bit accumulator.
760 The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
761 There is saturation on the subtraction, hence there is no risk of overflow.
762 */
arm_inv_clarke_q31(q31_t Ialpha,q31_t Ibeta,q31_t * pIa,q31_t * pIb)763 __STATIC_FORCEINLINE void arm_inv_clarke_q31(
764 q31_t Ialpha,
765 q31_t Ibeta,
766 q31_t * pIa,
767 q31_t * pIb)
768 {
769 q31_t product1, product2; /* Temporary variables used to store intermediate results */
770
771 /* Calculating pIa from Ialpha by equation pIa = Ialpha */
772 *pIa = Ialpha;
773
774 /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
775 product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
776
777 /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
778 product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
779
780 /* pIb is calculated by subtracting the products */
781 *pIb = __QSUB(product2, product1);
782 }
783
784
785
786
787
788
789 #ifdef __cplusplus
790 }
791 #endif
792
793 #endif /* ifndef _CONTROLLER_FUNCTIONS_H_ */
794
