From 375c36f971bdb853403bbc33d78bbde114b1ee0e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 30 Jun 2023 00:50:48 +0200 Subject: [PATCH] change sine implementation to deku65i version --- src/BLDCMotor.cpp | 6 +-- src/StepperMotor.cpp | 7 +--- src/common/base_classes/CurrentSense.cpp | 13 ++++-- src/common/foc_utils.cpp | 53 ++++++++++++------------ src/common/foc_utils.h | 7 ++++ 5 files changed, 46 insertions(+), 40 deletions(-) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 45108377..80dacbc3 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -539,12 +539,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { case FOCModulationType::SinePWM : // Sinusoidal PWM modulation // Inverse Park + Clarke transformation + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - _ca = _cos(angle_el); - _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index f1e51cce..16df4236 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -351,12 +351,9 @@ void StepperMotor::move(float new_target) { void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { // Sinusoidal PWM modulation // Inverse Park transformation + float _sa, _ca; + _sincos(angle_el, &_sa, &_ca); - // angle normalization in between 0 and 2pi - // only necessary if using _sin and _cos - approximation functions - angle_el = _normalizeAngle(angle_el); - float _ca = _cos(angle_el); - float _sa = _sin(angle_el); // Inverse park transform Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index becfa499..609162c2 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -38,8 +38,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){ // if motor angle provided function returns signed value of the current // determine the sign of the current // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1 - if(motor_electrical_angle) - sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1; + if(motor_electrical_angle) { + float ct; + float st; + _sincos(motor_electrical_angle, &st, &ct); + sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1; + } // return current magnitude return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta); } @@ -78,8 +82,9 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ } // calculate park transform - float ct = _cos(angle_el); - float st = _sin(angle_el); + float ct; + float st; + _sincos(angle_el, &st, &ct); DQCurrent_s return_current; return_current.d = i_alpha * ct + i_beta * st; return_current.q = i_beta * ct - i_alpha * st; diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index c38fd897..93e742a5 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -2,34 +2,29 @@ // function approximating the sine calculation by using fixed size array -// ~40us (float array) -// ~50us (int array) -// precision +-0.005 -// it has to receive an angle in between 0 and 2PI +// uses a 65 element lookup table and interpolation +// thanks to @dekutree for his work on optimizing this __attribute__((weak)) float _sin(float a){ - // int array instead of float array - // 4x200 points per 360 deg - // 2x storage save (int 2Byte float 4 Byte ) - // sin*10000 - static const uint16_t sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; - - if(a < _PI_2){ - //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873f* a)]; // float array optimized - return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized - }else if(a < _PI){ - // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized - return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized - }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized - } else { - // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized - return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized + // 16bit integer array for sine lookup. interpolation is used for better precision + // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size + // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps) + static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768}; + unsigned int i = (unsigned int)(a * (64*4*256 /_2PI)); + int t1, t2, frac = i & 0xff; + i = (i >> 8) & 0xff; + if (i < 64) { + t1 = sine_array[i]; t2 = sine_array[i+1]; + } + else if(i < 128) { + t1 = sine_array[128 - i]; t2 = sine_array[127 - i]; + } + else if(i < 196) { + t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i]; } + else { + t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i]; + } + return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8)); } // function approximating cosine calculation by using fixed size array @@ -44,6 +39,12 @@ __attribute__((weak)) float _cos(float a){ } +__attribute__((weak)) void _sincos(float a, float* s, float* c){ + *s = _sin(a); + *c = _cos(a); +} + + // normalizing radian angle to [0,2PI] __attribute__((weak)) float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index c5ff1852..cf3df719 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -71,6 +71,13 @@ float _sin(float a); * @param a angle in between 0 and 2PI */ float _cos(float a); +/** + * Function returning both sine and cosine of the angle in one call. + * Internally it uses the _sin and _cos functions, but you may wish to + * provide your own implementation which is more optimized. + */ +void _sincos(float a, float* s, float* c); + /** * normalizing radian angle to [0,2PI]