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