Team Fox
/
BAE_QM_MAR9
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
workinQM_5thJan_azad
by 
Team Fox
Diff: ACS.cpp
- Revision:
- 20:949d13045431
- Parent:
- 19:79e69017c855
- Child:
- 34:1b41c34b12ea
- Child:
- 39:670133e7ffd8
--- a/ACS.cpp Sat Jun 04 11:29:13 2016 +0000
+++ b/ACS.cpp Fri Jul 01 17:55:30 2016 +0000
@@ -7,10 +7,9 @@
#include "pin_config.h"
#include "ACS.h"
#include "EPS.h"
-
-/*timer for determining the execution time*/
-//Timer time;
-
+/*variables will get get updated value from FLash
+in case flash cups while testing i.e initial defaul values are kept as of now
+*/
//********************************flags******************************************//
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
@@ -19,6 +18,10 @@
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
extern uint8_t ACS_ATS_ENABLE;
extern uint8_t ACS_DATA_ACQ_ENABLE;
@@ -39,56 +42,101 @@
extern float data[6];
extern BAE_HK_actual actual_data;
+//global para
+//FUNCTION
+float max_invjm [9]= {1.0000,1.0000,1.0000,0.0471,4.6159,4.1582,4.4047,0.0755,4.1582};
+float min_invjm[9] = {-1.0000,-1.0000,-1.0000,-0.0471,-4.6159,-4.1582,-4.4047,-0.0755,-4.1582};
+float max_jm[9] = {0.3755,0.0176,0.2672,0.4895,0.2174,0.0452,1.0000,0.1209,0.0572};
+float min_jm[9] = {-0.2491,-0.0457,0.2271,0.1556,0.2222,0.0175,0.9998,0.0361,0.0922};
+//se some other better way
+/*
+float max_bb[3] = {0,1.0*e-04*0.1633,1.0*e-04*0.1528};
+float min_bb[3] = {0,1.0*e-04*(-0.1736),1.0*e-04*(-0.1419)};
+*/
+float max_bb[3] = {0,1.0*0.0001*0.1633,1.0*0.0001*0.1528};
+float min_bb[3] = {0,1.0*0.0001*(-0.1736),1.0*0.0001*(-0.1419)};
-//DigitalOut gpo1(PTC0); // enable of att sens2 switch
-//DigitalOut gpo2(PTC16); // enable of att sens switch
+//ACS
+uint8_t controlmode_mms = 0;
+uint8_t ATS1_EVENT_STATUS_RGTR=0x00;
+uint8_t ATS1_SENTRAL_STATUS_RGTR=0x00;
+uint8_t ATS1_ERROR_RGTR=0x00;
+uint8_t ATS2_EVENT_STATUS_RGTR=0x00;
+uint8_t ATS2_SENTRAL_STATUS_RGTR=0x00;
+uint8_t ATS2_ERROR_RGTR=0x00;
+uint8_t invjm_mms[9];
+uint8_t jm_mms[9];
+uint8_t bb_mms[3];
+uint8_t singularity_flag=0;
+
+uint8_t ACS_MAG_TIME_DELAY;// = 65;
+uint8_t ACS_DEMAG_TIME_DELAY;// = 65;
+uint16_t ACS_Z_FIXED_MOMENT;// = 1.3;
+uint8_t ACS_TR_Z_SW_STATUS;//=1;
+uint8_t ACS_TR_XY_SW_STATUS;//=1;
+//GLOBAL PARA
+uint8_t ACS_TR_X_PWM; //*
+uint8_t ACS_TR_Y_PWM; //*
+uint8_t ACS_TR_Z_PWM; //*
+//change
+uint16_t ACS_MM_X_COMSN = 1;
+uint16_t ACS_MM_Y_COMSN = 1;
+uint16_t ACS_MG_X_COMSN = 1;
+uint16_t ACS_MG_Y_COMSN = 1;
+uint16_t ACS_MM_Z_COMSN = 1;
+uint16_t ACS_MG_Z_COMSN = 1;
+
+uint8_t float_to_uint8(float min,float max,float val)
+{
+ if(val>max)
+ {return 0xff;
+ }
+ if(val<min)
+ {return 0x00;
+ }
+ float div=max-min;div=(255.0/div);val=((val-min)*div);
+ return (uint8_t)val;
+}
+
+
+void float_to_uint8_ARRAY(int d1,int d2, float *arr,float max[], float min[], uint8_t *valarr)
+{
+ for(int i=0;i<d1;i++)
+ for(int j=0;j<d2;j++)
+ {
+ printf("\n\r%f",*((arr+(i*d1))+j));
+ valarr[i*d1+j] = (uint8_t)float_to_uint8(min[i*d1+j],max[i*d1+j],*((arr+(i*d1))+j));
+ printf("\n\r%d",valarr[i*d1+j]);
+ }
+}
+
Serial pc_acs(USBTX,USBRX); //for usb communication
+
//CONTROL_ALGO
-
-float moment[3]; // Unit: Ampere*Meter^2
+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);
-float max_array(float arr[3]);
+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 moment[3],float b[3] ,float omega[3],uint8_t nominal,uint8_t detumbling,uint8_t ACS_DETUMBLING_ALGO_TYPE)
{
- 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;
}
@@ -96,83 +144,90 @@
{
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)
- {
- alarmmode=0;
- }
- else if(max_array(omega1)>OmegaMax && alarmmode==0)
- {
- alarmmode=1;
- }
-
+ if(nominal == 0)
+
+ {
+
+ if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
+ {
+ 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)
+ if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
{
- controlmode=0;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
- for (i=0;i<3;i++)
- {
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
- }
+ controlmode_mms = 0;
+ 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)
{
- controlmode=1;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
- for (i=0;i<3;i++)
- {
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
- }
+ controlmode_mms = 1;
+ 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;//*is this changed now
}
else
{
- controlmode=1;
- controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
- for (i=0;i<3;i++)
- {
- b1[i]=b1_copy[i];
- db1[i]=db1_copy[i];
- omega1[i]=omega1_copy[i];
- }
+ controlmode_mms = 1;
+ 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
- }
+ }
}
}
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],uint8_t &singularity_flag)
{
int i,j;
float det=0;
@@ -184,11 +239,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;
+ }
}
}
}
@@ -207,8 +270,9 @@
return temp_max;
}
+uint8_t singularity_flag_mms=0;
-void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
+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};
@@ -218,95 +282,119 @@
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;
+ //uint8_t 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_mms=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_mms==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_mms);
+ 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_mms);
+
+ //00000
+ float_to_uint8_ARRAY(3,3, (float*)invJm,max_invjm, min_invjm, invjm_mms);
+ float_to_uint8_ARRAY(3,3, (float*)Jm,max_jm, min_jm, jm_mms);
+ float_to_uint8_ARRAY(1,3, (float*)bb,max_bb, min_bb, bb_mms);
+
+ if (singularity_flag_mms==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_mms==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;
- }
-
+ ACS_STATUS = 5;
}
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
+ }
+ ACS_STATUS = 6;
+ }
+ else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
+ {
+ for(i=0;i<3;i++)
+ {
+ Mmnt[i]=-kdetumble*db[i];
+ }
+ ACS_STATUS = 4;
}
}
for(i=0;i<3;i++)
@@ -317,50 +405,108 @@
I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
-void FCTN_ACS_INIT(void); //initialization of registers happens
-void FCTN_ATS_DATA_ACQ(); //data is obtained
-void T_OUT(); //timeout function to stop infinite loop
-Timeout to; //Timeout variable to
+int FCTN_ACS_INIT(); //initialization of registers happens
+int SENSOR_INIT();
+int FCTN_ATS_DATA_ACQ(); //data is obtained
+int SENSOR_DATA_ACQ();
+//void T_OUT(); //timeout function to stop infinite loop
+
+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
-}
+//void T_OUT()
+//{
+// toFlag=0; //as T_OUT function gets called the while loop gets terminated
+//}
//DEFINING VARIABLES
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
-float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
+//int16_t bit_data done in actual_data structure itself;
-void FCTN_ACS_INIT()
-{
- ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag
- //FLAG();
- pc_acs.printf("Attitude sensor init called \n \r");
- //FLAG();
- /* as of now no reset
- cmd[0]=RESETREQ;
+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()
+{
+ cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
- wait_ms(2000); //waiting for loading configuration file stored in EEPROM
- // */
- //wait_ms(3000);
+ wait_ms(575);
+
+ //Verify magic number
+
+ cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
+ cmd[1]=BIT_HOST_UPLD_ENB;
+ i2c.write(SLAVE_ADDR,cmd,2);
+ wait_ms(20);
+
+ cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
+ cmd[1]=0x0000;
+ i2c.write(SLAVE_ADDR,cmd,3);
+ wait_ms(20);
+
+ cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
+ cmd[1]=0x00;
+ i2c.write(SLAVE_ADDR,cmd,2);
+ wait_ms(20);
+
+ return 0;
+}
+
+int SENSOR_INIT()
+{
+/// pc_acs.printf("Entered sensor init\n \r");
+ cmd[0]=RESETREQ;
+ cmd[1]=BIT_RESREQ;
+ ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
+ //wait_ms(575); //waiting for loading configuration file stored in EEPROM
+
+/// 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(575);
+
cmd[0]=SENTRALSTATUS;
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(20); // initially 100
- //to check whether EEPROM is uploaded
+ 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);
+
+ //to check whether EEPROM is uploaded properly
switch((int)store) {
- case(3): { //actually this state correct
+ case(3): {
break;
}
case(11): {
@@ -369,208 +515,899 @@
default: {
cmd[0]=RESETREQ;
cmd[1]=BIT_RESREQ;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(2000);//see if it can be changed
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
+ wait_ms(575);//should be 600
+
+ cmd[0]=SENTRALSTATUS;
+ 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);
+
}
}
- pc_acs.printf("\n\n\rwait is 1 \n\r");
- pc_acs.printf("Sentral Status is %x\n \r",(int)store);
-
- /*ways to make the sensors work even if one sensor cups first making data rate 0x00
- 1>make data rate 0x00 change the enable events register value to 0x0A or 0x22 as suitable
- 2>change the condition for getting the values from the sensors i.e in data_acq function
- 3>check the other register values
-
- other method is pass through working in how it works / decoding SENtral algorithms
- */
-
- 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);
- //___________________________________________________________________________________________________
-
- /*just leave it then see what happens?? the gyro data without inputing anything*/
+
+ int manual=0;
+ if( ((int)store != 11 )&&((int)store != 3))
+ {
+
+ cmd[0]=RESETREQ;
+ cmd[1]=BIT_RESREQ;
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
+ wait_ms(575);
+
+ manual = CONFIG_UPLOAD();
+
+ if(manual == 0)
+ {
+ //MANUAL CONFIGURATION FAILED
+ return 0;
+ }
+
+ }
+ cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
+ cmd[1]=BIT_RUN_ENB;
+ 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;
+ 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;
+ 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;
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if( ack!=0)
+ {
+ ack = i2c.write(SLAVE_ADDR,cmd,2);
+ if(ack!=0)
+ return 0;
+ }
+ //wait_ms(20);
+ cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
+ cmd[1]=0x00;
+ 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;
+ 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;
+ 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
+ {
+/// pc_acs.printf("Exited sensor init successfully\n \r");
+ return 1;
+ }
+
+
+//// pc_acs.printf("Sensor init failed \n \r") ;
+ return 0;
+}
+
+int FCTN_ACS_INIT()
+{
+ ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
+
- //___________________________________________________________________________________________________
- cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
- cmd[1]=BIT_GYROODR;
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(1);
- cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
- cmd[1]=0x00;//actually 0x00
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(1);
- 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(1);
- //_______________________________________________________________________________//
-
-/*start as of now this approach i.e pass through state*/
-/*
- cmd[0]=0x35;//event status
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rEvent Status at start is %x\n \r",(int)status);
-
- cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
- cmd[1]=0x01;//Places sentral in standby state
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(1);
+ 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);
- cmd[0]=0xA0; //Pass through control register
- cmd[1]=0x01;//places SENtral in pass through state
- i2c.write(SLAVE_ADDR,cmd,2);
- wait_ms(1);
+ 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)|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)|0xC0;
+
+ }
+/// pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
- cmd[0]=0x9E;//Pass through status reg
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&store,1);
- wait_ms(1);
- printf("\n\r the value of the pass through status register is = %x",(int)store);
+ if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
+ {
+
+
+ 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)|0x07;
+ ACS_INIT_STATUS = 0;
+ return 2;
+ }
+
+ ATS2_SW_ENABLE = 1;
+ wait_ms(5);
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
+
+
+ }
- cmd[0]=0x35;//event status
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rEvent Status at end initialization is %x\n \r",(int)status);
-*/
-
- //_______________________________________________________________________________//
+/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+/// pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
- ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
+ ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
+ return 0;
}
-void FCTN_ATS_DATA_ACQ()
+
+int SENSOR_DATA_ACQ()
{
- //time.start();
-
- ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
- if( ACS_ATS_ENABLE == 1)
- {
- FLAG();
- pc_acs.printf("attitude sensor execution called \n \r");
- toFlag=1; //toFlag is set to 1 so that it enters while loop
- to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
- while(toFlag) {
+ //int mag_only=0;
+/// pc_acs.printf("Entering Sensor data acq.\n \r");
+ char status;
+ int sentral;
+ int event;
+ int sensor;
+ int error;
+ int init;
+
+ uint8_t gyro_error=0;
+ uint8_t mag_error=0;
+
+ //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);
- //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
- if(((int)status&40)==40){ //when both work
-///// if((int)status==8){ //for just mag
-///// if((int)status == 36){ //for just gyro as status 24(in binary) = 36 in decimal
-///// if((int)status==___yet to be decided___){ //for pass through state see how it tworks
-
- /*don't ask for info if the gyro cupped as interrupt makes it go haywire that is enables the pins for gyro*/
- cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
-
-/*editing the data*/
+ 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;
+ }
- 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]);
- }
-
-/*read the status values to determine the actual condition/registers values for the gyro/mag off case */
-//starts here
+ event = (int)status;
+
+ if(ACS_ATS_STATUS&0xC0 == 0x40)
+ {
+ ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
+ }
+ else if(ACS_ATS_STATUS&0x0C == 0x04)
+ {
+ ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
+ }
+
+ cmd[0]=SENTRALSTATUS;
+ 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;
+ }
+
- cmd[0]=0x35;//event status
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rEvent Status at the end is %x",(int)status);
+ sentral = (int) status;
+
+ if(ACS_ATS_STATUS&0xC0 == 0x40)
+ {
+ ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
+ }
+ else if(ACS_ATS_STATUS&0x0C == 0x04)
+ {
+ ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
+ }
+
+/// pc_acs.printf("Event Status is %x\n \r",event);
+/// pc_acs.printf("Sentral Status is %x\n \r",sentral);
+
- cmd[0]=0x33;//enable events
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rEnable events is %x",(int)status);
-
-
- cmd[0]=0x57;//gyro rate
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rgyro rate is %x",(int)status);
-
-
- cmd[0]=0x55;//mag rate 0x00 indicate value lost
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rmag rate is %x",(int)status);
-
-
- cmd[0]=0x36;//sensorstatus
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rsensor Status is %x",(int)status);
-
+ if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )|| (sentral!= 3)) //check for any error in event status register
+ {
+
+
+ init = SENSOR_INIT();
+
+ 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;
+
+ cmd[0]=SENTRALSTATUS;
+ 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;
+ }
+
+ 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
+ {
+
+ 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)
+ {
+
+ if(ACS_ATS_STATUS&0xC0 == 0x40)
+ {
+ ATS1_ERROR_RGTR = 0x01;
+ }
+ else if(ACS_ATS_STATUS&0x0C == 0x04)
+ {
+ ATS2_ERROR_RGTR = 0x01;
+ }
+ ack = i2c.read(SLAVE_ADDR_READ,&status,1);
+ if(ack!=0)
+ return 0;
+ }
+
+ error = (int)status;
+
+ if(ACS_ATS_STATUS&0xC0 == 0x40)
+ {
+ ATS1_ERROR_RGTR = (uint8_t)error;
+ }
+ else if(ACS_ATS_STATUS&0x0C == 0x04)
+ {
+ ATS2_ERROR_RGTR = (uint8_t)error;
+ }
+
+ 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 0;
+
+ }
+
+ 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 )
+ {
+
+ 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
+ ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
+ if(ack != 0)
+ {
+ cmd[0]=MAG_XOUT_H; //LSB of x
+ i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
+ ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
+ if(ack !=1)
+ return 0;
+
+ }
+
+
+ // pc_acs.printf("\nGyro Values:\n");
+ if (gyro_error!=1)
+ {
+ for(int i=0; i<3; i++) {
+ //concatenating gyro LSB and MSB to get 16 bit signed data values
+ actual_data.bit_data_acs_mg[i]= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
+ gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
+ gyro_data[i]=gyro_data[i]/senstivity_gyro;
+ actual_data.AngularSpeed_actual[i] = gyro_data[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
+ actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
+
+ mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
+ mag_data[i]=mag_data[i]/senstivity_mag;
+ actual_data.Bvalue_actual[i] = mag_data[i];
+ }
+ }
+
+
+ if(mag_error == 1)
+ {
+
+ pc_acs.printf("Gyro only successful.\n \r");
+ return 1;
+ }
+ if(gyro_error == 1)
+ {
+ pc_acs.printf("Mag only successful.\n \r");
+ return 2;
+ }
- cmd[0]=0x50;//error register
- i2c.write(SLAVE_ADDR,cmd,1);
- i2c.read(SLAVE_ADDR_READ,&status,1);
- wait_ms(1);
- pc_acs.printf("\n\rerror register value is %x\n \r",(int)status);
-
-//end here
-
- // 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]);
+ pc_acs.printf("Reading data success.\n \r");
+ return 3;
+}
+
+
+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);
+
+
+ if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
+ {
+
+ acq = SENSOR_DATA_ACQ();
+ if(acq == 3)
+ {
+
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
+
+ //??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 3;
+ }
+ else if((acq == 2)||(acq==1))
+ {
+ 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;
+
+ }
}
- 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];
+
+ 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;
+ }
+
+ }
+
}
- // return(combined_values); //returning poiter combined values
- }
- //checking for the error
- else if (((int)status&2)==2) {
- FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
- }
+
+
}
- }
- else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
- {
- ACS_DATA_ACQ_STATUS = 'f';
- }
- ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+
+ if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
+ {
+ 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
+ {
+ 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");
+ if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(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
+ 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;
+ }
+
+ 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();
+
+ 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.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 //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 == 0)
+ {
+ 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;
+ }
+ }
+ }
+ }
+ 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");
+ return 0;
}
void FCTN_ACS_GENPWM_MAIN(float Moment[3])
{
- printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
+//// 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
@@ -580,10 +1417,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 --------------------------------------------//
@@ -598,40 +1432,41 @@
}
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);
+//// printf("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);
+//// 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);
+//// 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.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);
+//// 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");
+//// 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);
+//// 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;
- }
+ }
+ pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
//------------------------------------- y-direction TR--------------------------------------//
@@ -647,33 +1482,33 @@
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);
+//// 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);
+//// 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);
+//// 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);
+//// 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");
+//// 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);
+//// printf("DC for try is %f \r \n",l_duty_cycle_y);
PWM2.period(TIME_PERIOD);
PWM2 = l_duty_cycle_y/100 ;
}
@@ -681,252 +1516,75 @@
{
g_err_flag_TR_y = 1;
}
-
+ pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
//----------------------------------------------- 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);
+//// 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.0016 && l_current_z < 0.0171)
{
l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
- printf("DC for trz is %f \r \n",l_duty_cycle_z);
+//// 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.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);
+//// 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");
+//// 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);
+//// 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;
- }
+ g_err_flag_TR_z = 1;
+ }
+ pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+//changed
+ if(phase_TR_x)
+ ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
+ else
+ ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
+ if(phase_TR_y)
+ ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
+ else
+ ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
+ if(phase_TR_z)
+ ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
+ else
+ ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM2);
+
//-----------------------------------------exiting the function-----------------------------------//
- printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
+//// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
}
-
-
-/*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
+
\ No newline at end of file