Headfree Implementation Improvement

[QuadBow]

Hi,

In the file Multiwii.cpp you find the following code related to HEADFREE:

Code: Select all

    #if defined(HEADFREE)
      if(f.HEADFREE_MODE) { //to optimize
        float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
        float cosDiff = cos(radDiff);
        float sinDiff = sin(radDiff);
        int16_t rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
        rcCommand[ROLL] =  rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff; 
        rcCommand[PITCH] = rcCommand_PITCH;
      }
    #endif

Well, there is a note //to optimize which I do confirm, there is a need to optimise.
We do have the difference between 2 values (att.heading and headFreeModeHold) with a resolution of one degree.
And we need 2 trigonometric functions (sin, cos) with 1600 cycles each which is equal to an additional time of 2*1600/16e6 = 200us in total plus some mixed integer/float multiplications.

That leads to following impact shown in the attached image:
When running without HEADFREE the magical 650us time is met very well. However, once HEADFREE is switched on this time rises from 650us to 850us or even more. (Debug1 shows a related value of 900us.)

So, my question is:
Has already someone done some research in order to get rid of the trigonometric functions with float precision?
I could imagine that a trigonometric table would give the same result with better performance.

[QuadBow]

So, my question is:
Has already someone done some research in order to get rid of the trigonometric functions with float precision?
I could imagine that a trigonometric table would give the same result with better performance.

Since nobody has answered my question I did some modifications by my own.

Firstly, I inserted a look-up table of 91 entries at the begin of the file Multiwii.cpp.

Code: Select all

#if defined(OPTIMISE_SINCOS)
  float SINUS_TABLE[91] = {0.000000,0.017452,0.034899,0.052336,0.069756,0.087156,0.104528,0.121869,0.139173,0.156434,
                           0.173648,0.190809,0.207912,0.224951,0.241922,0.258819,0.275637,0.292372,0.309017,0.325568,
                           0.342020,0.358368,0.374607,0.390731,0.406737,0.422618,0.438371,0.453990,0.469472,0.484810,
                           0.500000,0.515038,0.529919,0.544639,0.559193,0.573576,0.587785,0.601815,0.615662,0.629320,
                           0.642788,0.656059,0.669131,0.681998,0.694658,0.707107,0.719340,0.731354,0.743145,0.754710,
                           0.766044,0.777146,0.788011,0.798635,0.809017,0.819152,0.829038,0.838671,0.848048,0.857167,
                           0.866025,0.874620,0.882948,0.891007,0.898794,0.906308,0.913545,0.920505,0.927184,0.933580,
                           0.939693,0.945519,0.951057,0.956305,0.961262,0.965926,0.970296,0.974370,0.978148,0.981627,
                           0.984808,0.987688,0.990268,0.992546,0.994522,0.996195,0.997564,0.998630,0.999291,0.999848,1.000000};
#endif

Then I replaced the current headfree code within the function annexCode() of the file Multiwii.cpp.

Code: Select all

  #if defined(HEADFREE)
    if(f.HEADFREE_MODE) { //to optimize
      float cosDiff, sinDiff;
      int16_t rcCommand_PITCH;
      #if defined(OPTIMISE_SINCOS)
          int16_t degDiff = att.heading - headFreeModeHold;
          if (degDiff <= - 180) degDiff += 360;
          if (degDiff >= + 180) degDiff -= 360;
          if (degDiff > 0){
             if (degDiff <= 90){ // 0°..90°
                sinDiff =  SINUS_TABLE[degDiff];
                cosDiff =  SINUS_TABLE[90 - degDiff];
                }
             else{               // 90°..180°
                sinDiff =  SINUS_TABLE[180 - degDiff];
                cosDiff = -SINUS_TABLE[-90 + degDiff];
                }
             }
          else {
             if (degDiff >= -90){ // 0°..-90°
                sinDiff = -SINUS_TABLE[-degDiff];
                cosDiff =  SINUS_TABLE[90 + degDiff];
                }
             else{               // -90°..-180°
                sinDiff = -SINUS_TABLE[180 + degDiff];
                cosDiff = -SINUS_TABLE[-90 - degDiff];
                }
             }
      #else // not defined OPTIMISE_SINCOS => original code with long lasting trigonometric functions from math library
          float radDiff = (att.heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
          cosDiff = cos(radDiff);
          sinDiff = sin(radDiff);
      #endif
      rcCommand_PITCH = rcCommand[PITCH]*cosDiff + rcCommand[ROLL]*sinDiff;
      rcCommand[ROLL]  = rcCommand[ROLL]*cosDiff - rcCommand[PITCH]*sinDiff;
      rcCommand[PITCH] = rcCommand_PITCH;
    }
  #endif

Now, I can run the optimised code saving the two long lasting trigonometric functions via the following definition in the file config.h.

Code: Select all

#define OPTIMISE_SINCOS

Recall that the resolution of heading is just one degree. Therefore, this small look-up table is sufficient and the results are the same as for the former non-optimised code.

As the attached image shows, the time required for the function annexCode() has been lowered from 850us to 750us as shown in the field debug3.

Time required with optimisation for HEADFREE

But, it is still more than the magical 650us, we are aiming for. Further optimisation did not succeed up to now. So,the next step was to optimise the similar trigonometric functions within the function loop().

[QuadBow]

The next step was to optimise the similiar trigonometric functions for GPS, too. For that I changed the code within the function loop() as follows.

Code: Select all

  #if GPS
  //TODO: split cos_yaw calculations into two phases (X and Y)
  if (( f.GPS_mode != GPS_MODE_NONE ) && f.GPS_FIX_HOME ) {
      float sin_yaw_y, cos_yaw_x;
      #if defined(OPTIMISE_SINCOS)
          int16_t degDiff = att.heading;
          if (degDiff <= - 180) degDiff += 360;
          if (degDiff >= + 180) degDiff -= 360;
          if (degDiff > 0){
             if (degDiff <= 90){ // 0°..90°
                sin_yaw_y =  SINUS_TABLE[degDiff];
                cos_yaw_x =  SINUS_TABLE[90 - degDiff];
                }
             else{               // 90°..180°
                sin_yaw_y =  SINUS_TABLE[180 - degDiff];
                cos_yaw_x = -SINUS_TABLE[-90 + degDiff];
                }
             }
          else {
             if (degDiff >= -90){ // 0°..-90°
                sin_yaw_y = -SINUS_TABLE[-degDiff];
                cos_yaw_x =  SINUS_TABLE[90 + degDiff];
                }
             else{               // -90°..-180°
                sin_yaw_y = -SINUS_TABLE[180 + degDiff];
                cos_yaw_x = -SINUS_TABLE[-90 - degDiff];
                }
             }
    #else // not defined OPTIMISE_SINCOS => original code with long lasting trigonometric functions from math library
       sin_yaw_y = sin(att.heading*0.0174532925f);
       cos_yaw_x = cos(att.heading*0.0174532925f);
    #endif
    GPS_angle[ROLL]   = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) / 10;
    GPS_angle[PITCH]  = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) / 10;
    } else {
      GPS_angle[ROLL]  = 0;
      GPS_angle[PITCH] = 0;
    }

That implementation hits the magical time of 650us quite well as shown in the field debug1.

Time of function annexCode() with optimisation for HEADFREE and GPS by a trigonometric look-up table

If you feel invited to test the optimised code do it by your own risk. (I still have not tested it, since I can not fly at the moment. I am awaiting a new ESC, which was defect.)

[brm]

nice approach!
thumbs up!