change sine implementation to deku65i version

Author: Richard Unger

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;

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;

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;

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);

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]