Revision 16:cc77770d787f, committed 2016-06-03

Comitter:

Bragadeesh153

Date:

Fri Jun 03 13:53:55 2016 +0000

Parent:

15:e09aaaccf134

Commit message:

ACS Data_Acq updated. Tested and working.

Changed in this revision

diff -r e09aaaccf134 -r cc77770d787f ACS.cpp
--- a/ACS.cpp	Tue Apr 19 21:27:07 2016 +0000
+++ b/ACS.cpp	Fri Jun 03 13:53:55 2016 +0000
@@ -17,6 +17,9 @@
 extern uint8_t ACS_MAIN_STATUS;
 extern uint8_t ACS_STATUS;
 
+extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
+extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
+
 extern uint8_t ACS_ATS_ENABLE;
 extern uint8_t ACS_DATA_ACQ_ENABLE;
 extern uint8_t ACS_STATE;
@@ -60,11 +63,12 @@
 
 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
 {
+    
     float b1[3];
     float omega1[3];
     b1[0] = b[0]/1000000.0;
     b1[1] = b[1]/1000000.0;
-    b1[2] = b[2]/1000000.0;
+    b1[2] = b[2]/1000000.0; 
     
     omega1[0] = omega[0]*3.14159/180;
     omega1[1] = omega[1]*3.14159/180;
@@ -314,9 +318,13 @@
 
 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
+int FCTN_ACS_INIT(); //initialization of registers happens
+int SENSOR_INIT();
+int FCTN_ATS_DATA_ACQ(); //data is obtained
+int SENSOR_DATA_ACQ();
 void T_OUT(); //timeout function to stop infinite loop
+
+void CONFIG_UPLOAD();
 Timeout to; //Timeout variable to
 int toFlag; 
 
@@ -338,23 +346,58 @@
 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()
+void CONFIG_UPLOAD()
 {
-    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
+    wait_ms(600);
+    
+    //Verify magic number
+    
+    cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
+    cmd[1]=BIT_HOST_UPLD_ENB;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    
+    cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
+    cmd[1]=0x0000;
+    i2c.write(SLAVE_ADDR,cmd,3);
+    wait_ms(100);
+    
+    
+    
+    
+    cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
+    cmd[1]=0x00;
+    i2c.write(SLAVE_ADDR,cmd,2);
+    wait_ms(100);
+    
+    
+    
+}
+
+int SENSOR_INIT()
+{
+    
+    pc_acs.printf("Entered sensor init\n \r");
+    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(800); //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);
+    pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+    
     //to check whether EEPROM is uploaded
     switch((int)store) { 
         case(3): {
+            cmd[0]=RESETREQ;
+            cmd[1]=BIT_RESREQ;
+            i2c.write(SLAVE_ADDR,cmd,2);
+            wait_ms(600);
             break;
         }
         case(11): {
@@ -364,50 +407,349 @@
             cmd[0]=RESETREQ;
             cmd[1]=BIT_RESREQ;
             i2c.write(SLAVE_ADDR,cmd,2);
-            wait_ms(2000);
+            wait_ms(600);
+            
         }
     }
-    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);
+    cmd[0]=SENTRALSTATUS;
+    i2c.write(SLAVE_ADDR,cmd,1);
+    i2c.read(SLAVE_ADDR_READ,&store,1);
     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
+    pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+    
+    int manual=0;
+    if((int)store != 11)
+    {
+
+            cmd[0]=RESETREQ;
+            cmd[1]=BIT_RESREQ;
+            i2c.write(SLAVE_ADDR,cmd,2);
+            wait_ms(600);
+            
+            //manually upload
+            
+            if(manual == 0)
+            {
+                return 0;
+            }
+                     
+    }
+    
+        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]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
+        cmd[1]=0x00;
+        wait_ms(100);
+        
+        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);
+         pc_acs.printf("Exited sensor init successfully\n \r");
+        return 1;
+
+    
+}
+int FCTN_ACS_INIT()
+{
+    ACS_INIT_STATUS = 1;     //set ACS_INIT_STATUS flag
+    if( (ATS1_SW_ENABLE!= 0 )&&(ATS2_SW_ENABLE!= 0 ) )
+    {
+    ATS2_SW_ENABLE = 1;
+    ATS1_SW_ENABLE = 0;
+    wait_ms(5);
+    ACS_ATS_STATUS =  (ACS_ATS_STATUS&0x0F)|0x60;
+    }
+    
+    int working=0;
+
+    pc_acs.printf("Attitude sensor init called \n \r");
+    pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+    pc_acs.printf("ATS Status & 0xC0 is %x\n\n \r",(int)(ACS_ATS_STATUS&0xC0));
+    pc_acs.printf("Sensor 1 condition is %d \n\n \r",(int)((  (ACS_ATS_STATUS & 0xC0) != 0xC0)&&(  (ACS_ATS_STATUS & 0xC0) != 0x80)));
+    
+    
+    if((  (ACS_ATS_STATUS & 0xC0) != 0xC0)&&(  (ACS_ATS_STATUS & 0xC0) != 0x80))
+    {
+
+        pc_acs.printf("Sensor 1 marked working \n \r");
+        working = SENSOR_INIT();
+        if(working ==1)
+            {
+                ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+                pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+                pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
+                ACS_INIT_STATUS = 0;
+                return 1;
+            }
+            
+            
+            
+            pc_acs.printf("Sensor 1 not working.Powering off.\n \r");
+            ATS1_SW_ENABLE = 1;
+            ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
+  
+    }
+    
+    pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
+    
+    if((  (ACS_ATS_STATUS & 0x0C) != 0x0C)&&(  (ACS_ATS_STATUS & 0x0C) != 0x08))
+    {
+                        
+            
+            ATS2_SW_ENABLE = 0;
+            wait_ms(5);
+            working = SENSOR_INIT();
+            if(working ==1)
+            {
+                pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+                pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r");
+                ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+                ACS_INIT_STATUS = 0;
+                return 2;
+            }
+        
+        
+    }
+    
+    pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+    pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
+    
+    
+    
+    ATS2_SW_ENABLE = 1;
+    ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
+    ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
+    return 0;
 }
 
-void FCTN_ATS_DATA_ACQ()
+
+int SENSOR_DATA_ACQ()
 {
-    ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
-    if( ACS_ATS_ENABLE == 1)
-    {
-    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) {
+        int mag_only=0;
+        pc_acs.printf("Entering Sensor data acq.\n \r");
+        char reg;
+        
+        //int sentral;
+        int event;
+        int sensor;
+        int error;
+        
+        int init;
         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);
+        event = (int)status;   
+        
+                    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];
+                actual_data.AngularSpeed_actual[i] = gyro_data[i];
+                actual_data.Bvalue_actual[i] = mag_data[i];
+                //data[i+3]=mag_data[i];
+            }
+            
+                 
+
+        //(event & 40 != 40 ) || (event & 08 != 08 ) || (event & 01 == 01 )|| (event & 02 == 02 )
+        
+        if  (  (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ))    //check for any error
+        {
+            cmd[0]=RESETREQ;
+            cmd[1]=BIT_RESREQ;
+            i2c.write(SLAVE_ADDR,cmd,2);
+            wait_ms(600);
+            
+            
+            
+            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 after resetting is %x\n \r",(int)status);
+            
+            if  ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ))         
+            {
+                
+
+                
+                  //  cmd[0]=SENTRALSTATUS;
+                  //  i2c.write(SLAVE_ADDR,cmd,1);
+                  ///  i2c.read(SLAVE_ADDR_READ,&reg,1);
+                  //  wait_ms(100);
+                   // sentral = (int)reg;
+                    
+                    cmd[0]=SENSORSTATUS;
+                    i2c.write(SLAVE_ADDR,cmd,1);
+                    i2c.read(SLAVE_ADDR_READ,&reg,1);
+                    wait_ms(100);
+                    
+                    sensor = (int)reg;
+                    
+                    cmd[0]=ERROR;
+                    i2c.write(SLAVE_ADDR,cmd,1);
+                    i2c.read(SLAVE_ADDR_READ,&reg,1);
+                    wait_ms(100);
+                    
+                    error = (int)reg;
+                    
+                    if( error&128 == 128)
+                    {
+                                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]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
+                                cmd[1]=0x00;
+                                wait_ms(100);
+                                cmd[0]=ERROR;
+                                i2c.write(SLAVE_ADDR,cmd,1);
+                                i2c.read(SLAVE_ADDR_READ,&reg,1);
+                                wait_ms(100);
+                                
+                                error = (int)reg;
+                                
+                                if( error&128 == 128)
+                                    {
+                                        pc_acs.printf("Rate error.Exiting.\n \r");
+                                        return 1;
+                                    }
+                                    
+ 
+                    }
+                    
+                    
+                    if((error&16 == 16) || (error&32 == 32) || (sensor!=0))
+                     {
+                                if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16)  )
+                                     {
+                                                
+                                            if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64)  )
+                                                  {
+                                                         pc_acs.printf("error in both sensors.Exiting.\n \r");
+                                                         return 1;
+                                                  }
+                                             pc_acs.printf("error in gyro alone.Exiting.\n \r");
+                                             return 2;
+                                      }      
+                                        pc_acs.printf("error in something else.Exiting.\n \r");  
+                                        return 1;                       
+                     }
+                     
+                     if(((int)status & 1 == 1 ))
+                     {
+                         pc_acs.printf("error in CPU Reset.calling init.\n \r");
+                         init = SENSOR_INIT();
+                         if(init == 0)
+                            return 1;
+                            
+                        cmd[0]=EVT_STATUS;
+                        i2c.write(SLAVE_ADDR,cmd,1);
+                        i2c.read(SLAVE_ADDR_READ,&status,1);
+                        wait_ms(100);
+                        if(((int)status & 1 == 1 ))
+                        {
+                            pc_acs.printf("Still error in CPU Reset.Exiting.\n \r");
+                            return 1;
+                        }
+                        pc_acs.printf("error in CPU Reset cleared.\n \r");
+                         
+                      }
+                      
+                    if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
+                        {
+                            pc_acs.printf("Data not ready waiting...\n \r");
+                            //POLL
+                            wait_ms(1500);
+                            cmd[0]=EVT_STATUS;
+                            i2c.write(SLAVE_ADDR,cmd,1);
+                            i2c.read(SLAVE_ADDR_READ,&status,1);
+                            wait_ms(100);
+                            if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
+                            {
+
+                                if((int)status & 32 != 32 )
+                                {
+                                    if((int)status & 8 != 8 )
+                                    {
+                                        pc_acs.printf("Both data still not ready.Exiting..\n \r");
+                                        return 1;
+                                    }
+                                    pc_acs.printf("Mag data only ready.Read..\n \r");
+                                    mag_only = 1;
+                                    //return 2;
+                                    
+                                }
+                                
+                                
+                            }
+                            
+                                             
+                        }
+                        
+     
+            }
+            
+            
+            
+                    
+            
+        }
+        
+        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);
@@ -438,19 +780,201 @@
                 actual_data.Bvalue_actual[i] = mag_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
+       
+        if(mag_only == 0)
+        {
+ 
+          pc_acs.printf("Reading data successful.\n \r");
+          return 0;
+        }
+        else if(mag_only == 1)
+        {
+                pc_acs.printf("Reading data partial success.\n \r");
+                return 2;
         }
+        
+            pc_acs.printf("Reading data success.\n \r");
+            return 0;
+        
+}
+
+
+
+int FCTN_ATS_DATA_ACQ()
+{
+    
+    int acq;
+    
+    pc_acs.printf("DATA_ACQ  called \n \r");
+    pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+    
+    // 0 success //1 full failure //2 partial failure
+
+    
+    
+    if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
+    {
+
+        acq = SENSOR_DATA_ACQ();
+        if(acq == 0)
+            {
+                
+                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+                 
+                 ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+                 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+                 pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
+                 return 0;
+            }
+        else if(acq == 2)
+            {
+                    ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
+                    pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r");
+                    return 2;
+                    
+                    /*if((ACS_ATS_STATUS & 0x0F == 0x00))
+                        {
+                            pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r");
+                            ATS1_SW_ENABLE = 1;
+                            ATS2_SW_ENABLE = 0;
+                            wait_ms(5);
+                            ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
+                            
+                            int acq;
+                            acq = SENSOR_DATA_ACQ();
+                            
+                            if(acq == 0)
+                                {
+                                ACS_DATA_ACQ_STATUS = 0;
+                                 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
+                                return 0;
+                                
+                                }
+                            else if(acq == 2)
+                                {
+                                ACS_DATA_ACQ_STATUS = 2;
+                                pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
+                                return 2;
+                                }
+                            
+                            else if(acq == 1)
+                                {
+                                    
+                                        int acq;
+                                        pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
+                                        ATS2_SW_ENABLE = 1;
+                                        ATS1_SW_ENABLE = 0;
+                                        wait_ms(5);
+                                        acq = SENSOR_DATA_ACQ();
+                                        if(acq == 0)
+                                            {   
+                                                pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r");
+                                                ACS_DATA_ACQ_STATUS = 0;
+                                                return 0;
+                                            }
+                                        else if(acq == 2)
+                                            {
+                                                pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r");
+                                                ACS_DATA_ACQ_STATUS = 2;
+                                                return 2;
+                                            }
+                                        else
+                                            {
+                                                pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
+                                                ATS1_SW_ENABLE = 0;
+                                                ACS_DATA_ACQ_STATUS = 1;
+                                                return 1;
+                                            }
+                                            
+                                                pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
+                                                ATS1_SW_ENABLE = 0;
+                                                ACS_DATA_ACQ_STATUS = 1;
+                                                return 1;
+        
+                                }
+                        
+                        }
+                        
+                        else
+                        {
+                            ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
+                            pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
+                            return 2;
+                            
+                            
+                        }*/
+
+            }
+            
+        else if(acq == 1)
+            {
+                 pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r");
+                 ATS1_SW_ENABLE = 1;
+                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
+            }
+            
+
+  
     }
-    }
-    else    //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
+    
+        
+        
+                               
+        ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
+    
+      
+    if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
     {
-       ACS_DATA_ACQ_STATUS = 'f';   
+        ATS2_SW_ENABLE = 0;
+        wait_ms(5); 
+        
+        acq = SENSOR_DATA_ACQ();
+        if(acq == 0)
+            {
+                 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
+                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+                 ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+                 return 0;
+            }
+        else if(acq == 2)
+            {
+                 
+                 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
+                 
+                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04;
+                 ACS_DATA_ACQ_STATUS = 2;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+                 return 2;
+
+            }
+            
+            else if(acq == 1)
+            {
+                pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r");
+                ATS2_SW_ENABLE = 1;
+                 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
+                //Sensor 2 also not working
+            }
+            
+            
+            
+
+       
     }
-    ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+    
+        pc_acs.printf(" Reading value from sensor 1 before exiting\n \r");
+        ATS1_SW_ENABLE = 0;
+        wait_ms(5);
+        SENSOR_DATA_ACQ();
+        ATS1_SW_ENABLE = 1;
+        wait_ms(5);
+    
+         ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
+         
+    pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+    pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
+    
+    ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
+    return 1;
 }
 
 void FCTN_ACS_GENPWM_MAIN(float Moment[3])

diff -r e09aaaccf134 -r cc77770d787f ACS.h
--- a/ACS.h	Tue Apr 19 21:27:07 2016 +0000
+++ b/ACS.h	Fri Jun 03 13:53:55 2016 +0000
@@ -12,8 +12,8 @@
 extern void FLAG();
 
 void FCTN_ATS_SWITCH(bool);
-void FCTN_ACS_INIT(); //initialization of registers happens
+int FCTN_ACS_INIT(); //initialization of registers happens
 //void FCTN_ATS_DATA_ACQ(float*,float*); // main function: checks errors, gets data, switches on/off the sensor
 //void FCTN_GET_DATA(float*,float*); //data is obtained
 void FCTN_T_OUT(); //timeout function to stop infinite loop
-void FCTN_ATS_DATA_ACQ();
+int FCTN_ATS_DATA_ACQ();

diff -r e09aaaccf134 -r cc77770d787f TCTM.cpp
--- a/TCTM.cpp	Tue Apr 19 21:27:07 2016 +0000
+++ b/TCTM.cpp	Fri Jun 03 13:53:55 2016 +0000
@@ -51,10 +51,10 @@
 //extern void FCTN_ACS_GENPWM_MAIN();
 extern void F_EPS();
 extern void FCTN_ACS_GENPWM_MAIN(float Moment[3]);
-extern void FCTN_ACS_INIT();
+extern int FCTN_ACS_INIT();
 
 
-extern void FCTN_ATS_DATA_ACQ();
+extern int FCTN_ATS_DATA_ACQ();
 extern void FCTN_ACS_CNTRLALGO(float*,float*);
 uint8_t telemetry[135];
 

diff -r e09aaaccf134 -r cc77770d787f main.cpp
--- a/main.cpp	Tue Apr 19 21:27:07 2016 +0000
+++ b/main.cpp	Fri Jun 03 13:53:55 2016 +0000
@@ -30,7 +30,7 @@
 
 uint8_t ACS_INIT_STATUS = 0;
 uint8_t ACS_DATA_ACQ_STATUS = 0;
-uint8_t ACS_ATS_STATUS = 0;
+uint8_t ACS_ATS_STATUS = 0x60;
 uint8_t ACS_MAIN_STATUS = 0;
 uint8_t ACS_STATUS = 0;
 
@@ -95,6 +95,7 @@
 //*****************************************************Assigning pins******************************************************//
 DigitalOut ATS1_SW_ENABLE(PTC0); // enable of att sens2 switch
 DigitalOut ATS2_SW_ENABLE(PTC16); // enable of att sens switch
+
 InterruptIn irpt_4m_mstr(PIN38);                                      //I2c interrupt from CDMS
 DigitalOut irpt_2_mstr(PIN4);                                        //I2C interrupt to CDMS
 I2CSlave slave (PIN1,PIN2);///pin1 pin2
@@ -762,11 +763,18 @@
     ACS_STATE = 4;
     ACS_ATS_ENABLE = 1;
     ACS_DATA_ACQ_ENABLE = 1;
+    
+    
     EPS_BATTERY_HEAT_ENABLE = 1;
     actual_data.power_mode=3;
     //............intializing pins................//
+    
+    ATS2_SW_ENABLE = 1;
+    ATS1_SW_ENABLE = 1;
+    wait_ms(5);
     ATS1_SW_ENABLE = 0;
-    ATS2_SW_ENABLE = 1;
+    
+    ACS_ATS_STATUS = 0x60;  //Set Sensor 1 working , Sensor2 working and powered off by default
 
     DRV_XY_EN = 1;
     DRV_Z_EN = 1;

diff -r e09aaaccf134 -r cc77770d787f pni.h
--- a/pni.h	Tue Apr 19 21:27:07 2016 +0000
+++ b/pni.h	Fri Jun 03 13:53:55 2016 +0000
@@ -2,17 +2,20 @@
 #define SLAVE_ADDR_READ    0x51
 #define SENTRALSTATUS      0x37
 #define RESETREQ           0x9B
+#define ERROR              0x50
+#define SENSORSTATUS       0x36
 #define MAGRATE            0x55
 #define ACCERATE           0x56
 #define GYRORATE           0x57
 #define QRATE_DIV          0x32
-#define ALGO_CTRL           0x54
+#define ALGO_CTRL          0x54
 #define ENB_EVT            0x33
-#define HOST_CTRL           0x34
+#define HOST_CTRL          0x34
 #define EVT_STATUS         0x35
 #define ALGO_STATUS        0x38
 #define GYRO_XOUT_H        0x22
 #define MAG_XOUT_H         0X12
+#define UPLOAD_ADDR        0x94
 
 //Configaration bits
 #define BIT_RESREQ     0x01
@@ -33,5 +36,6 @@
 #define BIT_GYROODR    0x0F
 #define BIT_MAGODR     0x64
 #define BIT_RUN_ENB    0x01
+#define BIT_HOST_UPLD_ENB 0x02
 #define BIT_ALGO_RAW   0x02
-#define BIT_EVT_ENB    0X2A
\ No newline at end of file
+#define BIT_EVT_ENB    0X2B
\ No newline at end of file