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:
- 19:403cb36e22ed
- Parent:
- 18:21740620c65e
--- a/ACS.cpp Mon Jun 13 13:44:31 2016 +0000
+++ b/ACS.cpp Tue Jun 28 10:11:54 2016 +0000
@@ -16,7 +16,6 @@
extern uint8_t ACS_ATS_STATUS;
extern uint8_t ACS_MAIN_STATUS;
extern uint8_t ACS_STATUS;
-extern uint8_t ACS_DETUMBLING_ALGO_TYPE;
extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
@@ -44,15 +43,12 @@
Serial pc_acs(USBTX,USBRX); //for usb communication
//CONTROL_ALGO
-float moment[3]; // Unit: Ampere*Meter^2
float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
-float db[3];
uint8_t flag_firsttime=1, alarmmode=0;
-void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1);
float max_array(float arr[3]);
void inverse(float mat[3][3],float inv[3][3]);
@@ -61,20 +57,21 @@
-void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3],int nominal)
+void FCTN_ACS_CNTRLALGO (float moment[3],float b[3] ,float omega[3],uint8_t nominal,uint8_t detumbling,uint8_t ACS_DETUMBLING_ALGO_TYPE)
{
-
+ float db[3];
float normalising_fact;
float b_copy[3], omega_copy[3], db_copy[3];
int i;
if(flag_firsttime==1)
{
for(i=0;i<3;i++)
- {
+ {
db[i]=0; // Unit: Tesla/Second
- }
+ }
flag_firsttime=0;
}
+
else
{
for(i=0;i<3;i++)
@@ -83,18 +80,18 @@
}
}
- if(nominal == 0)
+ if(nominal == 0)
{
if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
- {
- alarmmode=0;
- }
+ {
+ alarmmode=0;
+ }
else if(max_array(omega)>OmegaMax&& alarmmode==0)
- {
- alarmmode=1;
- }
+ {
+ alarmmode=1;
+ }
}
@@ -108,51 +105,52 @@
omega_copy[i]=omega[i];
}
- if((alarmmode==0)|| (nominal == 1))
- {
- controlmodes(b,db,omega,0);
- for (i=0;i<3;i++)
+ if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
{
- b[i]=b_copy[i];
- db[i]=db_copy[i];
- omega[i]=omega_copy[i];
- }
- if(max_array(moment)>MmntMax)
- {
- controlmodes(b,db,omega,1);
+ controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE);
for (i=0;i<3;i++)
{
b[i]=b_copy[i];
db[i]=db_copy[i];
omega[i]=omega_copy[i];
}
- if(max_array(moment)>MmntMax)
- {
- normalising_fact=max_array(moment)/MmntMax;
- for(i=0;i<3;i++)
- {
- moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
- }
- }
+ if(max_array(moment)>MmntMax)
+ {
+ controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
+ for (i=0;i<3;i++)
+ {
+ b[i]=b_copy[i];
+ db[i]=db_copy[i];
+ omega[i]=omega_copy[i];
+ }
+ if(max_array(moment)>MmntMax)
+ {
+ normalising_fact=max_array(moment)/MmntMax;
+ for(i=0;i<3;i++)
+ {
+ moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+ }
+ }
}
- ACS_STATUS = 5;
+
}
- else
+
+ else
{
- controlmodes(b,db,omega,1);
- for (i=0;i<3;i++)
- {
- b[i]=b_copy[i];
- db[i]=db_copy[i];
- omega[i]=omega_copy[i];
- }
+ controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
+ for (i=0;i<3;i++)
+ {
+ b[i]=b_copy[i];
+ db[i]=db_copy[i];
+ omega[i]=omega_copy[i];
+ }
if(max_array(moment)>MmntMax)
{
normalising_fact=max_array(moment)/MmntMax;
for(i=0;i<3;i++)
- {
- moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
- }
+ {
+ moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+ }
}
}
@@ -206,7 +204,7 @@
}
-void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1)
+void controlmodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
{
float bb[3]={0,0,0};
float d[3]={0,0,0};
@@ -230,73 +228,75 @@
if (singularity_flag==0)
{
for(i=0;i<3;i++)
- {
- db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
- }
+ {
+ db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
+ }
for(i=0;i<3;i++)
- {
- b[i]/=den; // Mormalized b. Hence no unit.
- }
+ {
+ b[i]/=den; // Mormalized b. Hence no unit.
+ }
if(b[2]>0.9 || b[2]<-0.9)
- {
- kz=kz2;
- kmu=kmu2;
- gamma=gamma2;
- }
+ {
+ kz=kz2;
+ kmu=kmu2;
+ gamma=gamma2;
+ }
for(i=0;i<2;i++)
- {
- Mu[i]=b[i];
- v[i]=-kmu*Mu[i];
- dv[i]=-kmu*db[i];
- z[i]=db[i]-v[i];
- u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
- }
+ {
+ Mu[i]=b[i];
+ v[i]=-kmu*Mu[i];
+ dv[i]=-kmu*db[i];
+ z[i]=db[i]-v[i];
+ u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
+ }
inverse(Jm,invJm,singularity_flag);
for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
{
- bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
+ for(j=0;j<3;j++)
+ {
+ bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
+ }
}
- }
for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
{
- d[i]+=bb[j]*invJm[i][j];
+ for(j=0;j<3;j++)
+ {
+ d[i]+=bb[j]*invJm[i][j];
+ }
}
- }
bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
bb[0]=0;
for(i=0;i<3;i++)
- {
- d[i]=invJm[2][i];
- invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
- invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
- invJm[0][i]=b[i];
- }
+ {
+ d[i]=invJm[2][i];
+ invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
+ invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
+ invJm[0][i]=b[i];
+ }
inverse(invJm,Jm,singularity_flag);
if (singularity_flag==0)
- {
- for(i=0;i<3;i++)
{
- for(j=0;j<3;j++)
- {
- tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
- }
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
+ }
+ }
+ for(i=0;i<3;i++)
+ {
+ bcopy[i]=b[i]*den;
+ }
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
+ Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
+ }
}
- for(i=0;i<3;i++)
- {
- bcopy[i]=b[i]*den;
- }
- for(i=0;i<3;i++)
- {
- Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
- Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
- }
- }
+
}
+
if (singularity_flag==1)
{
for (i=0;i<3;i++)
@@ -304,26 +304,29 @@
Mmnt[i]=2*MmntMax;
}
}
+ ACS_STATUS = 5;
}
+
else if(controlmode1==1)
{
- if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
- {
- for(i=0;i<3;i++)
+ if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
{
- Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
+ }
+ ACS_STATUS = 6;
}
- ACS_STATUS = 6;
- }
- else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
- {
- for(i=0;i<3;i++)
+ else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
{
- Mmnt[i]=-kdetumble*db[i];
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=-kdetumble*db[i];
+ }
+ ACS_STATUS = 4;
}
- ACS_STATUS = 4;
- }
}
+
for(i=0;i<3;i++)
{
moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
@@ -336,17 +339,10 @@
int SENSOR_INIT();
int FCTN_ATS_DATA_ACQ(); //data is obtained
int SENSOR_DATA_ACQ();
-void T_OUT(); //timeout function to stop infinite loop
int CONFIG_UPLOAD();
-Timeout to; //Timeout variable to
-int toFlag;
int count =0; // Time for which the BAE uC is running (in seconds)
-void T_OUT()
-{
- toFlag=0; //as T_OUT function gets called the while loop gets terminated
-}
//DEFINING VARIABLES
@@ -358,7 +354,9 @@
int16_t bit_data;
uint16_t time_data;
float gyro_data[3], mag_data[3];
-float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
+
+
+int ack;
int CONFIG_UPLOAD()
{
@@ -391,17 +389,48 @@
}
+
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(600); //waiting for loading configuration file stored in EEPROM
+ wait(1);
+ ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
+
+ pc_acs.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
+
+ if( ack!=0)
+ {
+ cmd[0]=RESETREQ;
+ cmd[1]=BIT_RESREQ;
+ ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
+ if(ack !=0)
+ return 0;
+ }
+
+ wait_ms(600);
+
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&store,1);
+
+ if( ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&store,1);
+ if(ack!=0)
+ return 0;
+ }
+
+
pc_acs.printf("Sentral Status is %x\n \r",(int)store);
@@ -416,13 +445,33 @@
default: {
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
wait_ms(600);
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(100);
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&store,1);
+
+ if( ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&store,1);
+ if(ack!=0)
+ return 0;
+ }
+
pc_acs.printf("Sentral Status is %x\n \r",(int)store);
}
@@ -430,12 +479,19 @@
int manual=0;
- if( ((int)store != 11 )&&((int)store != 11))
+
+ if( ((int)store != 11 )&&((int)store != 3))
{
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
wait_ms(600);
manual = CONFIG_UPLOAD();
@@ -450,32 +506,87 @@
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);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
cmd[1]=BIT_MAGODR;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
cmd[1]=BIT_GYROODR;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
- cmd[1]=0x00;
+ cmd[1]=0x00;
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(100);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
+
cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
cmd[1]=0x00;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
cmd[1]=BIT_EVT_ENB;
- i2c.write(SLAVE_ADDR,cmd,2);
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+
+ ack= i2c.read(SLAVE_ADDR_READ,&store,1);
+ if( ack!=0)
+ {
+ ack= i2c.read(SLAVE_ADDR_READ,&store,1);
+ if(ack!=0)
+ return 0;
+ }
+
+
pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
if( (int)store == 3) //Check if initialised properly and not in idle state
@@ -483,51 +594,7 @@
pc_acs.printf("Exited sensor init successfully\n \r");
return 1;
}
-
- else if((int)store == 11)
- {
- 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);
-
- cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(100);
- pc_acs.printf("Sentral Status after trying again is %x\n \r",(int)store);
-
- if( (int)store != 3)
- {
- pc_acs.printf("Problem with initialising\n \r");
- return 0;
- }
- }
-
+
pc_acs.printf("Sensor init failed \n \r") ;
return 0;
}
@@ -543,25 +610,24 @@
pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
- if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
+ if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
{
pc_acs.printf("Sensor 1 marked working \n \r");
working = SENSOR_INIT();
if(working ==1)
{
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
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"); //Sensor 1 INIT failure and power off
ATS1_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
}
@@ -573,15 +639,21 @@
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"); //Sensor2 INIT successful
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
ACS_INIT_STATUS = 0;
return 2;
}
+
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
}
@@ -591,8 +663,8 @@
- ATS2_SW_ENABLE = 1;
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
+
+
ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
return 0;
}
@@ -600,50 +672,55 @@
int SENSOR_DATA_ACQ()
{
- int mag_only=0;
+
pc_acs.printf("Entering Sensor data acq.\n \r");
char status;
-
int sentral;
int event;
int sensor;
int error;
int init;
- int ack1;
- int ack2;
+ uint8_t gyro_error=0;
+ uint8_t mag_error=0;
+
cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- //wait_ms(100);
+
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
event = (int)status;
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
-
- printf("Ack1: %d Ack2 : %d\n",ack1,ack2);
-
- if((ack1!=0)||(ack2!=0))
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
{
- cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- //wait_ms(100);
- cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
- if((ack1!=0)||(ack2!=0))
- {
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed
- actual_data.Bvalue_actual[i] = 0;
- }
-
- return 1;
- }
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
}
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
+
sentral = (int) status;
@@ -659,264 +736,247 @@
init = SENSOR_INIT();
- int ack1,ack2;
cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- //wait_ms(100);
+
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
event = (int)status;
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
- sentral = (int)status;
-
- if((ack1!=0)||(ack2!=0))
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
{
- cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- event = (int)status;
- wait_ms(100);
- cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
- sentral = (int)status;
- wait_ms(100);
-
- if((ack1!=0)||(ack2!=0))
- {
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed
- actual_data.Bvalue_actual[i] = 0;
- }
-
- return 1;
- }
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
}
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
+
+
+ sentral = (int) status;
pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status
- {
-
- int ack1,ack2;
- char status;
- cmd[0]=ERROR;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- error = (int)status;
-
- cmd[0]=SENSORSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
- sensor = (int)status;
-
-
- if((ack1!=0)||(ack2!=0))
- {
- cmd[0]=ERROR;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
- error = (int)status;
- wait_ms(100);
-
- cmd[0]=SENSORSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
- sensor = (int)status;
- wait_ms(100);
-
- if((ack1!=0)||(ack2!=0))
- {
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed
- actual_data.Bvalue_actual[i] = 0;
- }
-
- return 1;
- }
- }
-
-
-
- if((error!=0) || (sensor!=0))
- {
- if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
- {
-
- if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
- {
-
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- actual_data.Bvalue_actual[i] = 0;
- }
-
- pc_acs.printf("error in both sensors.Exiting.\n \r");
- return 1;
- }
- pc_acs.printf("error in gyro alone.Exiting.\n \r");
+ {
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- }
-
- mag_only = 1;
- //return 2;
- }
-
- if(mag_only!= 1){
- pc_acs.printf("error in something else.Exiting.\n \r");
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- actual_data.Bvalue_actual[i] = 0;
- }
- return 1;
- }
- }
-
- if((event & 1 == 1 ))
- {
- pc_acs.printf("error in CPU Reset.\n \r");
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- actual_data.Bvalue_actual[i] = 0;
- }
- return 1;
-
- }
-
- if((event & 8 != 8 )||(event & 32 != 32 ))
- {
- pc_acs.printf("Data not ready waiting...\n \r");
- //POLL
- wait_ms(1000);
- cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(100);
- if((event & 8 != 8 )||(event & 32 != 32 ))
- {
-
- if(event & 32 != 32 )
- {
- if(event & 8 != 8 )
+ cmd[0]=ERROR;
+
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
{
- pc_acs.printf("Both data still not ready.Exiting..\n \r");
-
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- actual_data.Bvalue_actual[i] = 0;
- }
- return 1;
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
}
+
+ error = (int)status;
- pc_acs.printf("Mag data only ready.Read..\n \r");
- mag_only = 1;
- //return 2;
-
- }
+ cmd[0]=SENSORSTATUS;
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
+
+
+ sensor = (int) status;
+
- }
-
-
- }
-
-
- }
+ if((error!=0) || (sensor!=0))
+ {
+ if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
+ {
+ pc_acs.printf("error in gyro alone..\n \r");
+ gyro_error = 1;
+ }
+
+ if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
+ {
+
+ pc_acs.printf("error in mag alone.Exiting.\n \r");
+ mag_error = 1;
+ }
+
+ if( (gyro_error!=1)&&(mag_error!=1))
+ {
+ pc_acs.printf("error in something else.Exiting.\n \r");
+ return 0;
+
+ }
+ }
+
+ if((event & 1 == 1 ))
+ {
+ pc_acs.printf("error in CPU Reset.\n \r");
+ return 1;
+
+ }
+
+ if((event & 8 != 8 )||(event & 32 != 32 ))
+ {
+ pc_acs.printf("Data not ready waiting...\n \r");
+ //POLL
+ wait_ms(200);
+
+ cmd[0]=EVT_STATUS;
+
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,1);
+ if(ack!=0)
+ return 0;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ {
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
+ event = (int)status;
+ if(event & 32 != 32 )
+ {
- if(mag_only !=1)
- {
- if(mag_only!= 1){
- pc_acs.printf("Error in something else.Exiting.\n \r");
- for(int i=0; i<3; i++) {
- actual_data.AngularSpeed_actual[i] = 0; //Set values to 0
- actual_data.Bvalue_actual[i] = 0;
- }
- return 1;
- }
+ pc_acs.printf("Mag data only ready.Read..\n \r");
+ gyro_error = 1;
+
+ }
+
+ if(event & 8 != 8 )
+ {
+ pc_acs.printf("Both data still not ready.Exiting..\n \r");
+ mag_error=1;
+ }
+
+
+
+
+ }
+
+
+ }
+
+ if((mag_error !=1)&&(gyro_error!=1))
+ {
+ pc_acs.printf("Error in something else.Exiting.\n \r");
+ return 0;
+ }
+
+ if((mag_error !=1)&&(gyro_error!=1))
+ {
+ pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
+ return 0;
+ }
- }
+ }
- }
+
cmd[0]=MAG_XOUT_H; //LSB of x
i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
- ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24);
+ ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
- if(ack1 != 0)
+ if(ack != 0)
{
wait_ms(100);
cmd[0]=MAG_XOUT_H; //LSB of x
i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
- ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24);
+ ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
wait_ms(100);
- if(ack1 !=1)
- {
- for(int i=0;i<3;i++)
- {
- actual_data.Bvalue_actual[i] = 0;
- actual_data.AngularSpeed_actual[i] = 0;
- }
- return 1;
- }
-
+ if(ack !=1)
+ return 0;
+
}
// 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)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
- gyro_data[i]=(float)bit_data;
- gyro_data[i]=gyro_data[i]/senstivity_gyro;
- gyro_data[i]+=gyro_error[i];
+
+ if (gyro_error!=1)
+ {
+ for(int i=0; i<3; i++) {
+ //concatenating gyro LSB and MSB to get 16 bit signed data values
+ bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
+ gyro_data[i]=(float)bit_data;
+ gyro_data[i]=gyro_data[i]/senstivity_gyro;
+ actual_data.AngularSpeed_actual[i] = gyro_data[i];
+ }
}
- for(int i=0; i<3; i++) {
- //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
- bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
- mag_data[i]=(float)bit_data;
- mag_data[i]=mag_data[i]/senstivity_mag;
- mag_data[i]+=mag_error[i];
+
+ if(mag_error!=1){
+ for(int i=0; i<3; i++) {
+ //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
+ bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
+ mag_data[i]=(float)bit_data;
+ mag_data[i]=mag_data[i]/senstivity_mag;
+ actual_data.Bvalue_actual[i] = 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];
- }
-
+
+
- if(mag_only == 0)
+ if(mag_error == 1)
{
- pc_acs.printf("Reading data successful.\n \r");
- return 0;
+ pc_acs.printf("Gyro only successful.\n \r");
+ return 1;
}
- else if(mag_only == 1)
+ if(gyro_error == 1)
{
-
- for(int i=0;i<3;i++)
- {
- actual_data.AngularSpeed_actual[i] = 0;
- }
-
-
- pc_acs.printf("Reading data partial success.\n \r");
- return 2;
+ pc_acs.printf("Mag only successful.\n \r");
+ return 2;
}
- pc_acs.printf("Reading data success.\n \r");
- return 0;
+ pc_acs.printf("Reading data success.\n \r");
+ return 3;
}
@@ -925,173 +985,434 @@
int FCTN_ATS_DATA_ACQ()
{
+ for(int i=0; i<3; i++) {
+ actual_data.AngularSpeed_actual[i] = 0;
+ actual_data.Bvalue_actual[i] = 0;
+ }
+
int acq;
+ int init;
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))
+ if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) //ATS1 status is 01XX to check if ATS1 is ON
+ {
+
+ acq = SENSOR_DATA_ACQ(); //ATS1 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
+
+ if(acq == 3) //Both available read and exit
+ {
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+
+ 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 3;
+ }
+ else if((acq == 2)||(acq==1)) //Only mag or only gyro
+ {
+
+ pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
+
+
+ if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
+
+ {
+
+ //other sensor both working, off or
+ //other sensor gyro working, this sensor not working , off
+ //other sensor mag working, this sensor not working,off
+
+
+ ATS1_SW_ENABLE = 1; //switch off sensor 1
+ wait_ms(5);
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
+ }
+
+
+ ATS2_SW_ENABLE = 0; //switch on sensor 2
+ wait_ms(5);
+
+ init = SENSOR_INIT(); //sensor 2 init
+
+ if( init == 0)
+ {
+
+ pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ }
+ return acq;
+ }
+
+ int acq2;
+ acq2 = SENSOR_DATA_ACQ();
+
+ if(acq2 == 3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
+ return 3;
+ }
+
+ else if(acq2 == 1)
+ {
+ if(acq==2)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ return 3;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
+ return 1;
+ }
+ }
+
+ else if(acq2==2) //Sensor 2 mag only, exit in both cases
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ return 2;
+ }
+
+
+
+ else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
+ {
+
+ pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
+ return acq;
+
+ }
+
+ }
+
+ else //Sensor 2 not working or both sensors gyro/mag ONLY
+ {
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
+ return 1;
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ return 2;
+ }
+ pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
+ return acq;
+
+ }
+ }
+
+ else if(acq == 0)
+ {
+ pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5); //Switch ON sensor 2
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+ if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
+ {
+
+
+ init = SENSOR_INIT();
+
+ if( init == 0)
+ {
+
+ pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
+ ATS2_SW_ENABLE = 1;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
+ return 0;
+ }
+
+ int acq2;
+ acq2 = SENSOR_DATA_ACQ();
+
+ if(acq2 == 3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
+ return 3;
+ }
+
+ else if(acq2 == 1)
+ {
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
+ return 1;
+
+ }
+
+ else if(acq2 == 2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ return 2;
+ }
+
+
+ else if(acq2 == 0)
+ {
+
+ pc_acs.printf(" Sensor 2 data acq failure..\n \r");
+ ATS2_SW_ENABLE = 1;
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+ return 0;
+
+ }
+
+ }
+
+ }
+ }
+
+
+ if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) //ATS2 status is 01XX to check if ATS2 is ON
{
- 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;
+ acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
+
+ if(acq == 3) //Both available read and exit
+ {
- /*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;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+
+ pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+ pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
+ return 3;
+ }
+ else if((acq == 2)||(acq==1)) //Only mag or only gyro
+ {
+
+ pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
+
- int acq;
- acq = SENSOR_DATA_ACQ();
+ if( (ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
+
+ {
- 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;
- }
+ //other sensor both working, off or
+ //other sensor gyro working, this sensor not working , off
+ //other sensor mag working, this sensor not working,off
+
+
+ ATS2_SW_ENABLE = 1; //switch off sensor 2
+ wait_ms(5);
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
+ }
- else if(acq == 1)
- {
+
+ ATS1_SW_ENABLE = 0; //switch on sensor 1
+ wait_ms(5);
+
+ init = SENSOR_INIT(); //sensor 2 init
+
+ if( init == 0)
+ {
+
+ pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS2_SW_ENABLE = 0;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ }
+ return acq;
+ }
+
+ int acq2;
+ acq2 = SENSOR_DATA_ACQ();
- 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;
+ if(acq2 == 3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
+ return 3;
+ }
+
+ else if(acq2 == 1)
+ {
+ if(acq==2)
+ {
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ return 3;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
return 1;
- }
+ }
+ }
+
+ else if(acq2==2) //Sensor 1 mag only, exit in both cases
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ return 2;
+ }
+
+
+
+ else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
+ {
- 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.Go to sensor 2 again.\n \r");
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
+ return acq;
+
+ }
+
+ }
- }
-
- 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 //Sensor 1 not working or both sensors gyro/mag ONLY
+ {
+ if(acq == 1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
+ return 1;
+ }
+ if(acq==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ return 2;
+ }
+ pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
+ return acq;
+
+ }
}
- else if(acq == 1)
+ else if(acq == 0)
{
- 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;
+ pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5); //Switch ON sensor 1
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+ if( (ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
+ {
+
+
+ init = SENSOR_INIT();
+
+ if( init == 0)
+ {
+
+ pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
+ ATS2_SW_ENABLE = 1;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
+ return 0;
+ }
+
+ int acq2;
+ acq2 = SENSOR_DATA_ACQ();
+
+ if(acq2 == 3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
+ return 3;
+ }
+
+ else if(acq2 == 1)
+ {
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
+ return 1;
+
+ }
+
+ else if(acq2 == 2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ return 2;
+ }
+
+
+ else if(acq2 == 0)
+ {
+
+ pc_acs.printf(" Sensor 1 data acq failure..\n \r");
+ ATS1_SW_ENABLE = 1;
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+ return 0;
+
+ }
+
+ }
+
}
-
+ }
- }
+
-
-
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
- if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
- {
- 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_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;
+ return 0;
}
void FCTN_ACS_GENPWM_MAIN(float Moment[3])
@@ -1121,25 +1442,22 @@
}
l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
- printf("current in trx is %f \r \n",l_current_x);
+ pc_acs.printf("\r\n current in trx is %f \r \n",l_current_x);
if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
- printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
{
l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
- printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
- PWM1 = l_duty_cycle_x/100 ;
+ PWM1 = l_duty_cycle_x/100;
}
else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
{
l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
- printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
@@ -1147,7 +1465,6 @@
{
printf("\n \r l_current_x====0");
l_duty_cycle_x = 0; // default value of duty cycle
- printf("DC for trx is %f \r \n",l_duty_cycle_x);
PWM1.period(TIME_PERIOD);
PWM1 = l_duty_cycle_x/100 ;
}
@@ -1155,6 +1472,8 @@
{
g_err_flag_TR_x = 1;
}
+
+ pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
//------------------------------------- y-direction TR--------------------------------------//
@@ -1170,25 +1489,22 @@
l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
- printf("current in try is %f \r \n",l_current_y);
+ pc_acs.printf("current in try is %f \r \n",l_current_y);
if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
- printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
{
l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
- printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
{
l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
- printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
@@ -1196,7 +1512,6 @@
{
printf("\n \r l_current_y====0");
l_duty_cycle_y = 0; // default value of duty cycle
- printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
@@ -1204,6 +1519,8 @@
{
g_err_flag_TR_y = 1;
}
+
+ pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
//----------------------------------------------- z-direction TR -------------------------//
@@ -1220,11 +1537,10 @@
l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
- printf("current in trz is %f \r \n",l_current_z);
+ pc_acs.printf("current in trz is %f \r \n",l_current_z);
if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
{
l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
- printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
@@ -1238,7 +1554,6 @@
else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
{
l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
- printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
@@ -1246,7 +1561,6 @@
{
printf("\n \r l_current_z====0");
l_duty_cycle_z = 0; // default value of duty cycle
- printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
}
@@ -1255,6 +1569,8 @@
g_err_flag_TR_z = 1;
}
+ pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+
//-----------------------------------------exiting the function-----------------------------------//
printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function