File content as of revision 1:bbddd1763652:

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

Serial pc1(USBTX, USBRX);
SPI spi_acs (PIN16, PIN17, PIN15);        // mosi, miso, sclk  PTE18,19,17
DigitalOut SSN_MAG (PIN61);               // ssn for magnetometer PTB11
DigitalInOut DRDY (PIN47);                 // drdy for magnetometer PTA17
DigitalOut ssn_gyr (PTE2);                //Slave Select pin of gyroscope  PTB16
InterruptIn dr(PTC6);                     //Interrupt pin for gyro PTC5
PwmOut PWM1(PIN93);                          //Functions used to generate PWM signal 
PwmOut PWM2(PIN94); 
PwmOut PWM3(PIN95);                          //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
float pwm1;
float pwm2;
float pwm3;
//--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//

void FUNC_ACS_GENPWM(float M[3])
{
     
     printf("\n\rEnterd PWMGEN function\n");
     for(int i = 0 ; i<3;i++)
     {
        printf(" %f \t ",M[i]);
     }
     float timep = 0.02 ;   //Time period is set to 0.02s  
     
     
     float DCx = 0;         //Duty cycle of Moment in x direction
     float ix = 0;          //Current sent in x TR's
     float Mx=M[0];         //Moment in x direction
     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("\n\rACS entered if\n\r");                                //gotta check what this means
        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)
     {
        printf("\n \r ix====0");
        DCx = 75;
        PWM1.period(timep);
        PWM1 = DCx/100 ;            
     }
     else                                           //not necessary
     {
        // printf("!!!!!!!!!!Error!!!!!!!!!");
     } 
     pwm1 = PWM1;
     printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1);
     
     
     float DCy = 0;         //Duty cycle of Moment in y direction
     float iy = 0;          //Current sent in y TR's
     float My=M[1];         //Moment in y direction
     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("\n\rACS entered if\n\r");
        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!!!!!!!!!");
     } 
     pwm2 = PWM2; 
     printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2);    
         
         
     float DCz = 0;         //Duty cycle of Moment in z direction
     float iz = 0;          //Current sent in z TR's
     float Mz=M[2];         //Moment in z direction
     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("\n\rACS entered if\n\r");
        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!!!!!!!!!");
     }   
     pwm3 = PWM3; 
     printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); 
    
     printf("\n\rExited PWMGEN function\n\r");
}
/*-------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/

void trsub_mag()
{
  trflag_mag=0;
} 

void FUNC_ACS_MAG_INIT()
{
  DRDY = 0;
  int a ;
  a=DRDY;
  printf("\n\r DRDY is %d\n\r",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;
}

void FUNC_ACS_MAG_EXEC(float mag_field[])
{
   printf("\n\rEntered magnetometer function\n\r");
   DRDY.write(0);
   int a;
   a = DRDY;
   printf("\n\r DRDY is %d\n\r",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("\n\r DRDY is high\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);
            
            mag_field[axis] = Bnewvalue[axis];
            printf("\t%lf\n\r",mag_field[axis]);
        }
        SSN_MAG=1;
        printf("\n\r exited magnetometer function\n");
        //return Bnewvalue;          //return here? doubt..
        
        break;
    }
   }
} 
/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------torque to moment conversion------------------------------------------------------------------------------------------*/
void moment_calc (float tauc[3], float b[3], float moment[3])
{
  float b1;
  b1 = pow(b[0],2) + pow(b[1],2) +pow(b[2],2) ;
  printf("\nvalue of b is %f\n",b1);
  moment[0] = ((tauc[1]*b[2])-(tauc[2]*b[1]))/b1;
  moment[1] = ((tauc[2]*b[0])-(tauc[0]*b[2]))/b1;
  moment[2] = ((tauc[0]*b[1])-(tauc[1]*b[0]))/b1;
  
}


/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/

void FUNC_ACS_CNTRLALGO(float *b,float *omega,float tauc[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]; //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("\n\r Entered cntrl algo\n\r");
    for(int i=0; i<3; i++) 
        {
        printf("%f\t",omega[i]);
        }
    for(int i=0; i<3; i++) 
        {
        printf("%f\t",b[i]);
        }
    //structure parameters
    void inverse (float mat[3][3], float inv[3][3]); 
    void getInput (float x[9]);
    tauc[0] =tauc[1] =tauc[2]=0 ;
    
    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++)                                        //gotta check what is this
    {
        printf("\n\rreached here\n\r");
        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(i=0; i<3; i++)                          // for loop included after checking original code
    { 
        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
    printf("\n \r calculatin tauc");
    for(i=0;i<3;i++)
    {
        for(j=0;j<3;j++)
            tauc[i] += Jm[i][j]*bb[j];
        printf(" %f \t",tauc[i]);    
    }
}

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


/*-------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------GYROSCOPE-------------------------------------------------------------------------------------------------*/

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;
    //printf("\n dr interrupt detected");
}
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();
    
    /*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();
}

void FUNC_ACS_EXEC_GYR(float*omega)
{
    printf("\n\rEntered gyro\n\r");
    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)
    {
       // printf("\n\r check1");
        wait_ms(5);   //This is required for this while loop to exit. I don't know why.
        if(drFlag==1)
        {
            //printf("\n\r check2");
            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
                omega[i] = data[i];
            }
            printf("\n\r");
            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("\n\rExited gyro\n\r");
    
}