Team Fox
ACS completed fully. All cases to be tested
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
ACS_Flowchart_BAE
by 
Team Fox
Diff: ACS.cpp
- Revision:
- 17:1e1955f3db75
- Parent:
- 16:cc77770d787f
- Child:
- 18:21740620c65e
--- 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