Team Fox
FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_FULL_Flowchart_BAE
by 
Team Fox
Diff: ACS.cpp
- Revision:
- 16:cc77770d787f
- Parent:
- 15:e09aaaccf134
- Child:
- 17:1e1955f3db75
--- 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,®,1);
+ // wait_ms(100);
+ // sentral = (int)reg;
+
+ cmd[0]=SENSORSTATUS;
+ i2c.write(SLAVE_ADDR,cmd,1);
+ i2c.read(SLAVE_ADDR_READ,®,1);
+ wait_ms(100);
+
+ sensor = (int)reg;
+
+ cmd[0]=ERROR;
+ i2c.write(SLAVE_ADDR,cmd,1);
+ i2c.read(SLAVE_ADDR_READ,®,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,®,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])