Diff: ACS.cpp

Revision:

0:7b4c00e3912f

Child:

3:07e15677a75c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ACS.cpp	Thu Dec 24 19:15:43 2015 +0000
@@ -0,0 +1,503 @@
+/*------------------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
+#include <mbed.h>
+#include <math.h>
+
+#include "pni.h" //pni header file
+#include "pin_config.h"
+#include "ACS.h"
+
+
+//********************************flags******************************************//
+extern uint32_t BAE_STATUS;
+extern uint32_t BAE_ENABLE;
+extern char ACS_INIT_STATUS;
+extern char ACS_DATA_ACQ_STATUS;
+extern char ACS_ATS_STATUS;
+extern char ACS_MAIN_STATUS;
+extern char ACS_STATUS;
+
+extern char ACS_ATS_ENABLE;
+extern char ACS_DATA_ACQ_ENABLE;
+extern char ACS_STATE;
+
+DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
+DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
+DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
+
+extern PwmOut PWM1; //x                         //Functions used to generate PWM signal 
+extern PwmOut PWM2; //y
+extern PwmOut PWM3; //z                         //PWM output comes from pins p6
+
+int g_err_flag_TR_x=0;       // setting x-flag to zero
+int g_err_flag_TR_y=0;       // setting y-flag to zero
+int g_err_flag_TR_z=0;       // setting z-flag to zero
+
+extern float data[6];
+
+
+//DigitalOut gpo1(PTC0); // enable of att sens2 switch
+//DigitalOut gpo2(PTC16); // enable of att sens switch
+
+
+Serial pc_acs(USBTX,USBRX); //for usb communication
+void inverse(float mat[3][3],float inv[3][3]);
+  
+int ctrl_count = 0;
+float bcopy[3];
+float moment[3];
+ ///////algo working well
+void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
+{
+    float db[3];
+    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,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;
+    
+    //................. calculating db values...........................
+    if(ctrl_count!=0)
+    {
+        for(i=0;i<3;i++)
+        db[i]= (b[i]-bcopy[i])/10;
+    }
+    else
+    {
+        for(i=0;i<3;i++)
+        db[i]= 0;
+    }
+    ctrl_count++;
+    //..................................................................
+    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]);
+        }
+
+    //.........................algo......................................
+    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++)
+    {
+        b[i]/=den;
+    }
+    // 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);
+    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]);
+    }
+    for(i=0;i<3;i++)
+    {
+        for(j=0;j<3;j++)
+            d[i]+=bb[j]*invJm[i][j];
+    }
+    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;
+    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];
+    }
+    inverse(invJm,Jm);
+    printf("\n \r calculating tauc");
+    for(i=0;i<3;i++)
+    {
+        for(j=0;j<3;j++)
+            tauc[i]+=Jm[i][j]*bb[j];            // calculating torque values
+            printf(" %f \t",tauc[i]);    
+    }
+    //..........................tauc to moment conversion..........................
+    printf("\n \r calculating moment");
+    for(i=0;i<3;i++)
+        bcopy[i]=b[i]*den;
+    for(i=0;i<3;i++)
+    {
+        moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
+        moment[i]/=den;
+        printf(" %f \t",moment[i]); 
+    }
+    printf("\n\r exited control algo\n");
+}
+//..........................function to find inverse..................
+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;
+    }
+}
+
+
+I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl  for the attitude sensors and battery gauge
+
+void FCTN_ACS_INIT(void); //initialization of registers happens
+void FCTN_ATS_DATA_ACQ(); //data is obtained
+void T_OUT(); //timeout function to stop infinite loop
+Timeout to; //Timeout variable to
+int toFlag; 
+
+int count =0; // Time for which the BAE uC is running (in seconds)
+void T_OUT()
+{
+    toFlag=0; //as T_OUT function gets called the while loop gets terminated
+}
+
+
+//DEFINING VARIABLES
+char cmd[2];
+char raw_gyro[6];
+char raw_mag[6];
+char store,status;
+int16_t bit_data;
+float gyro_data[3], mag_data[3],combined_values[6];
+float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
+float senstivity_mag  =32.768; //senstivity is obtained from 2^15/1000microtesla
+float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
+
+void  FCTN_ACS_INIT()
+{
+    ACS_INIT_STATUS = 's';     //set ACS_INIT_STATUS flag
+    FLAG();
+    pc_acs.printf("Attitude sensor init called \n \r");
+    //FLAG();
+    cmd[0]=RESETREQ;
+    cmd[1]=BIT_RESREQ;
+    i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
+    wait_ms(2000); //waiting for loading configuration file stored in EEPROM
+    cmd[0]=SENTRALSTATUS;
+    i2c.write(SLAVE_ADDR,cmd,1);
+    i2c.read(SLAVE_ADDR_READ,&store,1);
+    wait_ms(100);
+    //to check whether EEPROM is uploaded
+    switch((int)store) { 
+        case(3): {
+            break;
+        }
+        case(11): {
+            break;
+        }
+        default: {
+            cmd[0]=RESETREQ;
+            cmd[1]=BIT_RESREQ;
+            i2c.write(SLAVE_ADDR,cmd,2);
+            wait_ms(2000);
+        }
+    }
+    pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+    cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
+    cmd[1]=BIT_RUN_ENB;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
+    cmd[1]=BIT_MAGODR;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
+    cmd[1]=BIT_GYROODR;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
+    cmd[1]=0x00;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
+    cmd[1]=BIT_EVT_ENB;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
+}
+
+void FCTN_ATS_DATA_ACQ()
+{
+    ACS_DATA_ACQ_STATUS = 's';        //set ACS_DATA_ACQ_STATUS flag for att sens 2
+    if( ACS_ATS_ENABLE == 'e')
+    {
+    FLAG();
+    pc_acs.printf("attitude sensor execution called \n \r");
+    toFlag=1; //toFlag is set to 1 so that it enters while loop
+    to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated 
+    while(toFlag) {
+        cmd[0]=EVT_STATUS;
+        i2c.write(SLAVE_ADDR,cmd,1);
+        i2c.read(SLAVE_ADDR_READ,&status,1);
+        wait_ms(100);
+        pc_acs.printf("Event Status is %x\n \r",(int)status);
+        //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
+        if(((int)status&40)==40) {
+            cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 
+            i2c.write(SLAVE_ADDR,cmd,1);
+            i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
+            cmd[0]=MAG_XOUT_H; //LSB of x
+            i2c.write(SLAVE_ADDR,cmd,1);
+            i2c.read(SLAVE_ADDR_READ,raw_mag,6);
+        //    pc_acs.printf("\nGyro Values:\n");
+            for(int i=0; i<3; i++) {
+                //concatenating gyro LSB and MSB to get 16 bit signed data values
+                bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; 
+                gyro_data[i]=(float)bit_data;
+                gyro_data[i]=gyro_data[i]/senstivity_gyro;
+                gyro_data[i]+=gyro_error[i];
+    //            pc_acs.printf("%f\t",gyro_data[i]);
+            }
+       //     pc_acs.printf("\nMag Values:\n");
+            for(int i=0; i<3; i++) {
+                //concatenating mag LSB and MSB to get 16 bit signed data values
+                bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
+                mag_data[i]=(float)bit_data;
+                mag_data[i]=mag_data[i]/senstivity_mag;
+                mag_data[i]+=mag_error[i];
+      //          pc_acs.printf("%f\t",mag_data[i]);
+            }
+            for(int i=0; i<3; i++) {
+                data[i]=gyro_data[i];
+                data[i+3]=mag_data[i];
+            }
+          //  return(combined_values); //returning poiter combined values
+        } 
+       //checking for the error
+        else if (((int)status&2)==2) {
+            FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
+        }
+    }
+    }
+    else    //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
+    {
+       ACS_DATA_ACQ_STATUS = 'f';   
+    }
+    ACS_DATA_ACQ_STATUS = 'c';        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+}
+
+void FCTN_ACS_GENPWM_MAIN(float Moment[3])
+{
+    printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
+    
+    float l_duty_cycle_x=0;    //Duty cycle of Moment in x direction
+    float l_current_x=0;       //Current sent in x TR's
+    float l_duty_cycle_y=0;    //Duty cycle of Moment in y direction
+    float l_current_y=0;       //Current sent in y TR's
+    float l_duty_cycle_z=0;    //Duty cycle of Moment in z direction
+    float l_current_z=0;       //Current sent in z TR's
+ 
+    
+    for(int i = 0 ; i<3;i++)
+    {
+     //   printf(" %f \t ",Moment[i]);  // taking the moment values from control algorithm as inputs
+    }
+    
+    //-----------------------------  x-direction TR  --------------------------------------------//
+    
+    
+    float l_moment_x = Moment[0];         //Moment in x direction
+    
+    phase_TR_x = 1;  // setting the default current direction
+    if (l_moment_x <0)
+    {
+        phase_TR_x = 0;    // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
+        l_moment_x = abs(l_moment_x);
+    }
+    
+    l_current_x = l_moment_x * TR_CONSTANT ;        //Moment and Current always have the linear relationship
+    pc_acs.printf("current in trx is %f \r \n",l_current_x);
+    if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+    {
+        l_duty_cycle_x =  6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation 
+        pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+        PWM1.period(TIME_PERIOD);
+        PWM1 = l_duty_cycle_x/100 ;
+    }
+    else if( l_current_x >= 0.006 && l_current_x < 0.0116)
+    { 
+        l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+        PWM1.period(TIME_PERIOD);
+        PWM1 = l_duty_cycle_x/100 ;             
+    }
+    else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
+    {
+        l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+        PWM1.period(TIME_PERIOD);
+        PWM1 = l_duty_cycle_x/100 ;            
+    }
+    else if(l_current_x >= 0.0624 && l_current_x < 0.555)
+    {
+        l_duty_cycle_x =  331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+        PWM1.period(TIME_PERIOD);
+        PWM1 = l_duty_cycle_x/100 ;            
+    }
+    else if(l_current_x==0)
+    {
+        printf("\n \r l_current_x====0");
+        l_duty_cycle_x = 0;      // default value of duty cycle
+        pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+        PWM1.period(TIME_PERIOD);
+        PWM1 = l_duty_cycle_x/100 ;            
+    }
+    else                                           //not necessary
+    {
+        g_err_flag_TR_x = 1;
+    } 
+         
+    //------------------------------------- y-direction TR--------------------------------------//
+    
+     
+    float l_moment_y = Moment[1];         //Moment in y direction
+    
+    phase_TR_y = 1;  // setting the default current direction
+    if (l_moment_y <0)
+    {
+        phase_TR_y = 0;   //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high  
+        l_moment_y = abs(l_moment_y);
+    }
+    
+    
+    l_current_y = l_moment_y * TR_CONSTANT ;        //Moment and Current always have the linear relationship
+     pc_acs.printf("current in try is %f \r \n",l_current_y);
+    if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
+    {
+        l_duty_cycle_y =  6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+        PWM2.period(TIME_PERIOD);
+        PWM2 = l_duty_cycle_y/100 ;
+    }
+    else if( l_current_y >= 0.006 && l_current_y < 0.0116)
+    { 
+        l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+        PWM2.period(TIME_PERIOD);
+        PWM2 = l_duty_cycle_y/100 ;             
+    }
+    else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
+    {
+        l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+        PWM2.period(TIME_PERIOD);
+        PWM2 = l_duty_cycle_y/100 ;            
+    }
+    else if(l_current_y >= 0.0624 && l_current_y < 0.555)
+    {
+        l_duty_cycle_y =  331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+        PWM2.period(TIME_PERIOD);
+        PWM2 = l_duty_cycle_y/100 ;            
+    }
+    else if(l_current_y==0)
+    {
+        printf("\n \r l_current_y====0");
+        l_duty_cycle_y = 0; // default value of duty cycle
+        pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+        PWM2.period(TIME_PERIOD);
+        PWM2 = l_duty_cycle_y/100 ;            
+    }
+    else                               // not necessary
+    {
+      g_err_flag_TR_y = 1;
+    } 
+             
+    //----------------------------------------------- z-direction TR -------------------------//  
+    
+      
+    float l_moment_z = Moment[2];         //Moment in z direction
+    
+    phase_TR_z = 1;   // setting the default current direction
+    if (l_moment_z <0)
+    {
+        phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
+        l_moment_z = abs(l_moment_z);
+    }
+    
+    
+    l_current_z = l_moment_z * TR_CONSTANT ;        //Moment and Current always have the linear relationship
+     pc_acs.printf("current in trz is %f \r \n",l_current_z);
+    if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
+    {
+        l_duty_cycle_z =  6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+        PWM3.period(TIME_PERIOD);
+        PWM3 = l_duty_cycle_z/100 ;
+    }
+    else if( l_current_z >= 0.006 && l_current_z < 0.0116)
+    { 
+        l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+        PWM3.period(TIME_PERIOD);
+        PWM3 = l_duty_cycle_z/100 ;             
+    }
+    else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
+    {
+        l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+        PWM3.period(TIME_PERIOD);
+        PWM3 = l_duty_cycle_z/100 ;            
+    }
+    else if(l_current_z >= 0.0624 && l_current_z < 0.555)
+    {
+        l_duty_cycle_z =  331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
+        pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+        PWM3.period(TIME_PERIOD);
+        PWM3 = l_duty_cycle_z/100 ;            
+    }
+    else if(l_current_z==0)
+    {
+        printf("\n \r l_current_z====0");
+        l_duty_cycle_z = 0; // default value of duty cycle
+        pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+        PWM3.period(TIME_PERIOD);
+        PWM3 = l_duty_cycle_z/100 ;            
+    }
+    else                               // not necessary
+    {
+        g_err_flag_TR_z = 1;
+    }   
+    
+    //-----------------------------------------exiting the function-----------------------------------//
+    
+    printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
+ 
+}
+
+
+    
\ No newline at end of file