Team Fox
/
BAE_ACS_FULLYDONE_153
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_FULL_Flowchart_BAE
by 
Team Fox
Revision 19:403cb36e22ed, committed 2016-06-28
- Comitter:
- Bragadeesh153
- Date:
- Tue Jun 28 10:11:54 2016 +0000
- Parent:
- 18:21740620c65e
- Commit message:
- FINAL ACS TO BE USED FOR TESTING
Changed in this revision
--- 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
--- a/ACS.h Mon Jun 13 13:44:31 2016 +0000 +++ b/ACS.h Tue Jun 28 10:11:54 2016 +0000 @@ -9,21 +9,20 @@ #define kdetumble 2000000 #define MmntMax 1.1 // Unit: Ampere*Meter^2 #define OmegaMax 1*3.1415/180.0 // Unit: Radians/Second -#define ACS_DEMAG_TIME_DELAY 20 +#define ACS_DEMAG_TIME_DELAY 65 +#define ACS_Z_FIXED_MOMENT 1.3 #define senstivity_gyro 6.5536; //senstivity is obtained from 2^15/5000dps #define senstivity_mag 32.768; //senstivity is obtained from 2^15/1000microtesla -#define senstivity_time 32; //senstivity is obtained from 2^16/2048dps +#define senstivity_time 32; //senstivity is obtained from 2^16/2048 s void FCTN_ACS_GENPWM_MAIN(float*); -void FCTN_ACS_CNTRLALGO(float*,float*,int); -void controlmodes(float*, float*, float*, uint8_t); +void FCTN_ACS_CNTRLALGO(float*,float*,float*,uint8_t,uint8_t,uint8_t); +void controlmodes(float*,float*, float*, float*, uint8_t,uint8_t); void inverse(float mat[3][3],float inv[3][3]); extern void FLAG(); void FCTN_ATS_SWITCH(bool); 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 int FCTN_ATS_DATA_ACQ();
--- a/TCTM.cpp Mon Jun 13 13:44:31 2016 +0000
+++ b/TCTM.cpp Tue Jun 28 10:11:54 2016 +0000
@@ -18,6 +18,7 @@
extern DigitalOut DRV_Z_EN;
extern DigitalOut DRV_XY_EN;
extern uint8_t ACS_STATUS;
+extern uint8_t ACS_ATS_STATUS;
extern float b_old[3]; // Unit: Tesla
extern float db[3];
extern uint8_t flag_firsttime;
@@ -64,9 +65,16 @@
extern int SENSOR_INIT();
extern int FCTN_ATS_DATA_ACQ();
-extern void FCTN_ACS_CNTRLALGO(float*,float*,int);
uint8_t telemetry[135];
+extern AnalogIn CurrentInput; // Input from Current Multiplexer //PIN54
+
+
+extern DigitalOut SelectLineb3; // MSB of Select Lines
+extern DigitalOut SelectLineb2;
+extern DigitalOut SelectLineb1;
+extern DigitalOut SelectLineb0; // LSB of Select Lines
+
void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
{
@@ -514,114 +522,67 @@
{
case 0xE0:
{
- float mag_data[3]={0,0,0};
- float gyro_data[3]={0,0,0};
+ float mag_data_comm[3]={0,0,0};
+ float gyro_data_comm[3]={0,0,0};
+ float moment_comm[3];
printf("ACS_COMSN SOFTWARE\r\n");
//ACK_L234_telemetry
ACS_STATE = tc[4];
- if(ACS_STATE == 7) // Nominal mode
+ if(ACS_STATE == 7) // Nominal mode
{
printf("\n\r Nominal mode \n");
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
+
- FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1);
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
- }
+ printf("%f\t",moment_comm[i]);
+ }
- ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY
}
else if(ACS_STATE == 8) // Auto Control
{
- printf("\n\r Auto control mode \n");
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
-
-
- FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0);
+ printf("\n\r Auto control mode \n");
+
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
+ printf("%f\t",moment_comm[i]);
}
}
else if(ACS_STATE == 9) // Detumbling
{
- DRV_Z_EN = 1;
- DRV_XY_EN = 1;
-
- if(flag_firsttime==1)
- {
- for(int i=0;i<3;i++)
- {
- db[i]=0; // Unit: Tesla/Second
- }
- flag_firsttime=0;
- }
-
- else
- {
- for(int i=0;i<3;i++)
- {
- db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
- }
- }
-
-
- if (ACS_DETUMBLING_ALGO_TYPE == 0)
- {
-
- for(int i=0;i<3;i++)
- {
- moment[i]=-kdetumble*(mag_data[(i+1)%3]*gyro_data[(i+2)%3]-mag_data[(i+2)%3]*gyro_data[(i+1)%3]); // Unit: Ampere*Meter^2
- }
-
-
- ACS_STATUS = 6; // set ACS_STATUS = ACS_BOMEGA_CONTROL
- }
-
- else if(ACS_DETUMBLING_ALGO_TYPE == 1)
- {
-
- for(int i=0;i<3;i++)
- {
- moment[i]=-kdetumble*db[i]; // Unit: Ampere*Meter^2
- }
-
- ACS_STATUS = 4; // set ACS_STATUS = ACS_BDOT_CONTROL
- }
-
- for(int i=0;i<3;i++)
- {
-
- b_old[i]= mag_data[i]; // Unit: Tesla/Second
- }
-
+
+ FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
- printf("%f\t",moment[i]);
- }
+ printf("%f\t",moment_comm[i]);
+ }
+
}
- ACS_STATUS = 7;
+ else
+ {
+
+ ACS_STATUS = 7;
+ }
// Control algo commissioning
- FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7]
- FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11]
- FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15]
+ FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7]
+ FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11]
+ FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15]
// to include commission TR as well
for(uint8_t i=16;i<132;i++)
{
@@ -638,8 +599,10 @@
printf("HARDWARE_COMSN\r\n");
//ACK_L234_telemetry
- int init;
- int data;
+ int init1=0;
+ int init2=0;
+ int data1=0;
+ int data2=0;
float PWM_tc[3];
PWM_tc[0]=(float(tc[4]))*1;
@@ -662,14 +625,135 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
ATS2_SW_ENABLE = 1;
+ wait_ms(5);
ATS1_SW_ENABLE = 0;
- init = SENSOR_INIT();
- data = SENSOR_DATA_ACQ();
+ wait_ms(5);
+ init1 = SENSOR_INIT();
+ if( init1 == 1)
+ {
+ data1 = SENSOR_DATA_ACQ();
+ }
+
+
+ ATS1_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+
+ if(data1 == 0)
+ {
+ ATS2_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+ }
+ else if(data1==1)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
+ }
+ else if(data1==2)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
+ }
+ else if(data1==3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30;
+ }
+
+
+ init2 = SENSOR_INIT();
+ if( init2 == 1)
+ {
+ data2 = SENSOR_DATA_ACQ();
+ }
+
+ uint8_t ats_data=1;
- ATS2_SW_ENABLE = 1;
- ATS1_SW_ENABLE = 0;
- init = SENSOR_INIT();
- data = SENSOR_DATA_ACQ();
+ if(data2 == 0)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+ if(data1 == 2)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ }
+ else if(data1 == 3)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ }
+ else if(data1 == 1)
+ {
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
+ ats_data = 0;
+ }
+
+
+ }
+
+ else if(data2==1)
+ {
+
+
+
+ if(data1 == 2)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
+ }
+ else if(data1 == 3)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
+ ats_data = 0;
+ }
+ }
+
+ else if(data2==2)
+ {
+ if(data1 == 3)
+ {
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+ ATS1_SW_ENABLE = 0;
+ wait_ms(5);
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+ }
+ else
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
+ }
+ }
+ else if(data2==3)
+ {
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
+ }
+
+ SelectLineb3 =0;
+ SelectLineb2 =1;
+ SelectLineb1 =0;
+ SelectLineb0 =1;
+ int resistance;
+
+
PWM1 = PWM_tc[0];
PWM2 = 0;
@@ -677,7 +761,23 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- SENSOR_DATA_ACQ();
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
+
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (0*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (1*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (2*4)]);
@@ -690,9 +790,23 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
-
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
- SENSOR_DATA_ACQ();
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
@@ -704,16 +818,54 @@
wait_ms(ACS_DEMAG_TIME_DELAY);
- wait_ms(ACS_DEMAG_TIME_DELAY);
-
- SENSOR_DATA_ACQ();
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]);
FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]);
+ PWM1 = 0;
+ PWM2 = 0;
+ PWM3 = 0;
-
+ wait_ms(ACS_DEMAG_TIME_DELAY);
+
+ if(ats_data == 0)
+ SENSOR_DATA_ACQ();
+ actual_data.current_actual[5]=CurrentInput.read();
+ actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens);
+
+
+ resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]);
+ if(actual_data.current_actual[5]>1.47)
+ {
+ actual_data.current_actual[5]=3694/log(24.032242*resistance);
+ }
+ else{
+
+ actual_data.current_actual[5]=3365.4/log(7.60573*resistance);
+ }
+
+
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (8*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (9*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (10*4)]);
+ FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (11*4)]);
// for(int i=0; i<12; i++)
// FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
--- a/main.cpp Mon Jun 13 13:44:31 2016 +0000
+++ b/main.cpp Tue Jun 28 10:11:54 2016 +0000
@@ -22,7 +22,7 @@
uint8_t ACS_INIT_STATUS = 0;
uint8_t ACS_DATA_ACQ_STATUS = 0;
-uint8_t ACS_ATS_STATUS = 0x60;
+uint8_t ACS_ATS_STATUS = 0x73;
uint8_t ACS_MAIN_STATUS = 0;
uint8_t ACS_STATUS = 0;
uint8_t ACS_DETUMBLING_ALGO_TYPE = 0;
@@ -65,10 +65,7 @@
extern float gyro_data[3];
extern float mag_data[3];
-extern float moment[3];
-extern float b_old[3]; // Unit: Tesla
-extern float db[3];
-extern uint8_t flag_firsttime;
+float moment[3];
extern uint8_t BCN_FEN;
@@ -147,7 +144,7 @@
void F_ACS()
{
-
+
pc.printf("Entered ACS.\n\r");
ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
@@ -156,6 +153,9 @@
PWM2 = 0; //clear pwm pins
PWM3 = 0; //clear pwm pins
+ wait_ms(ACS_DEMAG_TIME_DELAY);
+
+
ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
@@ -225,7 +225,7 @@
moment[0] = 0;
moment[1] = 0;
- moment[2] =1.3; // is a dummy value
+ moment[2] =ACS_Z_FIXED_MOMENT; // is a dummy value
@@ -236,7 +236,7 @@
}
- else if(ACS_DATA_ACQ_STATUS == 1)
+ else if((ACS_DATA_ACQ_STATUS == 0)||(ACS_DATA_ACQ_STATUS == 1))
{
DRV_Z_EN = 1;
DRV_XY_EN = 0;
@@ -245,7 +245,7 @@
moment[0] = 0;
moment[1] = 0;
- moment[2] =1.3; // is a dummy value
+ moment[2] =ACS_Z_FIXED_MOMENT; // is a dummy value
FCTN_ACS_GENPWM_MAIN(moment) ;
ACS_MAIN_STATUS = 0;
@@ -263,7 +263,7 @@
moment[0] = 0;
moment[1] = 0;
- moment[2] =1.3; // 1.3 is a dummy value
+ moment[2] =ACS_Z_FIXED_MOMENT; // 1.3 is a dummy value
FCTN_ACS_GENPWM_MAIN(moment) ;
ACS_MAIN_STATUS = 0;
@@ -278,32 +278,9 @@
ACS_STATUS = 4; // set Set ACS_STATUS = ACS_BDOT_CONTROL
- float db[3];
- if(flag_firsttime==1)
- {
- for(int i=0;i<3;i++)
- {
- db[i]=0; // Unit: Tesla/Second
- }
- flag_firsttime=0;
- }
-
- else
- {
- for(int i=0;i<3;i++)
- {
- db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
- }
- }
-
-
-
- for(int i=0;i<3;i++)
- {
- moment[i]=-kdetumble*db[i];
- b_old[i]= mag_data[i]; // Unit: Tesla/Second
- }
+ ACS_DETUMBLING_ALGO_TYPE = 0x01;
+ FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
@@ -313,6 +290,7 @@
FCTN_ACS_GENPWM_MAIN(moment) ;
+
ACS_MAIN_STATUS = 0;
return;
@@ -325,7 +303,7 @@
DRV_Z_EN = 1;
DRV_XY_EN = 1;
- FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1);
+ FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
@@ -334,7 +312,6 @@
}
FCTN_ACS_GENPWM_MAIN(moment) ;
- ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY
ACS_MAIN_STATUS = 0;
return;
@@ -348,7 +325,7 @@
DRV_Z_EN = 1;
DRV_XY_EN = 1;
- FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0);
+ FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE);
printf("\n\r Moment values returned by control algo \n");
for(int i=0; i<3; i++)
{
@@ -366,58 +343,7 @@
DRV_Z_EN = 1;
DRV_XY_EN = 1;
- if(flag_firsttime==1)
- {
- for(int i=0;i<3;i++)
- {
- db[i]=0; // Unit: Tesla/Second
- }
- flag_firsttime=0;
- }
-
- else
- {
- for(int i=0;i<3;i++)
- {
- db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
- }
- }
-
-
- if (ACS_DETUMBLING_ALGO_TYPE == 0)
- {
-
- for(int i=0;i<3;i++)
- {
- moment[i]=-kdetumble*(mag_data[(i+1)%3]*gyro_data[(i+2)%3]-mag_data[(i+2)%3]*gyro_data[(i+1)%3]); // Unit: Ampere*Meter^2
- }
-
-
- ACS_STATUS = 6; // set ACS_STATUS = ACS_BOMEGA_CONTROL
- }
-
- else if(ACS_DETUMBLING_ALGO_TYPE == 1)
- {
-
- for(int i=0;i<3;i++)
- {
- moment[i]=-kdetumble*db[i]; // Unit: Ampere*Meter^2
- }
-
- ACS_STATUS = 4; // set ACS_STATUS = ACS_BDOT_CONTROL
- }
-
- for(int i=0;i<3;i++)
- {
-
- b_old[i]= mag_data[i]; // Unit: Tesla/Second
- }
-
- printf("\n\r Moment values returned by control algo \n");
- for(int i=0; i<3; i++)
- {
- printf("%f\t",moment[i]);
- }
+ FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE);
FCTN_ACS_GENPWM_MAIN(moment) ;
ACS_MAIN_STATUS = 0;