File content as of revision 2:edd107ea4740:

#include "ACS.h"
#include "MPU3300.h"
#include "pin_config.h"

//PwmOut PWM1(PTD4);        //Functions used to generate PWM signal 
                        //PWM output comes from pins p6
Serial pc1(USBTX, USBRX);
SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk  PTE18,19,17
DigitalOut SSN_MAG (PTC16); // ssn for magnetometer PTB11
DigitalInOut DRDY (PTE3); // drdy for magnetometer PTA17
DigitalOut ssn_gyr (PTE2);         //Slave Select pin of gyroscope  PTB16
InterruptIn dr(PTC6);       //Interrupt pin for gyro PTC5
PwmOut PWM1(D2);        //Functions used to generate PWM signal 
PwmOut PWM2(D3); 
PwmOut PWM3(D4);                   //PWM output comes from pins p6
Ticker tr;              //Ticker function to give values for limited amount of time for gyro
Timeout tr_mag;
uint8_t trflag_mag;
uint8_t trFlag;         //ticker Flag for gyro
uint8_t drFlag;         //data-ready interrupt flag for gyro

//--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//

void FUNC_ACS_GENPWM(float M[3])
 {
     

     printf("\nEnterd PWMGEN function\n");
     float DCx = 0;         //Duty cycle of Moment in x, y, z directions
     float ix = 0;          //Current sent in x, y, z TR's
     float timep = 0.02 ;  
     float Mx=M[0];            //Time period is set to 0.02s  
                             //Moment in x, y, z directions
     
      
     
        ix = Mx * 0.3 ;      //Moment and Current always have the linear relationship
     
        if( ix>0&& ix < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
         {
             DCx =  6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
             PWM1.period(timep);
             PWM1 = DCx/100 ;
         }
        else if( ix >= 0.006&& ix < 0.0116)
         { 
            DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
            PWM1.period(timep);
            PWM1 = DCx/100 ;             
         }
        else if (ix >= 0.0116&& ix < 0.0624)
         {
              
            DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
            PWM1.period(timep);
            PWM1 = DCx/100 ;            
         }
        else if(ix >= 0.0624&& ix < 0.555)
         {
            printf("\nACS entered if\n");
            DCx =  331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
            PWM1.period(timep);
            PWM1 = DCx/100 ;            
         }
         else if(ix==0)
         {
             DCx = 50;
            PWM1.period(timep);
            PWM1 = DCx/100 ;            
         }
         else
         {
            // printf("!!!!!!!!!!Error!!!!!!!!!");
         } 
         
    printf("\n moment :%f\n",DCx);
    float DCy = 0;         //Duty cycle of Moment in x, y, z directions
     float iy = 0;          //Current sent in x, y, z TR's
       
    float My=M[1];            //Time period is set to 0.2s  
                             //Moment in x, y, z directions
      
     
        iy = My * 0.3 ;      //Moment and Current always have the linear relationship
     
        if( iy>0&& iy < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
         {
             DCy =  6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
             PWM2.period(timep);
             PWM2 = DCy/100 ;
         }
        else if( iy >= 0.006&& iy < 0.0116)
         { 
            DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
            PWM2.period(timep);
            PWM2 = DCy/100 ;             
         }
        else if (iy >= 0.0116&& iy < 0.0624)
         {
              
            DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
            PWM2.period(timep);
            PWM2 = DCy/100 ;            
         }
        else if(iy >= 0.0624&& iy < 0.555)
         {
            printf("\nACS entered if\n");
            DCy =  331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
            PWM2.period(timep);
            PWM2 = DCy/100 ;            
         }
         else if(iy==0)
         {
             DCy = 0;
            PWM2.period(timep);
            PWM2 = DCy/100 ;            
         }
         else
         {
            // printf("!!!!!!!!!!Error!!!!!!!!!");
         } 
    float DCz = 0;         //Duty cycle of Moment in x, y, z directions
     float iz = 0;          //Current sent in x, y, z TR's
       
     float Mz=M[2];            //Time period is set to 0.2s  
                             //Moment in x, y, z directions
      
     
        iz = Mz * 0.3 ;      //Moment and Current always have the linear relationship
     
        if( iz>0&& iz < 0.006 )                     //Current and Duty cycle have the linear relationship between 1% and 100%
         {
             DCz =  6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
             PWM3.period(timep);
             PWM3 = DCz/100 ;
         }
        else if( iz >= 0.006&& iz < 0.0116)
         { 
            DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
            PWM3.period(timep);
            PWM3 = DCz/100 ;             
         }
        else if (iz >= 0.0116&& iz < 0.0624)
         {
              
            DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
            PWM3.period(timep);
            PWM3 = DCz/100 ;            
         }
        else if(iz >= 0.0624&& iz < 0.555)
         {
            printf("\nACS entered if\n");
            DCz =  331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
            PWM3.period(timep);
            PWM3 = DCz/100 ;            
         }
         else if(iz==0)
         {
             DCz = 0;
            PWM3.period(timep);
            PWM3 = DCz/100 ;            
         }
         else
         {
            // printf("!!!!!!!!!!Error!!!!!!!!!");
         }    
    
printf("\nExited PWMGEN function\n");
}
/*-------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/

void trsub_mag()
{
  trflag_mag=0;
} 

void FUNC_ACS_MAG_INIT()
 {
  //DRDY.output();
  DRDY = 0;
  int a ;
  a=DRDY;
  printf("\n DRDY is %d\n",a);
  SSN_MAG=1;                                    //pin is disabled
  spi_acs.format(8,0);                         //   8bits,Mode 0
  spi_acs.frequency(100000);                   //clock frequency
  
  SSN_MAG=0;                                   // Selecting pin
  wait_ms(10);                            //accounts for delay.can be minimised.
  
  spi_acs.write(0x83);                      //
  
  wait_ms(10);              
  
  unsigned char i;
  for(i=0;i<3;i++)//initialising values.
      {
         spi_acs.write(0x00);              //MSB of X,y,Z
         spi_acs.write(0xc8);             //LSB of X,Y,z;pointer increases automatically.
        }
   SSN_MAG=1;
 
}

float* FUNC_ACS_MAG_EXEC()
{
   //printf("\nEntered magnetometer function\n");
   //DRDY.output();
   DRDY.write(0);
   int a;
   a = DRDY;
   printf("\n DRDY is %d\n",a);
   SSN_MAG=0;                                //enabling slave to measure the values
   wait_ms(10);
   spi_acs.write(0x82);                     //initiates measurement
   wait_ms(10);
   spi_acs.write(0x01);                   //selecting x,y and z axes, measurement starts now
   SSN_MAG=1;
   wait_ms(10);

   trflag_mag=1;        
   tr_mag.attach(&trsub_mag,1);   //runs in background,makes trflag_mag=0 after 1s
   DRDY.input();
   while(trflag_mag)              /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else 
                                 loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and 
                               programme is terminated*/
  {
    wait_ms(5);
    if(DRDY==1)
    {
        printf("\nwth\n");
        SSN_MAG=0;
        spi_acs.write(0xc9);                  //command  byte for retrieving data
 
        unsigned char axis;
        float Bnewvalue[3]={0.0,0.0,0.0};
        int32_t Bvalue[3]={0,0,0}; 
        int32_t a= pow(2.0,24.0);
        int32_t b= pow(2.0,23.0);
 
        for(axis=0;axis<3;axis++)
        {
            Bvalue[axis]=spi_acs.write(0x00)<<16;    //MSB 1 is send first 
            wait_ms(10);
            Bvalue[axis]|=spi_acs.write(0x00)<<8;    //MSB 2 is send next
            wait_ms(10);
            Bvalue[axis]|=spi_acs.write(0x00);       //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
  
   
            if((Bvalue[axis]&b)==b)              
            {
                Bvalue[axis]=Bvalue[axis]-a;   //converting 2s complement to  signed decimal

            }
            Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0);  //1 LSB=(22nT)...final value of field obtained in micro tesla
  
            wait_ms(10);
            printf("\t%lf\n",Bnewvalue[axis]);

        }
        SSN_MAG=1;
        /* for test only to removed */
        Bnewvalue[0]=Bnewvalue[1]=Bnewvalue[2]=100;
        return Bnewvalue;          //return here? doubt..
        break;
    }
    
 }
 
} 
/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/

float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3])
{
    float db[3]; /// inputs
//initialization
    float bb[3] = {0, 0, 0};
    float d[3] = {0, 0, 0};
    float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
    float den = 0; 
    float den2;
    int i, j; //temporary variables
    float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs
    float invJm[3][3];
    float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
    printf("Entered cntrl algo\n");
    //structure parameters

    void inverse (float mat[3][3], float inv[3][3]); 
    void getInput (float x[9]);
    //functions
 
////////// Input from Matlab //////////////
    while(1) 
    {
       
 /*getInput(inputs);
//while(1)
 b[0] = inputs[0];
 b[1] = inputs[1];
 b[2] = inputs[2];
 db[0] = inputs[3];
 db[1] = inputs[4];
 db[2] = inputs[5]; 
 omega[0] = inputs[6];
 omega[1] = inputs[7];
 omega[2] = inputs[8];*/
/////////// Control Algorithm //////////////////////
// calculate norm b, norm db
        den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
        den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
       
        for(i=0;i<3;i++)
        {
            db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
//db[i]/=den*den*den;
        }
        
        for(i=0;i<3;i++)
        {
            printf("\nreached here\n");
            if(den!=0)
                //b[i]=b[i]/den;                                      //there is a problem here. The code gets stuck here.  Maf value is required 
                ;
        }
        
// select kz, kmu, gamma
        if(b[0]>0.9 || b[0]<-0.9)
        {
            kz = kz2;
            kmu = kmu2;
            gamma = gamma2;
        }
// calculate Mu, v, dv, z, u
        for(i=0;i<2;i++)
        {
            Mu[i] = b[i+1];
            v[i] = -kmu*Mu[i];
            dv[i] = -kmu*db[i+1];
            z[i] = db[i+1] - v[i];
            u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
        }
        inverse(Jm, invJm);
// calculate cross(omega,J*omega)for(i=0;i<3;i++)
         
        for(j=0;j<3;j++)
            bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);

// calculate invJm*cross(omega,J*omega) store in d
        for(i=0;i<3;i++)
        {
            for(j=0;j<3;j++)
                d[i] += bb[j]*invJm[i][j];
        }
// calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db) 
// bb =[0;u-d(2:3)] 
// store in bb
        bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
        bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
        bb[0] = 0;
// calculate N 
// reusing invJm as N
       
        for(i=0;i<3;i++)
        {
            d[i] = invJm[1][i];
            invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
            invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
            invJm[0][i] = b[i];
        }
// calculate inv(N) store in Jm
        inverse(invJm, Jm);
// calculate tauc
        for(i=0;i<3;i++)
        {
            for(j=0;j<3;j++)
                tauc[i] += Jm[i][j]*bb[j];
        }
         
        return(tauc);
    }
}
/////////// Output to Matlab //////////////////
/* for(i=0;i<3;i++) {
 printf("%f\n",tauc[i]*10000000);
 wait_ms(10);
 }
 }
 
}*/
 void inverse(float mat[3][3], float inv[3][3])
{
int i, j;
float det = 0;
for(i=0;i<3;i++)
{ for(j=0;j<3;j++)
inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
[(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
}
det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
for(i=0;i<3;i++)
{ for(j=0;j<3;j++)
inv[i][j] /= det;
}
}/*
void getInput (float x[9]) {
         //Functions used to generate PWM signal 
                        //PWM output comes from pins p6
Serial pc1(USBTX, USBRX);
 char c[10];
 char tempchar[8];
 int i, j;
 //float f[9];
 long n = 0;
 float flval = 0;
 for(j=0;j<9;j++) {
 for(i=0;i<9;i++) {
 c[i] = pc1.getc(); if(i<8) {
 tempchar[i] = c[i];
 }
 }
 sscanf (tempchar, "%8x", &n);
 memcpy(&flval, &n, sizeof(long));
 printf("%f\n", flval);
 x[j] = flval;
 }
}*/

void trSub();               
void drSub(); 
void init_gyro();       
float * FUNC_ACS_EXEC_GYR();

void drSub()            //In this function we setting data-ready flag to 1              
{
    drFlag=1;
}
void trSub()                    //In this function we are setting ticker flag to 0
{
    trFlag=0;
}
void FUNC_ACS_INIT_GYR()
{
    uint8_t response;               
    ssn_gyr=1;                  //Deselecting the chip 
    spi_acs.format(8,0);                // Spi format is 8 bits, and clock mode 3 
    spi_acs.frequency(1000000);     //frequency to be set as 1MHz
    drFlag=0;                   //Intially defining data-ready flag to be 0 
    dr.mode(PullDown);          
    dr.rise(&drSub);
    __disable_irq();
    
/*Following the above mentioned algorithm for initializing the register and changing its configuration*/
    ssn_gyr=0;                      //Selecting chip(Mpu-3300)
    spi_acs.write(USER_CTRL|READFLAG);   //sending USER_CTRL address with read bit
    response=spi_acs.write(DUMMYBIT);   //sending dummy bit to get default values of the register
                        
    ssn_gyr=1;                  //Deselecting the chip  
    wait(0.1);                  //waiting according the product specifications 
    
    ssn_gyr=0;                  //again selecting the chip  
    spi_acs.write(USER_CTRL);           //sending USER_CTRL address without read bit 
    spi_acs.write(response|BIT_I2C_IF_DIS);  //disabling the I2C mode in the register
    ssn_gyr=1;                  //deselecting the chip 
    wait(0.1);                  // waiting for 100ms before going for another register 
    
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1 
    response=spi_acs.write(DUMMYBIT);
    ssn_gyr=1;
    wait(0.1);
        
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1);
    response=spi_acs.write(response|BIT_CLKSEL_X);  //Selecting the X axis gyroscope as clock as mentioned above 
    ssn_gyr=1;                          
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
    response=spi_acs.write(DUMMYBIT);
    ssn_gyr=1;
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
    spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
    ssn_gyr=1;
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
    response=spi_acs.write(DUMMYBIT);
    ssn_gyr=1;
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(CONFIG); //sending CONFIG address to write to register
    spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
    ssn_gyr=1;
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
    response=spi_acs.write(DUMMYBIT);
    ssn_gyr=1;
    wait(0.1);
        
    ssn_gyr=0;
    spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
    spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
    ssn_gyr=1;
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(INT_ENABLE|READFLAG);       //sending address of INT_ENABLE with readflag
    response=spi_acs.write(DUMMYBIT);              //sending dummy byte to get the default values of the
                                                                          // regiser
    ssn_gyr=1;   
    wait(0.1);
    
    ssn_gyr=0;
    spi_acs.write(INT_ENABLE);                           //sending INT_ENABLE address to write to register
    spi_acs.write(response|BIT_DATA_RDY_ENABLE);  //Enbling data ready interrupt
    ssn_gyr=1;
    wait(0.1);
    
    __enable_irq();
}

float * FUNC_ACS_EXEC_GYR()
{
    printf("\nEntered gyro\n");
    uint8_t response;
    uint8_t MSB,LSB;
    int16_t bit_data;
    float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
    float senstivity = 145.6;     //senstivity is 145.6 for full scale mode of +/-225 deg/sec
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
    response=spi_acs.write(DUMMYBIT); //
    ssn_gyr=1;
    wait(0.1);
        
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
    response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
    ssn_gyr=1;
    wait(0.1);
    
    trFlag=1;
    tr.attach(&trSub,1); //executes the function trSub afer 1sec
    
    while(trFlag)
    {
        wait_ms(5);   //This is required for this while loop to exit. I don't know why.
        if(drFlag==1)
        {
            ssn_gyr=0;
            spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
            for(int i=0;i<3;i++)
            {
                MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
                LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
                bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
                data[i]=(float)bit_data;
                data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
                data[i]+=error[i]; //adding with error to remove offset errors
            }
            ssn_gyr=1;      
            for (int i=0;i<3;i++)
            {
                printf("%f\t",data[i]); //printing the angular velocity values
            }
            printf("\n");
            break;
        }
            drFlag=0;
    }
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
    response=spi_acs.write(DUMMYBIT);
    ssn_gyr=1;
    wait(0.1);
        
    ssn_gyr=0;
    spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
    response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
    ssn_gyr=1;
    wait(0.1);
    printf("\nExited gyro\n");
    return data;
}