Revision 0:93514bd41116, committed 2014-12-18

Comitter:

sakthipriya

Date:

Thu Dec 18 12:20:47 2014 +0000

Commit message:

reset prob solved n cdms reforms the structure from hk data

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ACS.cpp	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,586 @@
+#include "ACS.h"
+#include "MPU3300.h"
+
+//PwmOut PWM1(PTD4);        //Functions used to generate PWM signal 
+                        //PWM output comes from pins p6
+Serial pc1(USBTX, USBRX);
+SPI spi_acs (PTA16, PTA17, PTA15); // mosi, miso, sclk
+DigitalOut SSN_MAG (D8); // ssn for magnetometer
+DigitalIn DRDY (D12); // drdy for magnetometer
+DigitalOut ssn_gyr (D13);         //Slave Select pin of gyroscope
+InterruptIn dr(D7);       //Interrupt pin for gyro 
+PwmOut PWM1(A3);        //Functions used to generate PWM signal 
+PwmOut PWM2(A4); 
+PwmOut PWM3(A5);                   //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 = 0;
+            PWM1.period(timep);
+            PWM1 = DCx/100 ;            
+         }
+         else
+         {
+            // printf("!!!!!!!!!!Error!!!!!!!!!");
+         } 
+    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()
+ {
+   
+  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");
+   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
+ 
+   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)
+    {
+        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;
+        
+        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,3);                // 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;
+}
+
+ 
+ 
+
+
+          

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ACS.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+#include "math.h"
+
+void  FUNC_ACS_GENPWM(float *);
+float *  FUNC_ACS_MAG_EXEC(void);
+void FUNC_ACS_MAG_INIT();
+//void  Read_data_acs()
+float *  FUNC_ACS_CNTRLALGO(float*,float*);
+float * FUNC_ACS_EXEC_GYR();
+void FUNC_ACS_INIT_GYRO();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HK.cpp	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,225 @@
+#include "HK.h"
+ 
+ 
+//GPIO pins used=> D2-D12, A0-A1
+
+DigitalOut SelectLinesA[]={D2,D3,D4,D5};//to mux1=>voltage mux
+DigitalOut SelectLinesB[]={PTB18,PTB19,PTC10};//to mux2=>current mux(differential mux)
+DigitalOut SelectLinesC[]={PTC0,PTC13,PTC6,PTC16};//to mux3=>temp mux
+
+//--------------------------------------------MSB is SelectLines[0],LSB is SelectLines[3]-------------------------------- 
+
+AnalogIn CurrentInput(A0); // Input from Current Mux
+AnalogIn VoltageInput(A1); // Input from Voltage Multiplexer
+AnalogIn TemperatureInput(A2); /*Input from Temperature Multiplexer,thermistor Multiplexer- same multiplexer for both(lines 1-4 for thermistor,line 0 for temperature sensor)*/
+ 
+
+
+ 
+int quantiz(float start,float step,float x)     // accepts min and measured values and step->quantises on a scale 0-15..(4 bit quantisation)
+{
+    int y=(x-start)/step;
+    if(y<=0)y=0;
+    if(y>=15)y=15;
+    return y;
+}
+ 
+void init_beacon(ShortBeacy* x,SensorDataQuantised y)  
+{
+    (*x).Voltage[0]=2;//quantised value
+    (*x).Temp[0]=y.PanelTemperature[0];//quantised value
+    (*x).Temp[1]=y.PanelTemperature[1];//quantised value
+    (*x).AngularSpeed[0]=y.AngularSpeed[0];
+    (*x).AngularSpeed[1]=y.AngularSpeed[1];
+    
+    (*x).SubsystemStatus[0]=145;//dummy values----------to be changed-------------------
+    (*x).ErrorFlag[0]=3;//dummy values----------to be changed-------------------
+}
+
+SensorData Sensor; 
+SensorDataQuantised SensorQuantised;
+ShortBeacy Shortbeacon;
+void FUNC_HK_MAIN()             
+{
+    //define structure variables    
+ 
+    
+    
+    
+    //initialise all selectlines to zeroes->1st line of muxes selected
+    SelectLinesA[0]=SelectLinesA[1]=SelectLinesA[2]=SelectLinesA[3]=0;
+    SelectLinesB[0]=SelectLinesB[1]=SelectLinesB[2]=0;
+    SelectLinesC[0]=SelectLinesC[1]=SelectLinesC[2]=SelectLinesC[3]=0;
+    
+    int LoopIterator;
+    int SelectLineIterator;
+    
+    float resistance_thermistor,voltage_thermistor;//for thermistor
+ 
+  //measurement from voltage sensor=> 16 sensors in place
+ for(LoopIterator=0; LoopIterator<16; LoopIterator++) 
+{       
+        //following lines read the sensor values and stores them in 'SensorData' structure's variable 'Sensor'
+        Sensor.Voltage[LoopIterator]=(VoltageInput.read()*3.3*5.545454);//resistors in voltage divider=>15Mohm,3.3Mohm
+               
+        if(LoopIterator%2==0)
+            SensorQuantised.Voltage[LoopIterator/2]=quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
+                           
+        else
+            SensorQuantised.Voltage[(LoopIterator)/2]=SensorQuantised.Voltage[(LoopIterator)/2]<<4+quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
+            
+    
+    
+    // The following lines are used to iterate the select lines from 0 to 15
+    //following is an algorithm similar to counting binary numbers of 4 bit
+       for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
+        {
+            if(SelectLinesA[SelectLineIterator]==0)
+            {
+                SelectLinesA[SelectLineIterator]=1;
+                break;
+            }
+            else SelectLinesA[SelectLineIterator]=0;
+    
+        }
+    
+    
+        wait_us(10.0); //  A delay of 10 microseconds between each sensor output. Can be changed.
+ 
+ }
+ 
+ 
+ 
+ 
+ 
+ //measurement from current sensor=>  8 sensors in place 
+
+    for(LoopIterator=0; LoopIterator<8; LoopIterator++) 
+{       
+        //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
+        Sensor.Current[LoopIterator]=(CurrentInput.read()*3.3/(50*rsens));
+        if(LoopIterator%2==0)
+            SensorQuantised.Current[LoopIterator/2]=quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
+        else
+            SensorQuantised.Current[(LoopIterator)/2]=SensorQuantised.Current[(LoopIterator)/2]<<4+quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
+
+        
+        // The following lines are used to iterate the select lines from 0 to 7
+        //following is an algorithm similar to counting binary numbers of 3 bits
+        for(SelectLineIterator=2;SelectLineIterator>=0;SelectLineIterator--)
+        {
+            if(SelectLinesB[SelectLineIterator]==0)
+            {
+                SelectLinesB[SelectLineIterator]=1;
+                break;
+            }
+            else SelectLinesB[SelectLineIterator]=0;
+    
+        }
+    
+    
+        wait_us(10.0); //  A delay of 10 microseconds between each sensor output. Can be changed.
+ 
+}
+    
+    
+//measurement of temperature
+//temperature measurement=> 4 thermistors, 1 temperature sensor
+//mux line 1=>temp sensor, mux lines 2 to 5 =>thermistors
+
+    for(LoopIterator=0; LoopIterator<5; LoopIterator++) 
+{       
+        //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
+        Sensor.Temperature[LoopIterator]=(-90.7*3.3*TemperatureInput.read()+190.1543);
+        voltage_thermistor=TemperatureInput.read()*3.3;//voltage across thermistor
+        resistance_thermistor=24000*voltage_thermistor/(3.3-voltage_thermistor);//resistance of thermistor
+        //PanelTemperature will be updated depending on voltage_thermistor value later in the lines to follow
+        
+        if(LoopIterator%2==0)
+     {                
+            if(LoopIterator<1)                      //->corresponding to temperature sensor
+                SensorQuantised.Temperature[(LoopIterator)/2]=quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]);
+         
+            else                                    //->corresponding to thermistor
+            {    
+                if(voltage_thermistor<1.378) //Temperature>12 degC
+                    Sensor.PanelTemperature[(LoopIterator-1)]=(3694/log(24.032242*resistance_thermistor));
+                    
+                else   
+                    Sensor.PanelTemperature[(LoopIterator-1)]=(3365.4792/log(7.60404*resistance_thermistor));
+                    
+                    
+                SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[(LoopIterator-1)]);
+                    
+            }
+            
+     } 
+    
+    else
+     {           
+            if(LoopIterator<1)
+                SensorQuantised.Temperature[(LoopIterator)/2]=SensorQuantised.Temperature[(LoopIterator)/2]<<4+quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]); 
+            
+            else
+             {  
+                if(voltage_thermistor<1.378) //Temperature>12 degC 
+                     Sensor.PanelTemperature[LoopIterator-1]=(3694/log(24.032242*resistance_thermistor));
+                                    
+                  
+                else
+                     Sensor.PanelTemperature[LoopIterator-1]=(3365.4792/log(7.60404*resistance_thermistor));
+                    
+                SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=SensorQuantised.PanelTemperature[(LoopIterator-1)/2]<<4+quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[LoopIterator-1]);
+                  
+            }
+            
+      }
+      
+strcpy(SensorQuantised.Voltage,"green");
+    
+// The following lines are used to iterate the select lines from 0 to 4
+    
+       //following is an algorithm similar to counting binary numbers of 4 bit
+       for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
+        {
+            if(SelectLinesC[SelectLineIterator]==0)
+            {
+                SelectLinesC[SelectLineIterator]=1;
+                break;
+            }
+            else SelectLinesC[SelectLineIterator]=0;
+    
+        }
+    
+    
+        wait_us(10.0); //  A delay of 10 microseconds between each sensor output. Can be changed.
+        
+}
+
+
+ 
+
+    
+    //update magnetometer data->
+    //populate values in structure variable 'Sensor' from data to be given by Green
+     SensorQuantised.AngularSpeed[0]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[1]);
+     SensorQuantised.AngularSpeed[0]=SensorQuantised.AngularSpeed[0]<<4+quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[0]);
+     SensorQuantised.AngularSpeed[1]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[2]);
+     
+     //update gyro data->
+    //populate values in structure variable 'Sensor' from data to be given by Green
+     SensorQuantised.Bnewvalue[0]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[1]);
+     SensorQuantised.Bnewvalue[0]=SensorQuantised.Bnewvalue[0]<<4+quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[0]);
+     SensorQuantised.Bnewvalue[1]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[2]);
+
+     
+     
+     
+      //update beacon structure
+    init_beacon(&Shortbeacon,SensorQuantised);//Shortbeacon is passed 
+    
+    
+}
+
+
+    

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HK.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,67 @@
+//to be saved as HK.h
+
+#include "mbed.h"
+#define tstart -40
+#define tstep 8
+#define tstep_thermistor 8//verify!!
+#define tstart_thermistor -40
+#define vstart 3.3
+#define vstep 0.84667
+#define cstart 0.0691
+#define cstep 0.09133
+#define rsens 0.095
+#define Bnewvalue_start -100//in microTesla...max possible field is .0001 T 
+#define Bnewvalue_step 13.333
+#define AngularSpeed_start -10//max possible ang. velocity in space is 10 deg/sec
+#define AngularSpeed_step 1.3333 
+
+typedef struct SensorData
+{
+    float Voltage[16];
+    float Current[8];
+    float Temperature[1];
+    float PanelTemperature[4];
+    float BatteryTemperature;   //to be populated
+    char faultpoll;             //polled faults
+    char faultir;               //interrupted faults
+    char power_mode;            //power modes
+    
+    float AngularSpeed[3];      //in order x,y,z
+    float Bnewvalue[3];         //in order Bx,By,Bz
+   
+    
+} SensorData;
+
+ 
+typedef struct SensorDataQuantised {
+    char Voltage[8];
+    char Current[4];
+    char Temperature[1];                        //string not necessary.to be removed
+    char PanelTemperature[2];//read by the 4 thermistors on solar panels
+    char BatteryTemperature;    //to be populated   
+    char faultpoll;             //polled faults
+    char faultir;               //interrupted faults
+    char power_mode;            //power modes
+    char AngularSpeed[2];
+    char Bnewvalue[2];
+    
+    //float magnetometer,gyro=>to be addes
+} SensorDataQuantised;                              //structure name defined twice.to be removed
+ 
+ 
+typedef struct ShortBeacon 
+{
+    char Voltage[1];                            //battery voltage from gauge, needs  to be quantised
+    char AngularSpeed[2];                       //all the 3 data
+    char SubsystemStatus[1];                    //power modes
+    char Temp[2];                               //temp of solar panel
+    //Temp[0]'s LSB=> PanelTemperature[0], Temp[0]'s MSB=> PanelTemperature[1], Temp[1]'s LSB=> PanelTemperature[2], Temp[1]'s MSB=> PanelTemperature[3]
+    char ErrorFlag[1];                          //fault
+}ShortBeacy; 
+ 
+ 
+ 
+void FUNC_HK_MAIN();
+ 
+int quantiz(float start,float step,float x);
+void init_beacon(ShortBeacy* x,SensorDataQuantised y);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU3300.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,26 @@
+//MPU 3300 registers
+#define SMPLRT_DIV    0x19
+#define CONFIG        0x1A
+#define GYRO_CONFIG   0x1B
+#define GYRO_XOUT_H   0x43
+#define GYRO_XOUT_L   0x44
+#define GYRO_YOUT_H   0x45
+#define GYRO_YOUT_L   0x46
+#define GYRO_ZOUT_H   0x47
+#define GYRO_ZOUT_L   0x48
+#define USER_CTRL     0x6A
+#define PWR_MGMT_1    0x6B
+#define INT_ENABLE    0x38
+
+//MPU configuration bits
+#define READFLAG                  0x80
+#define DUMMYBIT                  0x00
+#define BITS_DLPF_CFG             0x07
+#define BITS_FS_SEL_3             0x08
+#define BITS_FS_SEL_4             0x10
+#define BIT_I2C_IF_DIS            0x10
+#define BITS_SMPLRT_DIV           0x00
+#define BIT_SLEEP                 0x40
+#define BIT_DATA_RDY_ENABLE       0x01
+#define BIT_DATA_RDY_INT          0x01
+#define BIT_CLKSEL_X              0x01

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/beacon.cpp	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,200 @@
+//switch off the sync!!!!!!!
+//switch off the preamble!!!!!!!
+/*for crc in tx:
+regIrq2(0x28) :
+
+regpacketconfig 1(0x37) :
+set crc detection/calc. on : | 0x10
+crcautoclearoff : | 0x08  
+
+for data whitening : regpacketconfig 1(0x37) :| 0x40
+for 
+
+
+
+*/
+// 6CC000 for 435 MHz
+//set all values as FF for checking on spectrum analyzer
+#include "beacon.h"
+#include "HK.h"
+Serial chavan(USBTX, USBRX);           // tx, rx
+SPI spi(PTD6,PTD7,PTD5);               // mosi, miso, sclk
+DigitalOut cs_bar(PTC11);                 //slave select or chip select
+//InterruptIn button(p9);
+//#define TIMES 16      
+//Timer t;
+
+/*void interrupt_func()
+{
+    chavan.printf("INTERRUPT_FUNC TRIGGERED\n  wait for 3 secs\n"); 
+    wait(3);
+    
+}*/
+
+extern  ShortBeacy Shortbeacon;
+void writereg(uint8_t reg,uint8_t val)
+{
+    cs_bar = 0;
+    spi.write(reg | 0x80);      
+    spi.write(val);
+    cs_bar = 1;
+}
+uint8_t readreg(uint8_t reg)
+{
+    uint8_t val;
+    cs_bar = 0;
+    spi.write(reg & ~0x80);        
+    val = spi.write(0);
+    cs_bar = 1;
+    return val;
+}
+        
+void FUNC_BEA() {
+    
+    //button.rise(&interrupt_func);         //interrupt enabled ( rising edge of pin 9)
+    printf("\nBeacon function entered\n");
+    wait(0.02);                             //takes 10 ms for POR event + 10ms for safety                 
+    
+    uint8_t byte_counter = 0;
+    
+    /*struct Short_beacon{
+        uint8_t Voltage[1];
+        uint8_t AngularSpeed[2];
+        uint8_t SubsystemStatus[1];
+        uint8_t Temp[3];
+        uint8_t ErrorFlag[1];
+        }Shortbeacon = { {0x22}, {0x22, 0x33} , {0x00},{0x00,0x00,0x00}, {0xFE} };
+    */
+    //filling hk data
+    //ShortBeacon Shortbeacon;
+    uint8_t short_beacon[] = { 0xAB, 0x8A, 0xE2, 0xBB, 0xB8, 0xA2, 0x8E,Shortbeacon.Voltage[0],Shortbeacon.AngularSpeed[0], Shortbeacon.AngularSpeed[1],Shortbeacon.SubsystemStatus[0],Shortbeacon.Temp[0],Shortbeacon.Temp[1],Shortbeacon.Temp[2],Shortbeacon.ErrorFlag[0]};
+    
+    //mask
+    //uint8_t mask[] = {0x80, 0x40, 0x20,0x10,0x8,0x4,0x2,0x1};
+        
+    for(int i = 0; i < 15 ; i++)
+    {
+        chavan.printf("0x%X\n",(short_beacon[i]));    
+    }
+    
+    spi.format(8,0);                    
+    spi.frequency(10000000);             //10MHz SCLK frequency(its max for rfm69hcw)
+    cs_bar = 1;                              // Chip must be deselected
+    
+    //initialization
+    //Common configuration registers
+    writereg(0x01,0x04);       //sequencer on,standby mode
+    writereg(0x02,0x08);       //packet-mode used  , ook modultion , no dc-shaping
+    writereg(0x03,0x68);       //1200bps datarate
+    writereg(0x04,0x2B);       //1200bps datarate
+    writereg(0x07,0x6C);       //Frequency MSB
+    writereg(0x08,0xC0);       //Frequency MID
+    writereg(0x09,0x00);       //Frequency LSB        ....6C C0 00 for 435 MHZ   
+    
+    //Transmitter registers
+    // RegPaLevel(default +13 dBm)
+    
+    //IRQ and Pin Mapping Registers
+    //no DIO mapped yet
+    //regirq1(0x27): modeready (8th bit) will be checked for interrupts
+    //regIrq2(0x28): fifothresh (5th bit) ,packetsent(3rd bit) will be checked for interrupts
+    
+    //Packet Engine Registers
+    writereg(0x2C,0x00);        //set preamble
+    writereg(0x2D,0x0A);        //set preamble
+    writereg(0x2E,0x80);        //sync off        
+    writereg(0x2F,0x5E);        //sync word 1               
+    writereg(0x37,0x08 | 0x40);// | 0x10);        //packetconfig1, 0x40 for data whitening (only for testing)                
+    writereg(0x38,0x00);        //payload length = 0 ... unlimited payload mode
+    writereg(0x3C,0xB0);         //fifothresh = 48      because we want it cleared once its 40!!!!
+    //Initialization complete
+    
+    //while(chavan.getc() == 't'){
+    //t.start();  
+    //Filling Data into FIFO 64 BYTES : eff.32 bits = 4bytes                  //fread
+    cs_bar = 0; 
+    spi.write(0x80);//fifo write access
+    for(byte_counter=0 ; byte_counter<4; byte_counter++)
+    {    
+        for(int i=7; i>=0  ; i--)
+        {
+            if((short_beacon[byte_counter] & (uint8_t) pow(2.0,i))!=0)
+            //if((short_beacon[byte_counter] & mask[i]) != 0)
+            {
+                spi.write(0xFF);
+                spi.write(0xFF);
+            }
+    else
+            {
+                spi.write(0x00);
+                spi.write(0x00);
+            }
+        }
+    }
+    cs_bar = 1; //cs_bar
+                
+    //Check for fifoThresh
+    printf("\nfor loop executed\n");
+    while((readreg(0x28) & 0x20) != 0x20);                         
+    printf("\nwhile loop executed\n");
+    //Highpower settings
+    writereg(0x11,0x7F);    //RegPalevel (20db)                //~
+    writereg(0x13,0x0F);    //RegOCP
+    writereg(0x5A,0x5D);    //RegTestPa1
+    writereg(0x5C,0x7C);    //RegTestPa2
+        
+    //Set to Tx mode
+    writereg(0x01,0x0C);
+    
+        printf("\npre 2nd while loop\n");
+        //Check for fifoThresh
+        while((readreg(0x28) & 0x20) != 0x00);  
+        printf("\n2nd while loop executed\n");        
+        while(byte_counter!=15){
+            
+        //writing again
+        cs_bar = 0;
+        spi.write(0x80);   
+        for(int i=7; i>=0 ;i--)
+        {
+            if((short_beacon[byte_counter] & (uint8_t) pow(2.0,i))!=0)
+            //if((short_beacon[byte_counter] & mask[i]) != 0)
+        {
+            spi.write(0xFF);
+            spi.write(0xFF);
+        }
+        else
+        {
+            spi.write(0x00);
+            spi.write(0x00);
+        }
+        }
+        cs_bar = 1;
+        byte_counter++;
+                       
+        //Check for fifoThresh
+        while((readreg(0x28) & 0x20) != 0x00);
+    }
+    printf("\n3rd big while loop executed\n");
+    //wait for packet sent bit to fire
+    while((readreg(0x28) & 0x08) != 0x08);
+    printf("\n4th while loop executed\n");
+    //chavan.printf("packet sent!!! \n");    
+    
+    //Switch back to Standby Mode
+    writereg(0x01,0x04);
+    
+    //Lowpowermode
+    writereg(0x11,0x9F);    //RegPalevel (13db)
+    writereg(0x13,0x1A);    //RegOCP
+    writereg(0x5A,0x55);    //RegTestPa1(setting PA_BOOST on RFIO)
+    writereg(0x5C,0x70);    //RegTestPa2(setting PA_BOOST on RFIO)
+    
+    //wait for modeready
+    while((readreg(0x27)&0x80)!=0x80);
+    
+    //t.stop();
+    //chavan.printf(" time taken to init + transmit = %f \n", t.read()) ;
+    //}
+printf("\nBeacon function exiting\n");
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/beacon.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,5 @@
+#include "mbed.h"
+
+void writereg(uint8_t reg,uint8_t val);
+uint8_t readreg(uint8_t reg);
+void FUNC_BEA();

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fault.cpp	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,171 @@
+#include "fault.h"
+#include "HK.h"
+
+
+DigitalIn fault0(FAULT0,PullUp);
+DigitalIn fault1(FAULT1,PullUp);
+DigitalIn fault2(FAULT2,PullUp);
+DigitalIn fault3(FAULT3,PullUp);
+DigitalIn fault4(FAULT4,PullUp);
+//DigitalIn fault5(FAULT5,PullUp);
+//DigitalIn fault6(FAULT6,PullUp);
+//DigitalIn fault7(FAULT7,PullUp);
+//DigitalIn fault8(FAULT8,PullUp);
+
+
+InterruptIn fault_IR1(FAULT5);
+InterruptIn fault_IR2(FAULT6);
+InterruptIn fault_IR3(FAULT7);
+InterruptIn fault_IR4(FAULT8);
+InterruptIn fault_IR5(FAULT9);
+
+
+
+DigitalOut clear1(FAULT_CLEAR1,0);
+DigitalOut clear2(FAULT_CLEAR2,0);
+DigitalOut clear3(FAULT_CLEAR3,0);
+DigitalOut clear4(FAULT_CLEAR4,0);
+DigitalOut clear5(FAULT_CLEAR5);
+DigitalOut clear6(FAULT_CLEAR6);
+DigitalOut clear7(FAULT_CLEAR7);
+DigitalOut clear8(FAULT_CLEAR8);
+DigitalOut clear9(FAULT_CLEAR9);
+
+
+DigitalOut acs_active(ACS);
+
+
+BusIn fault_poll(FAULT0,FAULT1,FAULT2,FAULT3,FAULT4);
+BusIn fault_ir(FAULT5,FAULT6,FAULT7,FAULT8,FAULT9);
+
+BusOut clear_poll(FAULT_CLEAR1,FAULT_CLEAR2,FAULT_CLEAR3,FAULT_CLEAR4);//to send fault data along with hk
+BusOut clear_ir(FAULT_CLEAR5,FAULT_CLEAR6,FAULT_CLEAR7,FAULT_CLEAR8,FAULT_CLEAR9);
+
+
+extern SensorDataQuantised SensorQuantised;
+extern int beacon_sc;  //to switch beacon between low and high power mode
+extern int acs_pflag;  //to activate/deactivate control algo  
+char out_poll;
+char out_ir;
+
+void FUNC_HK_FAULTS()
+{
+        
+        printf("Entered Fault management \n");
+        /*if(fault0==0)printf("CHARGER IS CHARGING THE BATTERY ");
+        if(fault0==1)printf("CHRGER GONE OFFLINE, BATTERY DRAINING");
+        
+        if(fault1==0)
+        {  clear1=1;printf("");}
+        else
+        { clear1=0;}
+        
+         if(fault2==0)
+        {  clear2=1;printf("");}
+        else
+        { clear2=0;}
+        
+         if(fault3==0)
+        {  clear3=1;printf("");}
+        else
+        { clear3=0;}
+        
+         if(fault4==0)
+        {  clear4=1;printf("");}
+        else
+        { clear4=0;}*/
+      
+     
+        
+         
+        
+        
+        
+        
+        //clear1 = !fault1; 
+        //clear2 = !fault2; 
+        //clear3 = !fault3; 
+        //clear4 = !fault4; 
+        //clear5 = !fault5; 
+        //clear6 = !fault6; 
+        //clear7 = !fault7; 
+        //clear8 = !fault8; 
+        
+        
+       // out_poll = clear_poll;
+        //out_ir = clear_ir;
+        SensorQuantised.faultpoll = fault_poll ;
+        SensorQuantised.faultir=fault_ir ;
+        printf(" %d , %d \n ",SensorQuantised.faultpoll, SensorQuantised.faultir ) ;
+        
+}
+
+
+
+void Clear_IR1()
+{clear5=!clear5;
+}
+
+void Clear_IR2()
+{clear6=!clear6;
+}
+
+void Clear_IR3()
+{clear7=!clear7;
+}
+
+void Clear_IR4()
+{clear8=!clear8;
+}
+
+void Clear_IR5()
+{clear9=!clear9;
+}
+
+void interrupt_fault()
+{
+    fault_IR1.rise(&Clear_IR1);
+    fault_IR2.rise(&Clear_IR2);
+    fault_IR3.rise(&Clear_IR3);
+    fault_IR4.rise(&Clear_IR4);
+    fault_IR5.rise(&Clear_IR5);
+    fault_IR1.fall(&Clear_IR1);
+    fault_IR2.fall(&Clear_IR2);
+    fault_IR3.fall(&Clear_IR3);
+    fault_IR4.fall(&Clear_IR4);
+    fault_IR5.fall(&Clear_IR5);
+}
+    
+
+void FUNC_HK_POWER(char flag)                  //flag corresponds to the power mode
+{
+    printf("Entered Power Management \n"); 
+    printf("Entering mode %c \n", flag);
+    switch (flag)
+    {
+        case '0': beacon_sc = 6;            //least power mode
+                  acs_pflag = 0;
+                  acs_active = 0;            //switching off a component of acs 
+        break;
+        case '1': beacon_sc = 3;
+                  acs_pflag = 0;
+                  acs_active = 0;
+        break;
+        case '2': beacon_sc = 3;
+                  acs_pflag = 0;
+                  acs_active = 0;
+        break;
+        case '3': beacon_sc = 3;             //normal mode  
+                  acs_pflag = 1;
+                  acs_active = 1;
+        break;    
+    }
+  /*  if (flag == '0')
+    beacon_sc = 30;
+    else
+    beacon_sc = 3;
+    if (flag == '3')
+    acs_pflag = 1;
+    else
+    acs_pflag = 0; */
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fault.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,35 @@
+#include "mbed.h"
+#include "pin_config.h"
+
+#define FAULT0 PIN31
+#define FAULT1 PIN42      //  FAULTBAR
+#define FAULT2 PIN40      //  3V3APGOOD 
+#define FAULT3 PIN39      //  3V3BPGOOD                         
+#define FAULT4  PIN41     //  3V3CPGOOD      
+#define FAULT5  PIN79      //  3V3AOCBAR 
+#define FAULT6  PIN80      //  3V3COC  
+#define FAULT7  PIN97      //  SW8FAULT   
+#define FAULT8  PIN83      //  SW6FAULT   
+#define FAULT9 PIN89      //  SW7FAULT  
+
+//CONTROL SIGNALS
+#define FAULT_CLEAR1 D8                            
+#define FAULT_CLEAR2 D9
+#define FAULT_CLEAR3 D10
+#define FAULT_CLEAR4 D11
+#define FAULT_CLEAR5 D12
+#define FAULT_CLEAR6 D13
+#define FAULT_CLEAR7 PTC7
+#define FAULT_CLEAR8 PTC10
+#define FAULT_CLEAR9 PTC11
+
+
+
+#define ACS PTE2
+#define TX PTE3
+#define PAYLOAD PTE6
+
+
+void FUNC_HK_FAULTS();
+void FUNC_HK_POWER(char flag);
+void interrupt_fault();
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,456 @@
+    #include "mbed.h"
+#include "rtos.h"
+#include "HK.h"
+#include "slave.h"
+#include "beacon.h"
+#include "ACS.h"
+#include "fault.h"
+#include "slave.h"
+
+Serial pc(USBTX, USBRX);
+
+InterruptIn interrupt(D9);
+InterruptIn master_reset(D11);
+
+
+Timer t;                                                        //To know the time of execution each thread
+Timer t1;
+                                                       //To know the time of entering  of each thread
+
+Thread *ptr_t_hk_acq;
+Thread *ptr_t_acs;
+//Thread *ptr_t_acs_write2flash;
+Thread *ptr_t_bea;
+//Thread *ptr_t_bea_telecommand;
+//Thread *ptr_t_fault;
+Thread *ptr_t_i2c;
+Thread *ptr_t_wdt;  
+
+
+
+I2CSlave slave(D14,D15);                       //configuring pins p27, p28 as I2Cslave
+
+
+DigitalOut data_ready(D10);
+int i2c_status=0;                                 //read/write mode for i2c 0 : write2slave, 1 : write2master
+int reset=0;
+int temp;
+
+typedef struct
+{
+    char data[25];          // To avoid dynamic memory allocation
+    int length;
+}i2c_data;
+
+
+
+
+
+
+//Mail<i2c_data,16> i2c_data_receive;
+Mail<i2c_data,16> i2c_data_send;
+
+//--------------------------------------------------------------------------------------------------------------------------------------------------
+//TASK 2 : HK
+//--------------------------------------------------------------------------------------------------------------------------------------------------
+
+char hk_data[25];
+extern SensorDataQuantised SensorQuantised;
+void T_HK_ACQ(void const *args)
+{
+    
+    while(1)
+    {
+        Thread::signal_wait(0x2);
+        SensorQuantised.power_mode='0';
+        printf("\nTHIS IS HK    %f\n",t1.read());
+        t.start();
+        FUNC_HK_FAULTS();
+        FUNC_HK_POWER(SensorQuantised.power_mode);                                                   //The power mode algorithm is yet to be obtained
+        FUNC_HK_MAIN();                                                             //Collecting HK data
+        //thread_2.signal_set(0x4);
+        //FUNC_I2C_SLAVE_MAIN(25);    
+        FUNC_I2C_IR2CDMS();
+        t.stop();
+        printf("The time to execute hk_acq is %f seconds\n",t.read());
+        t.reset();
+    }
+}
+
+//---------------------------------------------------------------------------------------------------------------------------------------
+//TASK 1 : ACS
+//---------------------------------------------------------------------------------------------------------------------------------------
+
+int acs_pflag = 1;
+void T_ACS(void const *args)
+{
+    float *mag_field;
+    float *omega;
+    float *moment;
+    while(1)
+    {
+        Thread::signal_wait(0x1);
+        printf("\nTHIS IS ACS   %f\n",t1.read());
+        t.start();
+        mag_field= FUNC_ACS_MAG_EXEC();                              //actual execution
+        omega = FUNC_ACS_EXEC_GYR();
+        
+        
+        
+        if(acs_pflag == 1)
+        {
+        moment = FUNC_ACS_CNTRLALGO(mag_field,omega);
+        FUNC_ACS_GENPWM(moment);                     
+        }
+       
+        t.reset();
+    }
+}
+/*
+void T_ACS_WRITE2FLASH(void const *args)
+{
+    while(1)
+    {
+        //printf("Writing in the flash\n");
+        osEvent evt = q_acs.get();
+        if(evt.status == osEventMail)
+        {
+            sensor_data *ptr = (sensor_data*)evt.value.p;
+            FUNC_ACS_WRITE2FLASH(ptr);
+            q_acs.free(ptr);
+        }
+        printf("Writing acs data in the flash\n");
+    }
+}
+
+*/
+//---------------------------------------------------BEACON--------------------------------------------------------------------------------------------
+
+int beac_flag=0;                                                            //To receive telecommand from ground.
+
+
+/*void T_BEA_TELECOMMAND(void const *args)
+{
+    char c = pc.getc();
+    if(c=='a')
+    {
+        printf("Telecommand detected\n");
+        beac_flag=1;
+    }
+}
+*/
+
+void T_BEA(void const *args)
+{
+    
+    while(1)
+    {
+        Thread::signal_wait(0x3);
+        printf("\nTHIS IS BEACON    %f\n",t1.read());
+        t.start();
+        
+        
+            
+        FUNC_BEA();
+            
+        
+        if(beac_flag==1)
+        {
+            Thread::wait(600000);
+            beac_flag = 0;
+        }
+        
+        printf("The time to execute beacon thread is %f seconds\n",t.read());
+        t.reset();
+    }
+}
+
+//---------------------------------------------------------------------------------------------------------------------------------------------------
+//TASK 4 : FAULT MANAGEMENT
+//---------------------------------------------------------------------------------------------------------------------------------------------------
+//Dummy fault rectifier functions
+
+/*Mail<int,16> faults;
+
+void FUNC_FAULT_FUNCT1()
+{
+    printf("\nFault 1 detected... \n");
+}
+
+void FUNC_FAULT_FUNCT2()
+{
+    printf("\nFault 2 detected...\n");
+}
+
+void T_FAULT(void const *args)
+{
+    while(1)
+    {
+        osEvent evt = faults.get();
+        if(evt.status==osEventMail)
+        {
+            int *fault_id= (int *)evt.value.p;
+            switch(*fault_id)
+            {
+                case 1: FUNC_FAULT_FUNCT1();
+                        break;
+                case 2: FUNC_FAULT_FUNCT2();
+                        break;
+            }
+            faults.free(fault_id);
+        }
+    }
+}*/
+
+extern SensorDataQuantised SensorQuantised;
+
+
+/*void T_FAULT(void const *args)
+{   
+    Sensor.power_mode='0';
+    while(1)
+    {
+        Thread :: signal_wait(0x2);
+        FAULTS();
+        POWER(Sensor.power_mode);
+        //Sensor.power_mode++;            //testing ... should be removed
+    }    
+}
+*/
+/*-------------------------------------------------------------------------------------------------------------------------------------------
+-------------------------------------------------------WATCHDOG----------------------------------------------------------------------------*/
+DigitalOut trigger(PIN63);                                             // has to be changed
+void T_WDT(void const * args)
+{
+    trigger = 1;
+    while(true)
+    {
+        Thread::signal_wait(0x5);                               //signal set from scheduler or sthing. r RTOS timer nce the timing is finalized
+        printf("\nEntered WD\n");
+        trigger = !trigger;
+    }
+}
+
+//---------------------------------------------------------------------------------------------------------------------------------------------------
+//TASK 5 : i2c data
+//---------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+
+
+
+
+void FUNC_I2C_WRITE2CDMS(char *data, int length=1)
+{
+       int slave_status = 1;
+   
+    while(slave_status)
+    {                 
+            slave.address(0x20);
+            if(slave.receive()==1)
+            {
+                slave_status=slave.write(data,length);
+               
+                
+            }
+            else if(slave.receive()==3 || slave.receive()==2)
+            {
+                slave_status=slave.read(data,length);
+            }
+            
+    }
+            
+            printf("\ndone\n\r");
+
+}
+char data_send[25],data_receive;
+void T_I2C_BAE(void const * args)
+{
+     //char data_send,data_receive;
+    while(1)
+    {
+        Thread::signal_wait(0x4);
+        //T_I2C_BAE();
+        //i2c_status = temp;
+        //wait(0.5);
+        printf("\n entered thread   %d\n\r",i2c_status);
+        if(i2c_status == 0 && reset !=1)
+        {
+            
+            FUNC_I2C_WRITE2CDMS(&data_receive,1);
+            printf("\n received\n");
+            /*i2c_data * i2c_data_r = i2c_data_receive.alloc();
+            i2c_data_r->data = data_receive;
+            i2c_data_r->length = 1;
+            i2c_data_receive.put(i2c_data_r);*/
+            printf("\n Data received from CDMS is %c \n\r",data_receive);
+            FUNC_I2C_TC_EXECUTE(data_receive);                             // This has to be done from a differen thread
+            
+        }
+        else if(i2c_status ==1 && reset !=1)
+        {
+            osEvent evt = i2c_data_send.get();
+            if (evt.status == osEventMail) 
+            {
+                i2c_data *i2c_data_s = (i2c_data*)evt.value.p;
+                strcpy(data_send,i2c_data_s -> data);
+                FUNC_I2C_WRITE2CDMS(data_send,25);
+                printf("\nData sent to CDMS is %s\n\r",data_send);
+                i2c_data_send.free(i2c_data_s);
+                i2c_status = 0;
+                //temp = i2c_status;
+                
+            }
+        }  
+           
+    }
+}
+
+        
+
+void FUNC_I2C_INT()
+{
+   reset = 0;
+   ptr_t_i2c->signal_set(0x4);
+   
+     
+}
+
+void FUNC_I2C_RESET()
+{
+    reset = 1;
+}
+
+
+
+void FUNC_I2C_IR2CDMS()
+{
+        
+       
+        strcpy(hk_data,SensorQuantised.Voltage);
+        strcat(hk_data,SensorQuantised.Current); 
+        strcat(hk_data,SensorQuantised.Temperature); 
+        strcat(hk_data,SensorQuantised.PanelTemperature);
+        strcat(hk_data,SensorQuantised.AngularSpeed);
+        strcat(hk_data,SensorQuantised.Bnewvalue);
+        char fdata[5] = {SensorQuantised.BatteryTemperature,SensorQuantised.faultpoll,SensorQuantised.faultir,SensorQuantised.power_mode};
+        strcat(hk_data,fdata);
+        //strcpy(hk_data,"voltage-crn-0371234awbt");                 //to test if cdms forms the original structure from hk data sent
+        printf("\nhk data : %s\n",hk_data);
+        data_ready=0;
+        //data = pcslave.getc();
+        reset =0;
+        i2c_status=1;
+        i2c_data * i2c_data_s = i2c_data_send.alloc();
+        strcpy(i2c_data_s->data,hk_data);
+        i2c_data_s->length = 25;
+        i2c_data_send.put(i2c_data_s); 
+        data_ready=1;
+        //temp = i2c_status;
+}
+      
+
+//------------------------------------------------------------------------------------------------------------------------------------------------
+//TELECOMMAND
+//------------------------------------------------------------------------------------------------------------------------------------------------
+void FUNC_I2C_TC_EXECUTE (char command)  
+{ switch(command)
+   {   case '0' : printf("command 0 executed");
+       break;
+       case '1' : printf("command 1 executed");
+       break;
+       case '2' : printf("command 2 executed");
+       break;
+       case '3' : printf("command 3 executed");
+    }
+}  
+   
+
+//------------------------------------------------------------------------------------------------------------------------------------------------
+//SCHEDULER
+//------------------------------------------------------------------------------------------------------------------------------------------------
+int beacon_sc = 3;
+uint16_t schedcount=1;
+void T_SC(void const *args)
+{
+    
+    printf("The value of i in scheduler is %d\n",schedcount);
+    if(schedcount == 65532)                         //to reset the counter
+    {
+        schedcount = 0;
+    }
+    
+    if(schedcount%1==0)
+    {
+        ptr_t_acs -> signal_set(0x1);
+        ptr_t_wdt -> signal_set(0x5);
+    }
+    if(schedcount%2==0)
+    {
+        //ptr_t_fault -> signal_set(0x2);
+        ptr_t_hk_acq -> signal_set(0x2);
+        
+    }
+    if(schedcount%beacon_sc==0)
+    {
+        if(beac_flag==0)
+        {
+            
+            //ptr_t_bea -> signal_set(0x3);
+        }
+    }
+    schedcount++;
+}
+    
+//---------------------------------------------------------------------------------------------------------------------------------------------
+
+int main()
+{
+    t1.start();
+    
+    ptr_t_hk_acq = new Thread(T_HK_ACQ);
+    ptr_t_acs = new Thread(T_ACS);
+    //ptr_t_acs_write2flash = new Thread(T_ACS_WRITE2FLASH);
+    ptr_t_bea = new Thread(T_BEA);
+    //ptr_t_bea_telecommand = new Thread(T_BEA_TELECOMMAND);
+    //ptr_t_fault = new Thread(T_FAULT);
+    ptr_t_i2c = new Thread(T_I2C_BAE);
+    //ptr_t_sc = new Thread(T_SC);
+    ptr_t_wdt = new Thread(T_WDT);
+  
+    interrupt_fault();
+    
+    //ptr_t_fault -> set_priority(osPriorityRealtime);
+    ptr_t_acs->set_priority(osPriorityAboveNormal);
+    ptr_t_i2c->set_priority(osPriorityHigh);
+    ptr_t_hk_acq->set_priority(osPriorityAboveNormal);
+    //ptr_t_acs_write2flash->set_priority(osPriorityBelowNormal);
+    ptr_t_bea->set_priority(osPriorityAboveNormal);
+    //ptr_t_bea_telecommand->set_priority(osPriorityIdle);
+    //ptr_t_sc->set_priority(osPriorityAboveNormal);
+    ptr_t_wdt -> set_priority(osPriorityIdle);
+    
+  
+   // ----------------------------------------------------------------------------------------------
+    //printf("\n T_FAULT priority is %d",ptr_t_fault->get_priority()); 
+    printf("\n T_ACS priority is %d",ptr_t_acs->get_priority());
+    printf("\n T_HK_ACQ priority is %d",ptr_t_hk_acq->get_priority());
+    //printf("\n T_ACS_WRITE2FLASH priority is %d",ptr_t_acs_write2flash->get_priority());
+    printf("\n T_BEA priority is %d",ptr_t_bea->get_priority());  
+    RtosTimer t_sc_timer(T_SC,osTimerPeriodic);
+    t_sc_timer.start(10000);
+    printf("\n%f\n",t1.read()); 
+     
+     
+      
+    master_reset.fall(&FUNC_I2C_RESET);
+    interrupt.rise(&FUNC_I2C_INT);
+       
+    while(1)
+    {   
+        //Thread::wait(10000);
+         //ir2master(); 
+         Thread::wait(5000);
+    }
+    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#13a25134ac60

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pin_config.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,101 @@
+// 100 LQFP format pin assignment
+#define PIN1 PTE0
+#define PIN2 PTE1
+#define PIN3 PTE2
+#define PIN4 PTE3
+#define PIN5 PTE4
+#define PIN6 PTE5
+#define PIN7 PTE6
+//#define 8 
+//#define 9 
+//#define 10 
+//#define 11 
+//#define 12 
+//#define 13 
+#define PIN14 PTE16
+#define PIN15 PTE17
+#define PIN16 PTE18
+#define PIN17 PTE19
+#define PIN18 PTE20
+#define PIN19 PTE21
+#define PIN20 PTE22
+#define PIN21 PTE23
+//#define 22 
+//#define 23 
+//#define 24 
+//#define 25 
+#define PIN26 PTE29
+#define PIN27 PTE30
+#define PIN28 PTE31
+//#define 29 
+//#define 30 
+#define PIN31 PTE24
+#define PIN32 PTE25
+#define PIN33 PTE26
+#define PIN34 PTA0
+#define PIN35 PTA1
+#define PIN36 PTA2
+#define PIN37 PTA3
+#define PIN38 PTA4
+#define PIN39 PTA5
+#define PIN40 PTA6
+#define PIN41 PTA7
+#define PIN42 PTA12
+#define PIN43 PTA13
+#define PIN44 PTA14
+#define PIN45 PTA15
+#define PIN46 PTA16
+#define PIN47 PTA17
+//#define 48 
+//#define 49 
+#define PIN50 PTA18
+#define PIN51 PTA19
+#define PIN52 PTA20
+#define PIN53 PTB0
+#define PIN54 PTB1
+#define PIN55 PTB2
+#define PIN56 PTB3
+#define PIN57 PTB7
+#define PIN58 PTB8
+#define PIN59 PTB9
+#define PIN60 PTB10
+#define PIN61 PTB11
+#define PIN62 PTB16
+#define PIN63 PTB17
+#define PIN64 PTB18
+#define PIN65 PTB19
+#define PIN66 PTB20
+#define PIN67 PTB21
+#define PIN68 PTB22
+#define PIN69 PTB23
+#define PIN70 PTC0
+#define PIN71 PTC1
+#define PIN72 PTC2
+#define PIN73 PTC3
+//#define 74 
+//#define 75 
+#define PIN76 PTC20
+#define PIN77 PTC21
+#define PIN78 PTC22
+#define PIN79 PTC23
+#define PIN80 PTC4
+#define PIN81 PTC5
+#define PIN82 PTC6
+#define PIN83 PTC7
+#define PIN84 PTC8
+#define PIN85 PTC9
+#define PIN86 PTC10
+#define PIN87 PTC11
+#define PIN88 PTC12
+#define PIN89 PTC13
+#define PIN90 PTC16
+#define PIN91 PTC17
+#define PIN92 PTC18
+#define PIN93 PTD0
+#define PIN94 PTD1
+#define PIN95 PTD2
+#define PIN96 PTD3
+#define PIN97 PTD4
+#define PIN98 PTD5
+#define PIN99 PTD6
+#define PIN100 PTD7
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/slave.h	Thu Dec 18 12:20:47 2014 +0000
@@ -0,0 +1,13 @@
+#include "mbed.h"
+#define WriteGeneral 3
+#define ReadAddressed 1
+#define slave_address 0x20
+
+
+void FUNC_I2C_WRITE2CDMS(char*,int);
+
+void FUNC_I2C_IR2CDMS();
+void FUNC_I2C_INT();
+void FUNC_I2C_RESET();
+void FUNC_I2C_TC_EXECUTE(char);
+