Faster cosine look up table

Hello.
I need to use cosine and sine functions to calculate 3 duty cycles for some PWM signals. I need about 12x cos/sin calculations for each 3x duty cycles, and I need to calculate these as often and quick as possible. I need to do the calculations with atleast a frequency of 25kHz, and still have a lot of cpu time free for other calculations and interrupts.

First I tried with the standard cos and sin functions. These take around 300µS, so not near enough the speed I need. I then tried to make my own look up table in the hope that this should be faster. It was faster, around 140µS, but still to slow for my application. Anyone have an idea on how to make it faster? Any special variables, or tricks I can use?

Here is my code:

volatile int a=0;
volatile int b=0;
volatile int c=0;
volatile int da=0;
volatile int db=0;
volatile int dc=0;

volatile int iq=0;
volatile int id=0;
volatile int ia,ib,ic;
volatile int pos=0;
int idref=0;
volatile int iqref = 0;
int kpdq=0;
int kidq=0;
int kpspeed=0;
int kispeed=0;
int Ts=1;
volatile int vd,vq;
volatile int prevvd,prevvq,preverrd,preverrq,va,vb,vc,ysat,prevysat,ysatd,ysatq,prevysatd,prevysatq=0;
int kaw=1;
void setup() {
  Serial.begin(9600);
  int t=analogRead(0);

  ADC->ADC_WPMR=0x00; //disable write protection on MR
  ADC->ADC_MR |= 0x200080; //set ADC in free running mode and settling time is 9 times ADC clock
  ADC->ADC_CR=2; //enable ADC clock
  ADC->ADC_CHER=0xFF; //enable A0->A7+A8
}

void loop() {
  int t=micros();
  pos=0;
  
  for(int i=0;i<100;i++){
    pos++;
  while((ADC->ADC_ISR & 0xFF)!=0xFF);
  a=ADC->ADC_CDR[4]; //read data on A3 pin
  b=ADC->ADC_CDR[3]; //read data on A4 pin
  c=ADC->ADC_CDR[2]; //read data on A5 pin
  
  //scale measured voltage into actual current    
  ia=25*a-24;
  ib=25*b-24;
  ic=25*c-24;
  //Park transformation, from adc to dq0 ref system
  id=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);
  iq=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);

  //PI controller with anti windup
  vd=kpdq*((idref-id)-preverrd)+kidq*(Ts/2)*((idref-id)+preverrd)+prevvd;
  if(vd>28){
    ysatd=28;
  }
  else if(vd<-28){
    ysatd=-28;
  }
  else{
    ysatd=vd;
  }
  vd=kpdq*((idref-id)-preverrd)+kidq*(Ts/2)*((idref-id)+preverrd)+prevvd+((ysatd-vd)+prevysatd)*kaw;
  prevysatd=(ysatd-vd)*kaw;
  prevvd=vd;
  preverrd=idref-id;
  
  
  vq=kpdq*((iqref-iq)-preverrq)+kidq*(Ts/2)*((iqref-iq)+preverrq)+prevvq;
  if(vq>28){
    ysatq=28;
  }
  else if(vq<-28){
    ysatq=-28;
  }
  else{
    ysatq=vq;
  }
  vq=kpdq*((iqref-iq)-preverrq)+kidq*(Ts/2)*((iqref-iq)+preverrq)+prevvq+((ysatq-vq)+prevysatq)*kaw;
  prevysatq=(ysatq-vq)*kaw;
  prevvq=vq;
  preverrq=iqref-iq;
 
  
  //dq0 to abc (inverse park transformation)  
  
  va=cosinus(pos)*vd-cosinus(pos)*vq;
  vb=cosinus(pos-120)*vd-cosinus(pos-120)*vq;
  vc=cosinus(pos+120)*vd-cosinus(pos+120)*vq;

  //convert voltage references to duty cycles
  da=((va*1)+1)*127;
  db=((vb*1)+1)*127;
  dc=((vc*1)+1)*127;
  }
  
  t=micros()-t;
  Serial.print("100 conversions in ");Serial.print(t);Serial.println(" micros");

}

//lookup table for cos
int cosinus(int posit){
  int i=0;
  int table[]={10000,9998,9994,9986,9976,9962,9945,9925,9903,9877,9848,9816,9781,9744,9703,9659,9613,9563,9511,9455,9397,9336,9272,9205,9135,9063,8988,8910,8829,8746,8660,8572,8480,8387,8290,8192,8090,7986,7880,7771,7660,7547,7431,7314,7194,7071,6947,6820,6691,6561,6428,6293,6157,6018,5878,5736,5592,5446,5299,5150,5000,4848,4695,4540,4384,4226,4067,3907,3746,3584,3420,3256,3090,2924,2756,2588,2419,2250,2079,1908,1736,1564,1392,1219,1045,872,698,523,349,175,0};
    if(posit>360)
  {
    posit=posit-360;
  }
  if(posit<-360)
  {
    posit=posit+360;
  }
  if(posit<=90&posit>=0)
    {
      i=posit;
      return table[i];
      
    }
  if(posit>90&posit<=180)
  {
    i=(((posit-90)/90)-1)*(-90);
    return table[i]*(-1);
  }
  if(posit>180&posit<=270)
  {
    i=(posit-180);
    return table[i]*(-1);
  }
  if(posit>270&posit<=360)
  {
    posit=posit-180;
    i=(((posit-90)/90)-1)*(-1);
    return table[i];
  }
  else
  {
    return -10;
  }  
}

Is it code for DUE? Why do you need 4 quadrant mapping, if you have plenty of memory to store 360 integers? Other things, this two lines:

id=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);
  iq=1*(cosinus(pos)*ia+cosinus(pos-120)*ib+cosinus(pos+120)*ic);

I noticed, it twice, it would make sense to store "pos-120" and "pos+120" as intermediate result in a variable, not to calculate it 4 times. Same with cosine(pos -/+ 120) - don't have to do look-up twice, store it in variable. Even both value located in RAM, storing "resolved" value would save time on indexing array.

Yes, its code for a DUE.

I need 4 quadrant mapping because the position (measured position) will vary between 0 and 360 degrees. I need to take both sine and cosine of all the angles. The values stored in the look up table is for the angles between 0 and 90 (degrees). So I use these values to give the values for the 3 other quadrants. As it stands now it only works for cosine, but its not a problem to make it work for sine also.

The two lines you are quoting are just there to generate the correct calculation time. The correct should be:

id=cos(pos)*ia+cos(pos-120)*ib+cos(pos+120)*ic;
iq=sin(pos)*ia+sin(pos-120)*ib+sin(pos+120)*ic;

Im just using my look up cosine to see the time the calculation will take.
It seems like the bigger the table is the more time it takes to get the correct value. Im not sure if this is the biggest "time sink", but if it is, it might be worth it to reduce the number of values and make some average in between?

I will add the pos+-120 as a variable to reduce calculations, thanks for the tip!

This statement:

  if(posit<=90&posit>=0)

and others like it should be using &&, not &. These statements also need spaces to make them more readable and, just to be safe, it also doesn't hurt to parenthesize each comparison.

  if((posit <= 90) && (posit >= 0))

Pete