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
Revision 17:1e1955f3db75, committed 2016-06-09
- Comitter:
- Bragadeesh153
- Date:
- Thu Jun 09 14:12:55 2016 +0000
- Parent:
- 16:cc77770d787f
- Child:
- 18:21740620c65e
- Commit message:
- ACS_LATEST_CODE
Changed in this revision
--- a/ACS.cpp Fri Jun 03 13:53:55 2016 +0000
+++ b/ACS.cpp Thu Jun 09 14:12:55 2016 +0000
@@ -16,6 +16,7 @@
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
@@ -40,56 +41,37 @@
extern BAE_HK_actual actual_data;
-//DigitalOut gpo1(PTC0); // enable of att sens2 switch
-//DigitalOut gpo2(PTC16); // enable of att sens switch
-
-
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
-int flag_firsttime=1, controlmode, alarmmode=0;
+float db[3];
+uint8_t flag_firsttime=1, alarmmode=0;
-void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode);
-void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
+
+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]);
//CONTROLALGO PARAMETERS
-void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
+
+
+void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3])
{
-
- float b1[3];
- float omega1[3];
- b1[0] = b[0]/1000000.0;
- b1[1] = b[1]/1000000.0;
- b1[2] = b[2]/1000000.0;
-
- omega1[0] = omega[0]*3.14159/180;
- omega1[1] = omega[1]*3.14159/180;
- omega1[2] = omega[2]*3.14159/180;
- controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
-
-}
-void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
-{
- float db1[3]; // Unit: Tesla/Second
- float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
- float MmntMax=1.1; // Unit: Ampere*Meter^2
- float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
+
float normalising_fact;
- float b1_copy[3], omega1_copy[3], db1_copy[3];
+ float b_copy[3], omega_copy[3], db_copy[3];
int i;
if(flag_firsttime==1)
{
for(i=0;i<3;i++)
{
- db1[i]=0; // Unit: Tesla/Second
+ db[i]=0; // Unit: Tesla/Second
}
flag_firsttime=0;
}
@@ -97,45 +79,43 @@
{
for(i=0;i<3;i++)
{
- db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
+ db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
}
- if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
+ if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
{
alarmmode=0;
}
- else if(max_array(omega1)>OmegaMax && alarmmode==0)
+ else if(max_array(omega)>OmegaMax&& alarmmode==0)
{
alarmmode=1;
}
for (i=0;i<3;i++)
{
- b1_copy[i]=b1[i];
- db1_copy[i]=db1[i];
- omega1_copy[i]=omega1[i];
+ b_copy[i]=b[i];
+ db_copy[i]=db[i];
+ omega_copy[i]=omega[i];
}
if(alarmmode==0)
{
- controlmode=0;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ controlmodes(b,db,omega,0);
for (i=0;i<3;i++)
{
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
+ b[i]=b_copy[i];
+ db[i]=db_copy[i];
+ omega[i]=omega_copy[i];
}
if(max_array(moment)>MmntMax)
{
- controlmode=1;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ controlmodes(b,db,omega,1);
for (i=0;i<3;i++)
{
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
+ b[i]=b_copy[i];
+ db[i]=db_copy[i];
+ omega[i]=omega_copy[i];
}
if(max_array(moment)>MmntMax)
{
@@ -149,13 +129,12 @@
}
else
{
- controlmode=1;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+ controlmodes(b,db,omega,1);
for (i=0;i<3;i++)
{
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
+ b[i]=b_copy[i];
+ db[i]=db_copy[i];
+ omega[i]=omega_copy[i];
}
if(max_array(moment)>MmntMax)
{
@@ -169,11 +148,11 @@
}
for (i=0;i<3;i++)
{
- b_old[i]=b1[i];
+ b_old[i]=b[i];
}
}
-void inverse(float mat[3][3],float inv[3][3])
+void inverse(float mat[3][3],float inv[3][3],int &singularity_flag)
{
int i,j;
float det=0;
@@ -185,11 +164,19 @@
}
}
det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
- for(i=0;i<3;i++)
+ if (det==0)
+ {
+ singularity_flag=1;
+ }
+ else
{
- for(j=0;j<3;j++)
+ singularity_flag=0;
+ for(i=0;i<3;i++)
{
- inv[i][j]/=det;
+ for(j=0;j<3;j++)
+ {
+ inv[i][j]/=det;
+ }
}
}
}
@@ -209,7 +196,7 @@
}
-void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
+void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1)
{
float bb[3]={0,0,0};
float d[3]={0,0,0};
@@ -219,95 +206,110 @@
int i, j;//temporary variables
float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
float invJm[3][3];
- float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000;
- int infflag; // Flag variable to check if the moment value is infinity or NaN
+ float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
+ int singularity_flag=0;
if(controlmode1==0)
{
den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
- for(i=0;i<3;i++)
+ if (den==0)
{
- 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.
+ singularity_flag=1;
}
- if(b[2]>0.9 || b[2]<-0.9)
- {
- kz=kz2;
- kmu=kmu2;
- gamma=gamma2;
- }
- for(i=0;i<2;i++)
+ if (singularity_flag==0)
{
- 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);
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
+ 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)
+ }
+ for(i=0;i<3;i++)
+ {
+ b[i]/=den; // Mormalized b. Hence no unit.
+ }
+ if(b[2]>0.9 || b[2]<-0.9)
+ {
+ 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);
+ }
+ 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(i=0;i<3;i++)
{
- 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++)
+ {
+ 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];
}
- }
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
+ inverse(invJm,Jm,singularity_flag);
+ if (singularity_flag==0)
{
- d[i]+=bb[j]*invJm[i][j];
+ 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
+ }
}
}
- 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++)
+ if (singularity_flag==1)
{
- 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);
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
+ for (i=0;i<3;i++)
{
- tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
+ Mmnt[i]=2*MmntMax;
}
}
- 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
- }
- infflag=0;
- for (i=0; i<3 && infflag==0; i++)
- {
- if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
- infflag=1;
- }
- if (infflag==1)
- {
- for (i=0; i<3; i++)
- Mmnt[i]=2*MmntMax;
- }
-
}
else if(controlmode1==1)
{
- 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
+ }
+ }
+ else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
+ {
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=-kdetumble*db[i];
+ }
}
}
for(i=0;i<3;i++)
@@ -324,7 +326,7 @@
int SENSOR_DATA_ACQ();
void T_OUT(); //timeout function to stop infinite loop
-void CONFIG_UPLOAD();
+int CONFIG_UPLOAD();
Timeout to; //Timeout variable to
int toFlag;
@@ -339,14 +341,14 @@
char cmd[2];
char raw_gyro[6];
char raw_mag[6];
+char reg_data[24];
char store,status;
int16_t bit_data;
-float gyro_data[3], mag_data[3],combined_values[6];
-float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
-float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
+uint16_t time_data;
+float gyro_data[3], mag_data[3];
float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
-void CONFIG_UPLOAD()
+int CONFIG_UPLOAD()
{
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
@@ -373,7 +375,7 @@
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(100);
-
+ return 0;
}
@@ -383,21 +385,17 @@
pc_acs.printf("Entered sensor init\n \r");
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
- wait_ms(800); //waiting for loading configuration file stored in EEPROM
+ 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
cmd[0]=SENTRALSTATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(100);
+
pc_acs.printf("Sentral Status is %x\n \r",(int)store);
- //to check whether EEPROM is uploaded
+ //to check whether EEPROM is uploaded properly
switch((int)store) {
case(3): {
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(600);
break;
}
case(11): {
@@ -409,16 +407,18 @@
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(600);
+ cmd[0]=SENTRALSTATUS;
+ i2c.write(SLAVE_ADDR,cmd,1);
+ i2c.read(SLAVE_ADDR_READ,&store,1);
+ wait_ms(100);
+ pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+
}
}
- cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(100);
- pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+
int manual=0;
- if((int)store != 11)
+ if( ((int)store != 11 )&&((int)store != 11))
{
cmd[0]=RESETREQ;
@@ -426,10 +426,11 @@
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(600);
- //manually upload
+ manual = CONFIG_UPLOAD();
if(manual == 0)
{
+ //MANUAL CONFIGURATION FAILED
return 0;
}
@@ -438,55 +439,99 @@
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[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to 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[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
cmd[1]=BIT_EVT_ENB;
i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(100);
- pc_acs.printf("Exited sensor init successfully\n \r");
- return 1;
+
+ cmd[0]=SENTRALSTATUS;
+ i2c.write(SLAVE_ADDR,cmd,1);
+ i2c.read(SLAVE_ADDR_READ,&store,1);
+ 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
+ {
+ 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;
+}
-
-}
int FCTN_ACS_INIT()
{
ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
- if( (ATS1_SW_ENABLE!= 0 )&&(ATS2_SW_ENABLE!= 0 ) )
- {
- ATS2_SW_ENABLE = 1;
- ATS1_SW_ENABLE = 0;
- wait_ms(5);
- ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- }
+
int working=0;
pc_acs.printf("Attitude sensor init called \n \r");
pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
- pc_acs.printf("ATS Status & 0xC0 is %x\n\n \r",(int)(ACS_ATS_STATUS&0xC0));
- pc_acs.printf("Sensor 1 condition is %d \n\n \r",(int)(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)));
- if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80))
+ 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");
@@ -494,7 +539,7 @@
if(working ==1)
{
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
- pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+ pc_acs.printf("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;
@@ -502,7 +547,7 @@
- pc_acs.printf("Sensor 1 not working.Powering off.\n \r");
+ 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;
@@ -510,7 +555,7 @@
pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
- if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08))
+ if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
{
@@ -520,7 +565,7 @@
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");
+ 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_INIT_STATUS = 0;
return 2;
@@ -536,7 +581,7 @@
ATS2_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
- ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
+ ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
return 0;
}
@@ -545,182 +590,216 @@
{
int mag_only=0;
pc_acs.printf("Entering Sensor data acq.\n \r");
- char reg;
+ char status;
- //int sentral;
+ int sentral;
int event;
int sensor;
int error;
+ int init;
- int init;
+ int ack1;
+ int ack2;
+
cmd[0]=EVT_STATUS;
i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(100);
- pc_acs.printf("Event Status is %x\n \r",(int)status);
- event = (int)status;
+ ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
+ //wait_ms(100);
+ 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);
- cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
- cmd[0]=MAG_XOUT_H; //LSB of x
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,raw_mag,6);
- // pc_acs.printf("\nGyro Values:\n");
- for(int i=0; i<3; i++) {
- //concatenating gyro LSB and MSB to get 16 bit signed data values
- bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
- gyro_data[i]=(float)bit_data;
- gyro_data[i]=gyro_data[i]/senstivity_gyro;
- gyro_data[i]+=gyro_error[i];
- // pc_acs.printf("%f\t",gyro_data[i]);
- }
- // pc_acs.printf("\nMag Values:\n");
- for(int i=0; i<3; i++) {
- //concatenating mag LSB and MSB to get 16 bit signed data values
- bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
- mag_data[i]=(float)bit_data;
- mag_data[i]=mag_data[i]/senstivity_mag;
- mag_data[i]+=mag_error[i];
- //pc_acs.printf("%f\t",mag_data[i]);
- }
- for(int i=0; i<3; i++) {
- // data[i]=gyro_data[i];
- actual_data.AngularSpeed_actual[i] = gyro_data[i];
- actual_data.Bvalue_actual[i] = mag_data[i];
- //data[i+3]=mag_data[i];
- }
-
-
-
- //(event & 40 != 40 ) || (event & 08 != 08 ) || (event & 01 == 01 )|| (event & 02 == 02 )
+ if((ack1!=0)||(ack2!=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;
+ }
+ }
- if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) //check for any error
+ sentral = (int) status;
+
+ pc_acs.printf("Event Status is %x\n \r",event);
+ pc_acs.printf("Sentral Status is %x\n \r",sentral);
+
+
+
+ if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )|| (sentral!= 3)) //check for any error in event status register
{
- cmd[0]=RESETREQ;
- cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(600);
+ init = SENSOR_INIT();
+
+ int ack1,ack2;
cmd[0]=EVT_STATUS;
i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(100);
- pc_acs.printf("Event Status after resetting is %x\n \r",(int)status);
+ ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
+ //wait_ms(100);
+ event = (int)status;
- if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ))
+ 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))
{
-
-
-
- // cmd[0]=SENTRALSTATUS;
- // i2c.write(SLAVE_ADDR,cmd,1);
- /// i2c.read(SLAVE_ADDR_READ,®,1);
- // wait_ms(100);
- // sentral = (int)reg;
-
- cmd[0]=SENSORSTATUS;
+ cmd[0]=EVT_STATUS;
i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,®,1);
+ ack1 = i2c.read(SLAVE_ADDR_READ,&status,1);
+ event = (int)status;
wait_ms(100);
-
- sensor = (int)reg;
-
- cmd[0]=ERROR;
+ cmd[0]=SENTRALSTATUS;
i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,®,1);
+ ack2 = i2c.read(SLAVE_ADDR_READ,&status,1);
+ sentral = (int)status;
wait_ms(100);
- error = (int)reg;
+ 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;
+ }
+ }
+
+ 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( error&128 == 128)
+ if((ack1!=0)||(ack2!=0))
{
- cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
- cmd[1]=BIT_MAGODR;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(100);
- cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
- cmd[1]=BIT_GYROODR;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(100);
- cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
- cmd[1]=0x00;
- wait_ms(100);
- cmd[0]=ERROR;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,®,1);
- wait_ms(100);
+ 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;
+ }
- error = (int)reg;
-
- if( error&128 == 128)
- {
- pc_acs.printf("Rate error.Exiting.\n \r");
- return 1;
- }
-
-
+ return 1;
+ }
}
- if((error&16 == 16) || (error&32 == 32) || (sensor!=0))
+
+ 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");
- return 2;
- }
+
+ 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");
- return 1;
+ 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(((int)status & 1 == 1 ))
+ if((event & 1 == 1 ))
{
- pc_acs.printf("error in CPU Reset.calling init.\n \r");
- init = SENSOR_INIT();
- if(init == 0)
+ 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;
-
- cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(100);
- if(((int)status & 1 == 1 ))
- {
- pc_acs.printf("Still error in CPU Reset.Exiting.\n \r");
- return 1;
- }
- pc_acs.printf("error in CPU Reset cleared.\n \r");
}
- if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
+ if((event & 8 != 8 )||(event & 32 != 32 ))
{
pc_acs.printf("Data not ready waiting...\n \r");
//POLL
- wait_ms(1500);
+ wait_ms(1000);
cmd[0]=EVT_STATUS;
i2c.write(SLAVE_ADDR,cmd,1);
i2c.read(SLAVE_ADDR_READ,&status,1);
wait_ms(100);
- if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
+ if((event & 8 != 8 )||(event & 32 != 32 ))
{
- if((int)status & 32 != 32 )
+ if(event & 32 != 32 )
{
- if((int)status & 8 != 8 )
+ if(event & 8 != 8 )
{
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;
}
+
pc_acs.printf("Mag data only ready.Read..\n \r");
mag_only = 1;
//return 2;
@@ -731,55 +810,79 @@
}
- }
+ }
}
+ 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;
+ }
+
+ }
+
}
- cmd[0]=EVT_STATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(100);
- pc_acs.printf("Event Status is %x\n \r",(int)status);
-
- //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
-
- cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
+
+
cmd[0]=MAG_XOUT_H; //LSB of x
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,raw_mag,6);
+ i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
+ ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24);
+
+ if(ack1 != 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);
+ 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;
+ }
+
+ }
+
+
// pc_acs.printf("\nGyro Values:\n");
for(int i=0; i<3; i++) {
//concatenating gyro LSB and MSB to get 16 bit signed data values
- bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
+ 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];
- // pc_acs.printf("%f\t",gyro_data[i]);
}
- // pc_acs.printf("\nMag Values:\n");
+
for(int i=0; i<3; i++) {
- //concatenating mag LSB and MSB to get 16 bit signed data values
- bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
+ //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];
- //pc_acs.printf("%f\t",mag_data[i]);
}
for(int i=0; i<3; i++) {
// data[i]=gyro_data[i];
actual_data.AngularSpeed_actual[i] = gyro_data[i];
actual_data.Bvalue_actual[i] = mag_data[i];
- //data[i+3]=mag_data[i];
}
+
+
if(mag_only == 0)
{
@@ -789,6 +892,13 @@
}
else if(mag_only == 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;
}
@@ -961,12 +1071,7 @@
}
- pc_acs.printf(" Reading value from sensor 1 before exiting\n \r");
- ATS1_SW_ENABLE = 0;
- wait_ms(5);
- SENSOR_DATA_ACQ();
- ATS1_SW_ENABLE = 1;
- wait_ms(5);
+
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
@@ -989,10 +1094,7 @@
float l_current_z=0; //Current sent in z TR's
- for(int i = 0 ; i<3;i++)
- {
- // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
- }
+ printf("\r\r");
//----------------------------- x-direction TR --------------------------------------------//
@@ -1093,20 +1195,21 @@
//----------------------------------------------- z-direction TR -------------------------//
-
+
+
float l_moment_z = Moment[2]; //Moment in z direction
- phase_TR_z = 1; // setting the default current direction
+ phase_TR_z = 1; // setting the default current direction
if (l_moment_z <0)
{
- phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
+ phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
l_moment_z = abs(l_moment_z);
}
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);
- if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
+ 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);
@@ -1126,7 +1229,7 @@
printf("DC for trz is %f \r \n",l_duty_cycle_z);
PWM3.period(TIME_PERIOD);
PWM3 = l_duty_cycle_z/100 ;
- }
+ }
else if(l_current_z==0)
{
printf("\n \r l_current_z====0");
@@ -1137,8 +1240,8 @@
}
else // not necessary
{
- g_err_flag_TR_z = 1;
- }
+ g_err_flag_TR_z = 1;
+ }
//-----------------------------------------exiting the function-----------------------------------//
@@ -1147,195 +1250,6 @@
}
-/*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
-{
- printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
-
- float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
- float l_current_x=0; //Current sent in x TR's
- float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
- float l_current_y=0; //Current sent in y TR's
- float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
- float l_current_z=0; //Current sent in z TR's
-
-
- for(int i = 0 ; i<3;i++)
- {
- printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
- }
-
- //----------------------------- x-direction TR --------------------------------------------//
-
-
- float l_moment_x = Moment[0]; //Moment in x direction
-
- phase_TR_x = 1; // setting the default current direction
- if (l_moment_x <0)
- {
- phase_TR_x = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
- l_moment_x = abs(l_moment_x);
- }
-
- l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
- pc_acs.printf("current in trx is %f \r \n",l_current_x);
- if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
- {
- l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
- pc_acs.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.006 && l_current_x < 0.0116)
- {
- l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
- pc_acs.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.0116 && l_current_x < 0.0624)
- {
- l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
- pc_acs.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.0624 && l_current_x < 0.555)
- {
- l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
- pc_acs.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)
- {
- printf("\n \r l_current_x====0");
- l_duty_cycle_x = 0; // default value of duty cycle
- pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
- PWM1.period(TIME_PERIOD);
- PWM1 = l_duty_cycle_x/100 ;
- }
- else //not necessary
- {
- g_err_flag_TR_x = 1;
- }
-
- //------------------------------------- y-direction TR--------------------------------------//
-
-
- float l_moment_y = Moment[1]; //Moment in y direction
-
- phase_TR_y = 1; // setting the default current direction
- if (l_moment_y <0)
- {
- phase_TR_y = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
- l_moment_y = abs(l_moment_y);
- }
-
-
- l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
- pc_acs.printf("current in try is %f \r \n",l_current_y);
- if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
- {
- l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
- pc_acs.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.006 && l_current_y < 0.0116)
- {
- l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
- pc_acs.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.0116&& l_current_y < 0.0624)
- {
- l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
- pc_acs.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.0624 && l_current_y < 0.555)
- {
- l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
- pc_acs.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)
- {
- printf("\n \r l_current_y====0");
- l_duty_cycle_y = 0; // default value of duty cycle
- pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
- PWM2.period(TIME_PERIOD);
- PWM2 = l_duty_cycle_y/100 ;
- }
- else // not necessary
- {
- g_err_flag_TR_y = 1;
- }
-
- //----------------------------------------------- z-direction TR -------------------------//
-
-
- float l_moment_z = Moment[2]; //Moment in z direction
-
- phase_TR_z = 1; // setting the default current direction
- if (l_moment_z <0)
- {
- phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
- l_moment_z = abs(l_moment_z);
- }
-
-
- l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
- pc_acs.printf("current in trz is %f \r \n",l_current_z);
- if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
- {
- l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
- PWM3.period(TIME_PERIOD);
- PWM3 = l_duty_cycle_z/100 ;
- }
- else if( l_current_z >= 0.006 && l_current_z < 0.0116)
- {
- l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
- PWM3.period(TIME_PERIOD);
- PWM3 = l_duty_cycle_z/100 ;
- }
- else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
- {
- l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
- PWM3.period(TIME_PERIOD);
- PWM3 = l_duty_cycle_z/100 ;
- }
- else if(l_current_z >= 0.0624 && l_current_z < 0.555)
- {
- l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
- PWM3.period(TIME_PERIOD);
- PWM3 = l_duty_cycle_z/100 ;
- }
- else if(l_current_z==0)
- {
- printf("\n \r l_current_z====0");
- l_duty_cycle_z = 0; // default value of duty cycle
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
- PWM3.period(TIME_PERIOD);
- PWM3 = l_duty_cycle_z/100 ;
- }
- else // not necessary
- {
- g_err_flag_TR_z = 1;
- }
-
- //-----------------------------------------exiting the function-----------------------------------//
-
- printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
-
-}*/
\ No newline at end of file
--- a/ACS.h Fri Jun 03 13:53:55 2016 +0000 +++ b/ACS.h Thu Jun 09 14:12:55 2016 +0000 @@ -5,9 +5,18 @@ //........................................... #define TIME_PERIOD 0.02 #define TR_CONSTANT 0.3 +#define sampling_time 10 +#define kdetumble 2000000 +#define MmntMax 1.1 // Unit: Ampere*Meter^2 +#define OmegaMax 1*3.1415/180.0 // Unit: Radians/Second + +#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 void FCTN_ACS_GENPWM_MAIN(float*); void FCTN_ACS_CNTRLALGO(float*,float*); +void controlmodes(float*, float*, float*, uint8_t); void inverse(float mat[3][3],float inv[3][3]); extern void FLAG();
--- a/main.cpp Fri Jun 03 13:53:55 2016 +0000
+++ b/main.cpp Thu Jun 09 14:12:55 2016 +0000
@@ -17,38 +17,25 @@
//i2c//
char data_send_flag = 'h';
-//.........acs...............//
-/* char ACS_INIT_STATUS = 'q';
-char ACS_DATA_ACQ_STATUS = 'q';
-char ACS_ATS_STATUS = 'q';
-char ACS_MAIN_STATUS = 'q';
-char ACS_STATUS = 'q';
+//.........ACS...............//
-char ACS_ATS_ENABLE = 'q';
-char ACS_DATA_ACQ_ENABLE = 'q';
-char ACS_STATE = 'q';*/
uint8_t ACS_INIT_STATUS = 0;
uint8_t ACS_DATA_ACQ_STATUS = 0;
uint8_t ACS_ATS_STATUS = 0x60;
uint8_t ACS_MAIN_STATUS = 0;
uint8_t ACS_STATUS = 0;
+uint8_t ACS_DETUMBLING_ALGO_TYPE = 0;
+
+uint8_t ACS_TR_Z_SW_STATUS=1;
+uint8_t ACS_TR_XY_SW_STATUS=1;
uint8_t ACS_ATS_ENABLE = 1;
uint8_t ACS_DATA_ACQ_ENABLE = 1;
-uint8_t ACS_STATE = 4;
+uint8_t ACS_STATE = 7;
-//.....................eps...................//
+//.....................EPS...................//
//eps init
-/*char EPS_INIT_STATUS = 'q';
-char EPS_BATTERY_GAUGE_STATUS = 'q';
-//eps main
-char EPS_MAIN_STATUS = 'q';
-char EPS_BATTERY_TEMP_STATUS = 'q';
-char EPS_STATUS = 'q';
-
-char EPS_BATTERY_HEAT_ENABLE = 'q';
-*/
uint8_t EPS_INIT_STATUS = 0;
uint8_t EPS_BATTERY_GAUGE_STATUS = 0;
@@ -71,11 +58,20 @@
Timer t_start;
Timer t_tc;
Timer t_tm;
+
Serial pc(USBTX, USBRX);
int power_flag_dummy=2;
-float data[6];
+
+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;
+extern uint8_t alarmmode;
+
+
extern uint8_t BCN_FEN;
extern BAE_HK_actual actual_data;
extern BAE_HK_quant quant_data;
@@ -122,13 +118,6 @@
InterruptIn ir7(PIN42);//Charger IC - Fault Bar
-//DigitalOut TRXY_SW_EN(PIN71); //TR XY Switch
-//DigitalOut DRV_Z_SLP(PIN88); //Sleep pin of driver z
-//DigitalOut TRZ_SW(PIN40); //TR Z Switch
-//DigitalOut CDMS_RESET(PIN7); // CDMS RESET
-//DigitalOut BCN_SW(PIN14); //Beacon switch
-//DigitalOut DRV_XY_SLP(PIN82);
-
DigitalOut TRXY_SW(PIN71); //TR XY Switch
DigitalOut DRV_Z_EN(PIN88); //Sleep pin of driver z
@@ -155,202 +144,325 @@
uint8_t iterI1;
uint8_t iterI2;
-
+extern float max_array(float arr[3]);
+
void F_ACS()
{
+ pc.printf("Entered ACS.\n\r");
- //...................//
+ ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
- if(pf1check == 1)
- {
- if(iterP1 >= 3)
- {
- ATS1_SW_ENABLE = 1; // turn off ats1 permanently
- //FCTN_SWITCH_ATS(0); // switch on ATS2
- }
- else
- {
- ATS1_SW_ENABLE = 0;
- iterP1++;
- }
- pf1check = 0;
- }
- if(pf2check == 1)
- {
- if(iterP1 >= 3)
- {
- ATS2_SW_ENABLE = 1; // turn off ats2 permanently
- ACS_DATA_ACQ_ENABLE = 0;
- ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- }
- else
- {
- ATS2_SW_ENABLE = 0;
- iterP2++;
- }
- pf2check = 0;
- }
- if(if1check == 1)
- {
- if(iterI1 >= 3)
- {
- TRXY_SW = 0; // turn off TRXY permanently
- }
- else
- {
- TRXY_SW = 1; //switch on TRXY
- iterI1++;
- }
- }
- if(if2check == 1)
- {
- if(iterI2 >= 3)
- {
- TRZ_SW = 0; // turn off TRZ permanently
- ACS_STATE = 2 ; // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- }
- else
- {
- TRZ_SW = 1; //switch on Z
- iterI2++;
- }
- }
-
- //float b1[3]={-23.376,-37.56,14.739}, omega1[3]={-1.52,2.746,0.7629}, moment1[3]= {1.0498,-1.0535,1.3246};
- //b1[3] = {22, 22,10};
- //omega1[3] = {2.1,3.0,1.5};
- // ATS1_SW_ENABLE = 0; // att sens2 switch is disabled
- // ATS2_SW_ENABLE = 0; // att sens switch is disabled
-
-
-
- //Thread::signal_wait(0x1);
- ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag
PWM1 = 0; //clear pwm pins
PWM2 = 0; //clear pwm pins
PWM3 = 0; //clear pwm pins
- pc.printf("\n\rEntered ACS %f\n",t_start.read());
+
+
+ ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ();
- if(ACS_DATA_ACQ_ENABLE == 1)// check if ACS_DATA_ACQ_ENABLE = 1?
- {
- //FLAG();
- FCTN_ATS_DATA_ACQ(); //the angular velocity is stored in the first 3 values and magnetic field values in next 3
- pc.printf("gyro values\n\r"); //printing the angular velocity and magnetic field values
+ //printing the angular speed and magnetic field values
+
+ pc.printf("gyro values\n\r");
for(int i=0; i<3; i++)
{
printf("%f\n\r",actual_data.AngularSpeed_actual[i]);
}
+
pc.printf("mag values\n\r");
for(int i=0; i<3; i++)
{
pc.printf("%f\n\r",actual_data.Bvalue_actual[i]);
}
- // for(int i=0;i<3;i++)
-// {
-// omega1[i]= data[i];
-// b1[i] = data[i+3];
-// }
- }//if ACS_DATA_ACQ_ENABLE = 1
- else
+
+ for(int i=0;i<3;i++)
{
- // Z axis actuation is the only final solution,
+ mag_data[i] = actual_data.Bvalue_actual[i]/1000000;
+ gyro_data[i] = actual_data.AngularSpeed_actual[i]*3.14159/180;
}
- if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
+
+
+
+ if(ACS_STATE == 0) // check ACS_STATE = ACS_CONTROL_OFF?
{
printf("\n\r acs control off\n");
- FLAG();
ACS_STATUS = 0; // set ACS_STATUS = ACS_CONTROL_OFF
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(actual_data.power_mode<2)
+ {
+ printf("\n\r Low Power \n\r");
+
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
}
- else
+
+ else if(ACS_TR_Z_SW_STATUS != 1)
+ {
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 2; // set ACS_STAUS = ACS_TRZ_DISABLED
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(ACS_TR_XY_SW_STATUS != 1)
+ {
+
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED , Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // is a dummy value
+
+
+
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_DATA_ACQ_STATUS == 1)
{
- if(actual_data.power_mode>1)
-
- {
- if(ACS_STATE == 2) // check ACS_STATE = ACS_ZAXIS_MOMENT_ONLY
- {
- FLAG();
- printf("\n\r z axis moment only\n");
- ACS_STATUS = 2; // set ACS_STATUS = ACS_ZAXIS_MOMENT_ONLY
- // FCTN_ACS_CNTRLALGO(b1, omega1);
- moment[0] = 0;
- moment[1] = 0;
- moment[2] =1.3;// is a dummy value
- FCTN_ACS_GENPWM_MAIN(moment) ;
- }
- else
- {
- if(ACS_STATE == 3) // check ACS_STATE = ACS_DATA_ACQ_FAILURE
- {
- FLAG();
- printf("\n\r acs data failure ");
- ACS_STATUS = 3; // set ACS_STATUS = ACS_DATA_ACQ_FAILURE
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }
- else
- {
- if(ACS_STATE == 4) // check ACS_STATE = ACS_NOMINAL_ONLY
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set Set ACS_STATUS = ACS_DATA_ACQN_FAILURE , Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // is a dummy value
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 5)
+ {
+
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 0;
+
+ ACS_STATUS = 3; // set ACS_STAUS = ACS_TRXY_DISABLED by ACS_STATE i.e Z axis only
+
+ moment[0] = 0;
+ moment[1] = 0;
+ moment[2] =1.3; // 1.3 is a dummy value
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_DATA_ACQ_STATUS == 2) // MM only is available
+ {
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
+
+ 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++)
{
- FLAG();
- printf("\n\r nominal");
- ACS_STATUS = 4; // set ACS_STATUS = ACS_NOMINAL_ONLY
- FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual);
- printf("\n\r moment values returned by control algo \n");
- for(int i=0; i<3; i++)
- {
- printf("%f\t",moment[i]);
- }
- FCTN_ACS_GENPWM_MAIN(moment) ;
+ db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
- else
+ }
+
+
+
+ for(int i=0;i<3;i++)
+ {
+ moment[i]=-kdetumble*db[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_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 7) // Nominal mode
+ {
+
+ printf("\n\r Nominal mode \n");
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
+
+ alarmmode = 0;
+ float normalising_fact;
+
+ 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++)
{
- if(ACS_STATE == 5) // check ACS_STATE = ACS_AUTO_CONTROL
- {
- FLAG();
- printf("\n\r auto control");
- ACS_STATUS = 5; // set ACS_STATUS = ACS_AUTO_CONTROL
- //FCTN_ACS_AUTOCTRL_LOGIC // gotta include this code
- }
- else
- {
- if(ACS_STATE == 6) // check ACS_STATE = ACS_DETUMBLING_ONLY
- {
- FLAG();
- printf("\n\r Entered detumbling \n");
- ACS_STATUS = 6; // set ACS_STATUS = ACS_DETUMBLING_ONLY
- FCTN_ACS_CNTRLALGO(actual_data.Bvalue_actual,actual_data.AngularSpeed_actual); // detumbling code has to be included
- FCTN_ACS_GENPWM_MAIN(moment) ;
- }
- else
- {
- FLAG();
- printf("\n\r invalid state");
- ACS_STATUS = 7 ; // set ACS_STATUS = INVALID STATE
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }//else of invalid
- }//else of autocontrol
- }//else of nominal
- }//else of data acg failure
+ db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
+ }
+ }
+
+ controlmodes(mag_data,db,gyro_data, 0);
+
+ if(max_array(moment)>MmntMax)
+ {
+ normalising_fact=max_array(moment)/MmntMax;
+ for(int i=0;i<3;i++)
+ {
+ moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+ }
+ }
+
+ 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_GENPWM_MAIN(moment) ;
+
+ ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY
+
+ ACS_MAIN_STATUS = 0;
+ return;
+
+ }
+
+ else if(ACS_STATE == 8) // Auto Control
+ {
+
+ printf("\n\r Auto control mode \n");
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
+
+
+ alarmmode=0;
+ FCTN_ACS_CNTRLALGO(mag_data,gyro_data);
+ printf("\n\r Moment values returned by control algo \n");
+ for(int i=0; i<3; i++)
+ {
+ printf("%f\t",moment[i]);
+ }
+ FCTN_ACS_GENPWM_MAIN(moment) ;
+ ACS_STATUS = 6; // set ACS_STATUS = ACS_AUTO_CONTROL
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ else if(ACS_STATE == 9) // Detumbling
+ {
+ DRV_Z_EN = 1;
+ DRV_XY_EN = 1;
- }//else fo z axis moment only
- }//if power >2
- else
- {
- FLAG();
- printf("\n\r low power");
- ACS_STATUS = 1; // set ACS_STATUS = ACS_LOW_POWER
- PWM1 = 0; //clear pwm pins
- PWM2 = 0; //clear pwm pins
- PWM3 = 0; //clear pwm pins
- }
- } //else for acs control off
+ 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_GENPWM_MAIN(moment) ;
+
+ ACS_MAIN_STATUS = 0;
+ return;
+ }
+
+ ACS_STATUS = 7; //INVALID_STATE
+ DRV_Z_EN = 0;
+ DRV_XY_EN = 0;
ACS_MAIN_STATUS = 0; //clear ACS_MAIN_STATUS flag
}
@@ -630,10 +742,22 @@
schedcount = 1;
}
if(schedcount%1==0)
- { pc.printf("\nSTATE IS !!!!!! = %x !!\n",ACS_STATE);
- pc.printf("\niterp1 !!!!!! = %x !!\n",iterP1);
- pc.printf("\niteri2 IS !!!!!! = %x !!\n",iterI2);
- F_ACS();
+ {
+
+
+ pc.printf("\n\r\r\r\r \t\t******ACS******\r\r\r\r\r");
+
+ pc.printf("ACSSTATE IS !!!!!! = %x !!\n\r",ACS_STATE);
+
+ float acs_start = (float) t_start.read();
+
+ F_ACS();
+
+ float acs_end = float( t_start.read() - acs_start ) ;
+ printf("\nTime taken for ACS is:\t %f\n\r",acs_end);
+
+ pc.printf("\n\r\r\r\r \t\t******ACS EXIT******\r\r\r\r\r");
+
}
if(schedcount%2==0)
@@ -760,11 +884,10 @@
{
printf("\n\r Initialising BAE ");
//..........intial status....//
- ACS_STATE = 4;
+ ACS_STATE = 8;
ACS_ATS_ENABLE = 1;
ACS_DATA_ACQ_ENABLE = 1;
-
EPS_BATTERY_HEAT_ENABLE = 1;
actual_data.power_mode=3;
//............intializing pins................//