![](/media/cache/group/nine_tails_kurama_by_zubair43-d4zyf5s.jpg.50x50_q85.jpg)
FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of ACS_FULL_Flowchart_BAE by
Revision 19:403cb36e22ed, committed 2016-06-28
- Comitter:
- Bragadeesh153
- Date:
- Tue Jun 28 10:11:54 2016 +0000
- Parent:
- 18:21740620c65e
- Commit message:
- FINAL ACS TO BE USED FOR TESTING
Changed in this revision
--- a/ACS.cpp Mon Jun 13 13:44:31 2016 +0000 +++ b/ACS.cpp Tue Jun 28 10:11:54 2016 +0000 @@ -16,7 +16,6 @@ extern uint8_t ACS_ATS_STATUS; extern uint8_t ACS_MAIN_STATUS; extern uint8_t ACS_STATUS; -extern uint8_t ACS_DETUMBLING_ALGO_TYPE; extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch @@ -44,15 +43,12 @@ Serial pc_acs(USBTX,USBRX); //for usb communication //CONTROL_ALGO -float moment[3]; // Unit: Ampere*Meter^2 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla -float db[3]; uint8_t flag_firsttime=1, alarmmode=0; -void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1); float max_array(float arr[3]); void inverse(float mat[3][3],float inv[3][3]); @@ -61,20 +57,21 @@ -void FCTN_ACS_CNTRLALGO ( float b[3] , float omega[3],int nominal) +void FCTN_ACS_CNTRLALGO (float moment[3],float b[3] ,float omega[3],uint8_t nominal,uint8_t detumbling,uint8_t ACS_DETUMBLING_ALGO_TYPE) { - + float db[3]; float normalising_fact; float b_copy[3], omega_copy[3], db_copy[3]; int i; if(flag_firsttime==1) { for(i=0;i<3;i++) - { + { db[i]=0; // Unit: Tesla/Second - } + } flag_firsttime=0; } + else { for(i=0;i<3;i++) @@ -83,18 +80,18 @@ } } - if(nominal == 0) + if(nominal == 0) { if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1) - { - alarmmode=0; - } + { + alarmmode=0; + } else if(max_array(omega)>OmegaMax&& alarmmode==0) - { - alarmmode=1; - } + { + alarmmode=1; + } } @@ -108,51 +105,52 @@ omega_copy[i]=omega[i]; } - if((alarmmode==0)|| (nominal == 1)) - { - controlmodes(b,db,omega,0); - for (i=0;i<3;i++) + if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0)) { - b[i]=b_copy[i]; - db[i]=db_copy[i]; - omega[i]=omega_copy[i]; - } - if(max_array(moment)>MmntMax) - { - controlmodes(b,db,omega,1); + controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE); for (i=0;i<3;i++) { b[i]=b_copy[i]; db[i]=db_copy[i]; omega[i]=omega_copy[i]; } - if(max_array(moment)>MmntMax) - { - normalising_fact=max_array(moment)/MmntMax; - for(i=0;i<3;i++) - { - moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 - } - } + if(max_array(moment)>MmntMax) + { + controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE); + for (i=0;i<3;i++) + { + b[i]=b_copy[i]; + db[i]=db_copy[i]; + omega[i]=omega_copy[i]; + } + if(max_array(moment)>MmntMax) + { + normalising_fact=max_array(moment)/MmntMax; + for(i=0;i<3;i++) + { + moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 + } + } } - ACS_STATUS = 5; + } - else + + else { - controlmodes(b,db,omega,1); - for (i=0;i<3;i++) - { - b[i]=b_copy[i]; - db[i]=db_copy[i]; - omega[i]=omega_copy[i]; - } + controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE); + for (i=0;i<3;i++) + { + b[i]=b_copy[i]; + db[i]=db_copy[i]; + omega[i]=omega_copy[i]; + } if(max_array(moment)>MmntMax) { normalising_fact=max_array(moment)/MmntMax; for(i=0;i<3;i++) - { - moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 - } + { + moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 + } } } @@ -206,7 +204,7 @@ } -void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1) +void controlmodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE) { float bb[3]={0,0,0}; float d[3]={0,0,0}; @@ -230,73 +228,75 @@ if (singularity_flag==0) { for(i=0;i<3;i++) - { - db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) - } + { + db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) + } for(i=0;i<3;i++) - { - b[i]/=den; // Mormalized b. Hence no unit. - } + { + b[i]/=den; // Mormalized b. Hence no unit. + } if(b[2]>0.9 || b[2]<-0.9) - { - kz=kz2; - kmu=kmu2; - gamma=gamma2; - } + { + kz=kz2; + kmu=kmu2; + gamma=gamma2; + } for(i=0;i<2;i++) - { - Mu[i]=b[i]; - v[i]=-kmu*Mu[i]; - dv[i]=-kmu*db[i]; - z[i]=db[i]-v[i]; - u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); - } + { + Mu[i]=b[i]; + v[i]=-kmu*Mu[i]; + dv[i]=-kmu*db[i]; + z[i]=db[i]-v[i]; + u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); + } inverse(Jm,invJm,singularity_flag); for(i=0;i<3;i++) - { - for(j=0;j<3;j++) { - bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]); + for(j=0;j<3;j++) + { + bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]); + } } - } for(i=0;i<3;i++) - { - for(j=0;j<3;j++) { - d[i]+=bb[j]*invJm[i][j]; + for(j=0;j<3;j++) + { + d[i]+=bb[j]*invJm[i][j]; + } } - } bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]); bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]); bb[0]=0; for(i=0;i<3;i++) - { - d[i]=invJm[2][i]; - invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; - invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; - invJm[0][i]=b[i]; - } + { + d[i]=invJm[2][i]; + invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; + invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; + invJm[0][i]=b[i]; + } inverse(invJm,Jm,singularity_flag); if (singularity_flag==0) - { - for(i=0;i<3;i++) { - for(j=0;j<3;j++) - { - tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 - } + for(i=0;i<3;i++) + { + for(j=0;j<3;j++) + { + tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 + } + } + for(i=0;i<3;i++) + { + bcopy[i]=b[i]*den; + } + for(i=0;i<3;i++) + { + Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; + Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 + } } - for(i=0;i<3;i++) - { - bcopy[i]=b[i]*den; - } - for(i=0;i<3;i++) - { - Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; - Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 - } - } + } + if (singularity_flag==1) { for (i=0;i<3;i++) @@ -304,26 +304,29 @@ Mmnt[i]=2*MmntMax; } } + ACS_STATUS = 5; } + else if(controlmode1==1) { - if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo - { - for(i=0;i<3;i++) + if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo { - Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2 + for(i=0;i<3;i++) + { + Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2 + } + ACS_STATUS = 6; } - ACS_STATUS = 6; - } - else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo - { - for(i=0;i<3;i++) + else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo { - Mmnt[i]=-kdetumble*db[i]; + for(i=0;i<3;i++) + { + Mmnt[i]=-kdetumble*db[i]; + } + ACS_STATUS = 4; } - ACS_STATUS = 4; - } } + for(i=0;i<3;i++) { moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2 @@ -336,17 +339,10 @@ int SENSOR_INIT(); int FCTN_ATS_DATA_ACQ(); //data is obtained int SENSOR_DATA_ACQ(); -void T_OUT(); //timeout function to stop infinite loop int CONFIG_UPLOAD(); -Timeout to; //Timeout variable to -int toFlag; int count =0; // Time for which the BAE uC is running (in seconds) -void T_OUT() -{ - toFlag=0; //as T_OUT function gets called the while loop gets terminated -} //DEFINING VARIABLES @@ -358,7 +354,9 @@ int16_t bit_data; uint16_t time_data; float gyro_data[3], mag_data[3]; -float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; + + +int ack; int CONFIG_UPLOAD() { @@ -391,17 +389,48 @@ } + int SENSOR_INIT() { pc_acs.printf("Entered sensor init\n \r"); cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; - i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up - wait_ms(600); //waiting for loading configuration file stored in EEPROM + wait(1); + ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up + + pc_acs.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM + + if( ack!=0) + { + cmd[0]=RESETREQ; + cmd[1]=BIT_RESREQ; + ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat + if(ack !=0) + return 0; + } + + wait_ms(600); + cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - i2c.read(SLAVE_ADDR_READ,&store,1); + ack = i2c.write(SLAVE_ADDR,cmd,1); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&store,1); + + if( ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&store,1); + if(ack!=0) + return 0; + } + + pc_acs.printf("Sentral Status is %x\n \r",(int)store); @@ -416,13 +445,33 @@ default: { cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } wait_ms(600); cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - i2c.read(SLAVE_ADDR_READ,&store,1); - wait_ms(100); + ack = i2c.write(SLAVE_ADDR,cmd,1); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&store,1); + + if( ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&store,1); + if(ack!=0) + return 0; + } + pc_acs.printf("Sentral Status is %x\n \r",(int)store); } @@ -430,12 +479,19 @@ int manual=0; - if( ((int)store != 11 )&&((int)store != 11)) + + if( ((int)store != 11 )&&((int)store != 3)) { cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } wait_ms(600); manual = CONFIG_UPLOAD(); @@ -450,32 +506,87 @@ cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors cmd[1]=BIT_RUN_ENB; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer cmd[1]=BIT_MAGODR; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope cmd[1]=BIT_GYROODR; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer - cmd[1]=0x00; + cmd[1]=0x00; + ack = i2c.write(SLAVE_ADDR,cmd,2); - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } + cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values cmd[1]=0x00; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values cmd[1]=BIT_EVT_ENB; - i2c.write(SLAVE_ADDR,cmd,2); + ack = i2c.write(SLAVE_ADDR,cmd,2); + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,2); + if(ack!=0) + return 0; + } cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - i2c.read(SLAVE_ADDR_READ,&store,1); + ack = i2c.write(SLAVE_ADDR,cmd,1); + + if( ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + + ack= i2c.read(SLAVE_ADDR_READ,&store,1); + if( ack!=0) + { + ack= i2c.read(SLAVE_ADDR_READ,&store,1); + if(ack!=0) + return 0; + } + + pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store); if( (int)store == 3) //Check if initialised properly and not in idle state @@ -483,51 +594,7 @@ pc_acs.printf("Exited sensor init successfully\n \r"); return 1; } - - else if((int)store == 11) - { - cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors - cmd[1]=BIT_RUN_ENB; - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - - cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer - cmd[1]=BIT_MAGODR; - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope - cmd[1]=BIT_GYROODR; - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer - cmd[1]=0x00; - wait_ms(100); - - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values - cmd[1]=0x00; - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - - cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values - cmd[1]=BIT_EVT_ENB; - i2c.write(SLAVE_ADDR,cmd,2); - wait_ms(100); - - cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - i2c.read(SLAVE_ADDR_READ,&store,1); - wait_ms(100); - pc_acs.printf("Sentral Status after trying again is %x\n \r",(int)store); - - if( (int)store != 3) - { - pc_acs.printf("Problem with initialising\n \r"); - return 0; - } - } - + pc_acs.printf("Sensor init failed \n \r") ; return 0; } @@ -543,25 +610,24 @@ pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); - if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11 + if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11 { pc_acs.printf("Sensor 1 marked working \n \r"); working = SENSOR_INIT(); if(working ==1) { - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r"); ACS_INIT_STATUS = 0; return 1; } - - + pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off ATS1_SW_ENABLE = 1; - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; } @@ -573,15 +639,21 @@ ATS2_SW_ENABLE = 0; wait_ms(5); + working = SENSOR_INIT(); if(working ==1) { pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; ACS_INIT_STATUS = 0; return 2; } + + ATS2_SW_ENABLE = 1; + wait_ms(5); + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; } @@ -591,8 +663,8 @@ - ATS2_SW_ENABLE = 1; - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; + + ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working return 0; } @@ -600,50 +672,55 @@ int SENSOR_DATA_ACQ() { - int mag_only=0; + pc_acs.printf("Entering Sensor data acq.\n \r"); char status; - int sentral; int event; int sensor; int error; int init; - int ack1; - int ack2; + uint8_t gyro_error=0; + uint8_t mag_error=0; + cmd[0]=EVT_STATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - //wait_ms(100); + + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + event = (int)status; cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - - printf("Ack1: %d Ack2 : %d\n",ack1,ack2); - - if((ack1!=0)||(ack2!=0)) + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) { - cmd[0]=EVT_STATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - //wait_ms(100); - cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - if((ack1!=0)||(ack2!=0)) - { - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed - actual_data.Bvalue_actual[i] = 0; - } - - return 1; - } + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + + sentral = (int) status; @@ -659,264 +736,247 @@ init = SENSOR_INIT(); - int ack1,ack2; cmd[0]=EVT_STATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - //wait_ms(100); + + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + event = (int)status; cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - sentral = (int)status; - - if((ack1!=0)||(ack2!=0)) + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) { - cmd[0]=EVT_STATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - event = (int)status; - wait_ms(100); - cmd[0]=SENTRALSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - sentral = (int)status; - wait_ms(100); - - if((ack1!=0)||(ack2!=0)) - { - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed - actual_data.Bvalue_actual[i] = 0; - } - - return 1; - } + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + + + + sentral = (int) status; pc_acs.printf("Event Status after resetting and init is %x\n \r",event); if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status - { - - int ack1,ack2; - char status; - cmd[0]=ERROR; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - error = (int)status; - - cmd[0]=SENSORSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - sensor = (int)status; - - - if((ack1!=0)||(ack2!=0)) - { - cmd[0]=ERROR; - i2c.write(SLAVE_ADDR,cmd,1); - ack1 = i2c.read(SLAVE_ADDR_READ,&status,1); - error = (int)status; - wait_ms(100); - - cmd[0]=SENSORSTATUS; - i2c.write(SLAVE_ADDR,cmd,1); - ack2 = i2c.read(SLAVE_ADDR_READ,&status,1); - sensor = (int)status; - wait_ms(100); - - if((ack1!=0)||(ack2!=0)) - { - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //acknowledgement failed - actual_data.Bvalue_actual[i] = 0; - } - - return 1; - } - } - - - - if((error!=0) || (sensor!=0)) - { - if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) ) - { - - if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) ) - { - - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - actual_data.Bvalue_actual[i] = 0; - } - - pc_acs.printf("error in both sensors.Exiting.\n \r"); - return 1; - } - pc_acs.printf("error in gyro alone.Exiting.\n \r"); + { - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - } - - mag_only = 1; - //return 2; - } - - if(mag_only!= 1){ - pc_acs.printf("error in something else.Exiting.\n \r"); - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - actual_data.Bvalue_actual[i] = 0; - } - return 1; - } - } - - if((event & 1 == 1 )) - { - pc_acs.printf("error in CPU Reset.\n \r"); - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - actual_data.Bvalue_actual[i] = 0; - } - return 1; - - } - - if((event & 8 != 8 )||(event & 32 != 32 )) - { - pc_acs.printf("Data not ready waiting...\n \r"); - //POLL - wait_ms(1000); - cmd[0]=EVT_STATUS; - i2c.write(SLAVE_ADDR,cmd,1); - i2c.read(SLAVE_ADDR_READ,&status,1); - wait_ms(100); - if((event & 8 != 8 )||(event & 32 != 32 )) - { - - if(event & 32 != 32 ) - { - if(event & 8 != 8 ) + cmd[0]=ERROR; + + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) { - pc_acs.printf("Both data still not ready.Exiting..\n \r"); - - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - actual_data.Bvalue_actual[i] = 0; - } - return 1; + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; } + + error = (int)status; - pc_acs.printf("Mag data only ready.Read..\n \r"); - mag_only = 1; - //return 2; - - } + cmd[0]=SENSORSTATUS; + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + + + + sensor = (int) status; + - } - - - } - - - } + if((error!=0) || (sensor!=0)) + { + if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) ) + { + pc_acs.printf("error in gyro alone..\n \r"); + gyro_error = 1; + } + + if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) ) + { + + pc_acs.printf("error in mag alone.Exiting.\n \r"); + mag_error = 1; + } + + if( (gyro_error!=1)&&(mag_error!=1)) + { + pc_acs.printf("error in something else.Exiting.\n \r"); + return 0; + + } + } + + if((event & 1 == 1 )) + { + pc_acs.printf("error in CPU Reset.\n \r"); + return 1; + + } + + if((event & 8 != 8 )||(event & 32 != 32 )) + { + pc_acs.printf("Data not ready waiting...\n \r"); + //POLL + wait_ms(200); + + cmd[0]=EVT_STATUS; + + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + { + ack = i2c.write(SLAVE_ADDR,cmd,1); + if(ack!=0) + return 0; + } + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + { + ack = i2c.read(SLAVE_ADDR_READ,&status,1); + if(ack!=0) + return 0; + } + + event = (int)status; + if(event & 32 != 32 ) + { - if(mag_only !=1) - { - if(mag_only!= 1){ - pc_acs.printf("Error in something else.Exiting.\n \r"); - for(int i=0; i<3; i++) { - actual_data.AngularSpeed_actual[i] = 0; //Set values to 0 - actual_data.Bvalue_actual[i] = 0; - } - return 1; - } + pc_acs.printf("Mag data only ready.Read..\n \r"); + gyro_error = 1; + + } + + if(event & 8 != 8 ) + { + pc_acs.printf("Both data still not ready.Exiting..\n \r"); + mag_error=1; + } + + + + + } + + + } + + if((mag_error !=1)&&(gyro_error!=1)) + { + pc_acs.printf("Error in something else.Exiting.\n \r"); + return 0; + } + + if((mag_error !=1)&&(gyro_error!=1)) + { + pc_acs.printf("Error in both gyro and mag.Exiting.\n \r"); + return 0; + } - } + } - } + cmd[0]=MAG_XOUT_H; //LSB of x i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together - ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24); + ack = i2c.read(SLAVE_ADDR_READ,reg_data,24); - if(ack1 != 0) + if(ack != 0) { wait_ms(100); cmd[0]=MAG_XOUT_H; //LSB of x i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together - ack1 = i2c.read(SLAVE_ADDR_READ,reg_data,24); + ack = i2c.read(SLAVE_ADDR_READ,reg_data,24); wait_ms(100); - if(ack1 !=1) - { - for(int i=0;i<3;i++) - { - actual_data.Bvalue_actual[i] = 0; - actual_data.AngularSpeed_actual[i] = 0; - } - return 1; - } - + if(ack !=1) + return 0; + } // pc_acs.printf("\nGyro Values:\n"); - for(int i=0; i<3; i++) { - //concatenating gyro LSB and MSB to get 16 bit signed data values - bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i]; - gyro_data[i]=(float)bit_data; - gyro_data[i]=gyro_data[i]/senstivity_gyro; - gyro_data[i]+=gyro_error[i]; + + if (gyro_error!=1) + { + for(int i=0; i<3; i++) { + //concatenating gyro LSB and MSB to get 16 bit signed data values + bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i]; + gyro_data[i]=(float)bit_data; + gyro_data[i]=gyro_data[i]/senstivity_gyro; + actual_data.AngularSpeed_actual[i] = gyro_data[i]; + } } - for(int i=0; i<3; i++) { - //concatenating mag LSB and MSB to get 16 bit signed data values Extract data - bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i]; - mag_data[i]=(float)bit_data; - mag_data[i]=mag_data[i]/senstivity_mag; - mag_data[i]+=mag_error[i]; + + if(mag_error!=1){ + for(int i=0; i<3; i++) { + //concatenating mag LSB and MSB to get 16 bit signed data values Extract data + bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i]; + mag_data[i]=(float)bit_data; + mag_data[i]=mag_data[i]/senstivity_mag; + actual_data.Bvalue_actual[i] = mag_data[i]; + } } - for(int i=0; i<3; i++) { - // data[i]=gyro_data[i]; - actual_data.AngularSpeed_actual[i] = gyro_data[i]; - actual_data.Bvalue_actual[i] = mag_data[i]; - } - + + - if(mag_only == 0) + if(mag_error == 1) { - pc_acs.printf("Reading data successful.\n \r"); - return 0; + pc_acs.printf("Gyro only successful.\n \r"); + return 1; } - else if(mag_only == 1) + if(gyro_error == 1) { - - for(int i=0;i<3;i++) - { - actual_data.AngularSpeed_actual[i] = 0; - } - - - pc_acs.printf("Reading data partial success.\n \r"); - return 2; + pc_acs.printf("Mag only successful.\n \r"); + return 2; } - pc_acs.printf("Reading data success.\n \r"); - return 0; + pc_acs.printf("Reading data success.\n \r"); + return 3; } @@ -925,173 +985,434 @@ int FCTN_ATS_DATA_ACQ() { + for(int i=0; i<3; i++) { + actual_data.AngularSpeed_actual[i] = 0; + actual_data.Bvalue_actual[i] = 0; + } + int acq; + int init; pc_acs.printf("DATA_ACQ called \n \r"); pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); - // 0 success //1 full failure //2 partial failure - - if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) + if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) //ATS1 status is 01XX to check if ATS1 is ON + { + + acq = SENSOR_DATA_ACQ(); //ATS1 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only + + if(acq == 3) //Both available read and exit + { + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; + + pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); + pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); + return 3; + } + else if((acq == 2)||(acq==1)) //Only mag or only gyro + { + + pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r"); + + + if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) ) + + { + + //other sensor both working, off or + //other sensor gyro working, this sensor not working , off + //other sensor mag working, this sensor not working,off + + + ATS1_SW_ENABLE = 1; //switch off sensor 1 + wait_ms(5); + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; + } + + + ATS2_SW_ENABLE = 0; //switch on sensor 2 + wait_ms(5); + + init = SENSOR_INIT(); //sensor 2 init + + if( init == 0) + { + + pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); + ATS2_SW_ENABLE = 1; + wait_ms(5); + ATS1_SW_ENABLE = 0; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1 + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + } + return acq; + } + + int acq2; + acq2 = SENSOR_DATA_ACQ(); + + if(acq2 == 3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; + pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit + return 3; + } + + else if(acq2 == 1) + { + if(acq==2) + { + ATS2_SW_ENABLE = 1; + wait_ms(5); + ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + return 3; + } + else + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only + return 1; + } + } + + else if(acq2==2) //Sensor 2 mag only, exit in both cases + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + return 2; + } + + + + else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1 + { + + pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); + ATS2_SW_ENABLE = 1; + wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40; + return acq; + + } + + } + + else //Sensor 2 not working or both sensors gyro/mag ONLY + { + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq + return 1; + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + return 2; + } + pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); + return acq; + + } + } + + else if(acq == 0) + { + pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all + ATS1_SW_ENABLE = 1; + wait_ms(5); //Switch ON sensor 2 + ATS2_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; + if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX + { + + + init = SENSOR_INIT(); + + if( init == 0) + { + + pc_acs.printf(" Sensor 2 also data acq failure.\n \r"); + ATS2_SW_ENABLE = 1; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit + return 0; + } + + int acq2; + acq2 = SENSOR_DATA_ACQ(); + + if(acq2 == 3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; + pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working + return 3; + } + + else if(acq2 == 1) + { + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; + return 1; + + } + + else if(acq2 == 2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + return 2; + } + + + else if(acq2 == 0) + { + + pc_acs.printf(" Sensor 2 data acq failure..\n \r"); + ATS2_SW_ENABLE = 1; + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; + return 0; + + } + + } + + } + } + + + if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) //ATS2 status is 01XX to check if ATS2 is ON { - acq = SENSOR_DATA_ACQ(); - if(acq == 0) - { - - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; - - ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 - pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); - pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r"); - return 0; - } - else if(acq == 2) - { - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; - pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r"); - return 2; + acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only + + if(acq == 3) //Both available read and exit + { - /*if((ACS_ATS_STATUS & 0x0F == 0x00)) - { - pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r"); - ATS1_SW_ENABLE = 1; - ATS2_SW_ENABLE = 0; - wait_ms(5); - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; + + pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); + pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r"); + return 3; + } + else if((acq == 2)||(acq==1)) //Only mag or only gyro + { + + pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r"); + - int acq; - acq = SENSOR_DATA_ACQ(); + if( (ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) ) + + { - if(acq == 0) - { - ACS_DATA_ACQ_STATUS = 0; - pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); - return 0; - - } - else if(acq == 2) - { - ACS_DATA_ACQ_STATUS = 2; - pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); - return 2; - } + //other sensor both working, off or + //other sensor gyro working, this sensor not working , off + //other sensor mag working, this sensor not working,off + + + ATS2_SW_ENABLE = 1; //switch off sensor 2 + wait_ms(5); + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02; + } - else if(acq == 1) - { + + ATS1_SW_ENABLE = 0; //switch on sensor 1 + wait_ms(5); + + init = SENSOR_INIT(); //sensor 2 init + + if( init == 0) + { + + pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); + ATS1_SW_ENABLE = 1; + wait_ms(5); + ATS2_SW_ENABLE = 0; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2 + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + } + return acq; + } + + int acq2; + acq2 = SENSOR_DATA_ACQ(); - int acq; - pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r"); - ATS2_SW_ENABLE = 1; - ATS1_SW_ENABLE = 0; - wait_ms(5); - acq = SENSOR_DATA_ACQ(); - if(acq == 0) - { - pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); - ACS_DATA_ACQ_STATUS = 0; - return 0; - } - else if(acq == 2) - { - pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r"); - ACS_DATA_ACQ_STATUS = 2; - return 2; - } - else - { - pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); - ATS1_SW_ENABLE = 0; - ACS_DATA_ACQ_STATUS = 1; + if(acq2 == 3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; + pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit + return 3; + } + + else if(acq2 == 1) + { + if(acq==2) + { + ATS1_SW_ENABLE = 1; + wait_ms(5); + ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + return 3; + } + else + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only return 1; - } + } + } + + else if(acq2==2) //Sensor 1 mag only, exit in both cases + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + return 2; + } + + + + else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2 + { - pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r"); - ATS1_SW_ENABLE = 0; - ACS_DATA_ACQ_STATUS = 1; - return 1; - - } + pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r"); + ATS1_SW_ENABLE = 1; + wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same + ATS2_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04; + return acq; + + } + + } - } - - else - { - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40; - pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r"); - return 2; - - - }*/ - + else //Sensor 1 not working or both sensors gyro/mag ONLY + { + if(acq == 1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq + return 1; + } + if(acq==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + return 2; + } + pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r"); + return acq; + + } } - else if(acq == 1) + else if(acq == 0) { - pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); - ATS1_SW_ENABLE = 1; - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; + pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all + ATS2_SW_ENABLE = 1; + wait_ms(5); //Switch ON sensor 1 + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; + if( (ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX + { + + + init = SENSOR_INIT(); + + if( init == 0) + { + + pc_acs.printf(" Sensor 1 also data acq failure.\n \r"); + ATS2_SW_ENABLE = 1; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit + return 0; + } + + int acq2; + acq2 = SENSOR_DATA_ACQ(); + + if(acq2 == 3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; + pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working + return 3; + } + + else if(acq2 == 1) + { + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; + return 1; + + } + + else if(acq2 == 2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + return 2; + } + + + else if(acq2 == 0) + { + + pc_acs.printf(" Sensor 1 data acq failure..\n \r"); + ATS1_SW_ENABLE = 1; + + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; + return 0; + + } + + } + } - + } - } + - - - ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0; - if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) - { - ATS2_SW_ENABLE = 0; - wait_ms(5); - - acq = SENSOR_DATA_ACQ(); - if(acq == 0) - { - pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; - ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 - return 0; - } - else if(acq == 2) - { - - pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r"); - - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04; - ACS_DATA_ACQ_STATUS = 2; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 - return 2; - - } - - else if(acq == 1) - { - pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r"); - ATS2_SW_ENABLE = 1; - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; - //Sensor 2 also not working - } - - - - - - } + + + - ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E; - + + pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r"); - ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2 - return 1; + return 0; } void FCTN_ACS_GENPWM_MAIN(float Moment[3]) @@ -1121,25 +1442,22 @@ } l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship - printf("current in trx is %f \r \n",l_current_x); + pc_acs.printf("\r\n current in trx is %f \r \n",l_current_x); if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation - printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } else if (l_current_x >= 0.0016 && l_current_x < 0.0171) { l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation - printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); - PWM1 = l_duty_cycle_x/100 ; + PWM1 = l_duty_cycle_x/100; } else if(l_current_x >= 0.0171 && l_current_x < 0.1678) { l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation - printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } @@ -1147,7 +1465,6 @@ { printf("\n \r l_current_x====0"); l_duty_cycle_x = 0; // default value of duty cycle - printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } @@ -1155,6 +1472,8 @@ { g_err_flag_TR_x = 1; } + + pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); //------------------------------------- y-direction TR--------------------------------------// @@ -1170,25 +1489,22 @@ l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship - printf("current in try is %f \r \n",l_current_y); + pc_acs.printf("current in try is %f \r \n",l_current_y); if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation - printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } else if (l_current_y >= 0.0016 && l_current_y < 0.0171) { l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation - printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } else if(l_current_y >= 0.0171 && l_current_y < 0.1678) { l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation - printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } @@ -1196,7 +1512,6 @@ { printf("\n \r l_current_y====0"); l_duty_cycle_y = 0; // default value of duty cycle - printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } @@ -1204,6 +1519,8 @@ { g_err_flag_TR_y = 1; } + + pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); //----------------------------------------------- z-direction TR -------------------------// @@ -1220,11 +1537,10 @@ l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship - printf("current in trz is %f \r \n",l_current_z); + pc_acs.printf("current in trz is %f \r \n",l_current_z); if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation - printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } @@ -1238,7 +1554,6 @@ else if(l_current_z >= 0.0171 && l_current_z < 0.1678) { l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation - printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } @@ -1246,7 +1561,6 @@ { printf("\n \r l_current_z====0"); l_duty_cycle_z = 0; // default value of duty cycle - printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } @@ -1255,6 +1569,8 @@ g_err_flag_TR_z = 1; } + pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); + //-----------------------------------------exiting the function-----------------------------------// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
--- a/ACS.h Mon Jun 13 13:44:31 2016 +0000 +++ b/ACS.h Tue Jun 28 10:11:54 2016 +0000 @@ -9,21 +9,20 @@ #define kdetumble 2000000 #define MmntMax 1.1 // Unit: Ampere*Meter^2 #define OmegaMax 1*3.1415/180.0 // Unit: Radians/Second -#define ACS_DEMAG_TIME_DELAY 20 +#define ACS_DEMAG_TIME_DELAY 65 +#define ACS_Z_FIXED_MOMENT 1.3 #define senstivity_gyro 6.5536; //senstivity is obtained from 2^15/5000dps #define senstivity_mag 32.768; //senstivity is obtained from 2^15/1000microtesla -#define senstivity_time 32; //senstivity is obtained from 2^16/2048dps +#define senstivity_time 32; //senstivity is obtained from 2^16/2048 s void FCTN_ACS_GENPWM_MAIN(float*); -void FCTN_ACS_CNTRLALGO(float*,float*,int); -void controlmodes(float*, float*, float*, uint8_t); +void FCTN_ACS_CNTRLALGO(float*,float*,float*,uint8_t,uint8_t,uint8_t); +void controlmodes(float*,float*, float*, float*, uint8_t,uint8_t); void inverse(float mat[3][3],float inv[3][3]); extern void FLAG(); void FCTN_ATS_SWITCH(bool); int FCTN_ACS_INIT(); //initialization of registers happens -//void FCTN_ATS_DATA_ACQ(float*,float*); // main function: checks errors, gets data, switches on/off the sensor -//void FCTN_GET_DATA(float*,float*); //data is obtained void FCTN_T_OUT(); //timeout function to stop infinite loop int FCTN_ATS_DATA_ACQ();
--- a/TCTM.cpp Mon Jun 13 13:44:31 2016 +0000 +++ b/TCTM.cpp Tue Jun 28 10:11:54 2016 +0000 @@ -18,6 +18,7 @@ extern DigitalOut DRV_Z_EN; extern DigitalOut DRV_XY_EN; extern uint8_t ACS_STATUS; +extern uint8_t ACS_ATS_STATUS; extern float b_old[3]; // Unit: Tesla extern float db[3]; extern uint8_t flag_firsttime; @@ -64,9 +65,16 @@ extern int SENSOR_INIT(); extern int FCTN_ATS_DATA_ACQ(); -extern void FCTN_ACS_CNTRLALGO(float*,float*,int); uint8_t telemetry[135]; +extern AnalogIn CurrentInput; // Input from Current Multiplexer //PIN54 + + +extern DigitalOut SelectLineb3; // MSB of Select Lines +extern DigitalOut SelectLineb2; +extern DigitalOut SelectLineb1; +extern DigitalOut SelectLineb0; // LSB of Select Lines + void FCTN_CONVERT_UINT (uint8_t input[2], float* output) { @@ -514,114 +522,67 @@ { case 0xE0: { - float mag_data[3]={0,0,0}; - float gyro_data[3]={0,0,0}; + float mag_data_comm[3]={0,0,0}; + float gyro_data_comm[3]={0,0,0}; + float moment_comm[3]; printf("ACS_COMSN SOFTWARE\r\n"); //ACK_L234_telemetry ACS_STATE = tc[4]; - if(ACS_STATE == 7) // Nominal mode + if(ACS_STATE == 7) // Nominal mode { printf("\n\r Nominal mode \n"); - DRV_Z_EN = 1; - DRV_XY_EN = 1; + - FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1); + FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) { - printf("%f\t",moment[i]); - } + printf("%f\t",moment_comm[i]); + } - ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY } else if(ACS_STATE == 8) // Auto Control { - printf("\n\r Auto control mode \n"); - DRV_Z_EN = 1; - DRV_XY_EN = 1; - - - FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0); + printf("\n\r Auto control mode \n"); + + FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) { - printf("%f\t",moment[i]); + printf("%f\t",moment_comm[i]); } } else if(ACS_STATE == 9) // Detumbling { - DRV_Z_EN = 1; - DRV_XY_EN = 1; - - if(flag_firsttime==1) - { - for(int i=0;i<3;i++) - { - db[i]=0; // Unit: Tesla/Second - } - flag_firsttime=0; - } - - else - { - for(int i=0;i<3;i++) - { - db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second - } - } - - - if (ACS_DETUMBLING_ALGO_TYPE == 0) - { - - for(int i=0;i<3;i++) - { - moment[i]=-kdetumble*(mag_data[(i+1)%3]*gyro_data[(i+2)%3]-mag_data[(i+2)%3]*gyro_data[(i+1)%3]); // Unit: Ampere*Meter^2 - } - - - ACS_STATUS = 6; // set ACS_STATUS = ACS_BOMEGA_CONTROL - } - - else if(ACS_DETUMBLING_ALGO_TYPE == 1) - { - - for(int i=0;i<3;i++) - { - moment[i]=-kdetumble*db[i]; // Unit: Ampere*Meter^2 - } - - ACS_STATUS = 4; // set ACS_STATUS = ACS_BDOT_CONTROL - } - - for(int i=0;i<3;i++) - { - - b_old[i]= mag_data[i]; // Unit: Tesla/Second - } - + + FCTN_ACS_CNTRLALGO(moment_comm,mag_data_comm,gyro_data_comm,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) { - printf("%f\t",moment[i]); - } + printf("%f\t",moment_comm[i]); + } + } - ACS_STATUS = 7; + else + { + + ACS_STATUS = 7; + } // Control algo commissioning - FCTN_CONVERT_FLOAT(moment[0],&telemetry[4]); //telemetry[4] - telemetry[7] - FCTN_CONVERT_FLOAT(moment[1],&telemetry[8]); //telemetry[8] - telemetry[11] - FCTN_CONVERT_FLOAT(moment[2],&telemetry[12]); //telemetry[12] - telemetry[15] + FCTN_CONVERT_FLOAT(moment_comm[0],&telemetry[4]); //telemetry[4] - telemetry[7] + FCTN_CONVERT_FLOAT(moment_comm[1],&telemetry[8]); //telemetry[8] - telemetry[11] + FCTN_CONVERT_FLOAT(moment_comm[2],&telemetry[12]); //telemetry[12] - telemetry[15] // to include commission TR as well for(uint8_t i=16;i<132;i++) { @@ -638,8 +599,10 @@ printf("HARDWARE_COMSN\r\n"); //ACK_L234_telemetry - int init; - int data; + int init1=0; + int init2=0; + int data1=0; + int data2=0; float PWM_tc[3]; PWM_tc[0]=(float(tc[4]))*1; @@ -662,14 +625,135 @@ wait_ms(ACS_DEMAG_TIME_DELAY); ATS2_SW_ENABLE = 1; + wait_ms(5); ATS1_SW_ENABLE = 0; - init = SENSOR_INIT(); - data = SENSOR_DATA_ACQ(); + wait_ms(5); + init1 = SENSOR_INIT(); + if( init1 == 1) + { + data1 = SENSOR_DATA_ACQ(); + } + + + ATS1_SW_ENABLE = 1; + wait_ms(5); + ATS2_SW_ENABLE = 0; + wait_ms(5); + + if(data1 == 0) + { + ATS2_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; + } + else if(data1==1) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; + } + else if(data1==2) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20; + } + else if(data1==3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x30; + } + + + init2 = SENSOR_INIT(); + if( init2 == 1) + { + data2 = SENSOR_DATA_ACQ(); + } + + uint8_t ats_data=1; - ATS2_SW_ENABLE = 1; - ATS1_SW_ENABLE = 0; - init = SENSOR_INIT(); - data = SENSOR_DATA_ACQ(); + if(data2 == 0) + { + ATS2_SW_ENABLE = 1; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; + if(data1 == 2) + { + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + } + else if(data1 == 3) + { + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; + } + else if(data1 == 1) + { + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; + ats_data = 0; + } + + + } + + else if(data2==1) + { + + + + if(data1 == 2) + { + ATS2_SW_ENABLE = 1; + wait_ms(5); + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60; + } + else if(data1 == 3) + { + ATS2_SW_ENABLE = 1; + wait_ms(5); + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; + } + else + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; + ats_data = 0; + } + } + + else if(data2==2) + { + if(data1 == 3) + { + ATS2_SW_ENABLE = 1; + wait_ms(5); + ATS1_SW_ENABLE = 0; + wait_ms(5); + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02; + ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70; + } + else + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06; + } + } + else if(data2==3) + { + ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07; + } + + SelectLineb3 =0; + SelectLineb2 =1; + SelectLineb1 =0; + SelectLineb0 =1; + int resistance; + + PWM1 = PWM_tc[0]; PWM2 = 0; @@ -677,7 +761,23 @@ wait_ms(ACS_DEMAG_TIME_DELAY); - SENSOR_DATA_ACQ(); + if(ats_data == 0) + SENSOR_DATA_ACQ(); + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (0*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (1*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (2*4)]); @@ -690,9 +790,23 @@ wait_ms(ACS_DEMAG_TIME_DELAY); - + if(ats_data == 0) + SENSOR_DATA_ACQ(); + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + - SENSOR_DATA_ACQ(); FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]); @@ -704,16 +818,54 @@ wait_ms(ACS_DEMAG_TIME_DELAY); - wait_ms(ACS_DEMAG_TIME_DELAY); - - SENSOR_DATA_ACQ(); + if(ats_data == 0) + SENSOR_DATA_ACQ(); + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (4*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (5*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (6*4)]); FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (7*4)]); + PWM1 = 0; + PWM2 = 0; + PWM3 = 0; - + wait_ms(ACS_DEMAG_TIME_DELAY); + + if(ats_data == 0) + SENSOR_DATA_ACQ(); + actual_data.current_actual[5]=CurrentInput.read(); + actual_data.current_actual[5]= actual_data.current_actual[5]*3.3/(50*rsens); + + + resistance=24000*actual_data.current_actual[5]/(3.3-actual_data.current_actual[5]); + if(actual_data.current_actual[5]>1.47) + { + actual_data.current_actual[5]=3694/log(24.032242*resistance); + } + else{ + + actual_data.current_actual[5]=3365.4/log(7.60573*resistance); + } + + + FCTN_CONVERT_FLOAT(actual_data.current_actual[5],&telemetry[3 + (8*4)]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[0],&telemetry[3 + (9*4)]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[1],&telemetry[3 + (10*4)]); + FCTN_CONVERT_FLOAT(actual_data.Bvalue_actual[2],&telemetry[3 + (11*4)]); // for(int i=0; i<12; i++) // FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&telemetry[16 + (i*4)]);
--- a/main.cpp Mon Jun 13 13:44:31 2016 +0000 +++ b/main.cpp Tue Jun 28 10:11:54 2016 +0000 @@ -22,7 +22,7 @@ uint8_t ACS_INIT_STATUS = 0; uint8_t ACS_DATA_ACQ_STATUS = 0; -uint8_t ACS_ATS_STATUS = 0x60; +uint8_t ACS_ATS_STATUS = 0x73; uint8_t ACS_MAIN_STATUS = 0; uint8_t ACS_STATUS = 0; uint8_t ACS_DETUMBLING_ALGO_TYPE = 0; @@ -65,10 +65,7 @@ extern float gyro_data[3]; extern float mag_data[3]; -extern float moment[3]; -extern float b_old[3]; // Unit: Tesla -extern float db[3]; -extern uint8_t flag_firsttime; +float moment[3]; extern uint8_t BCN_FEN; @@ -147,7 +144,7 @@ void F_ACS() { - + pc.printf("Entered ACS.\n\r"); ACS_MAIN_STATUS = 1; //set ACS_MAIN_STATUS flag @@ -156,6 +153,9 @@ PWM2 = 0; //clear pwm pins PWM3 = 0; //clear pwm pins + wait_ms(ACS_DEMAG_TIME_DELAY); + + ACS_DATA_ACQ_STATUS = (uint8_t) FCTN_ATS_DATA_ACQ(); @@ -225,7 +225,7 @@ moment[0] = 0; moment[1] = 0; - moment[2] =1.3; // is a dummy value + moment[2] =ACS_Z_FIXED_MOMENT; // is a dummy value @@ -236,7 +236,7 @@ } - else if(ACS_DATA_ACQ_STATUS == 1) + else if((ACS_DATA_ACQ_STATUS == 0)||(ACS_DATA_ACQ_STATUS == 1)) { DRV_Z_EN = 1; DRV_XY_EN = 0; @@ -245,7 +245,7 @@ moment[0] = 0; moment[1] = 0; - moment[2] =1.3; // is a dummy value + moment[2] =ACS_Z_FIXED_MOMENT; // is a dummy value FCTN_ACS_GENPWM_MAIN(moment) ; ACS_MAIN_STATUS = 0; @@ -263,7 +263,7 @@ moment[0] = 0; moment[1] = 0; - moment[2] =1.3; // 1.3 is a dummy value + moment[2] =ACS_Z_FIXED_MOMENT; // 1.3 is a dummy value FCTN_ACS_GENPWM_MAIN(moment) ; ACS_MAIN_STATUS = 0; @@ -278,32 +278,9 @@ ACS_STATUS = 4; // set Set ACS_STATUS = ACS_BDOT_CONTROL - float db[3]; - if(flag_firsttime==1) - { - for(int i=0;i<3;i++) - { - db[i]=0; // Unit: Tesla/Second - } - flag_firsttime=0; - } - - else - { - for(int i=0;i<3;i++) - { - db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second - } - } - - - - for(int i=0;i<3;i++) - { - moment[i]=-kdetumble*db[i]; - b_old[i]= mag_data[i]; // Unit: Tesla/Second - } + ACS_DETUMBLING_ALGO_TYPE = 0x01; + FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) @@ -313,6 +290,7 @@ FCTN_ACS_GENPWM_MAIN(moment) ; + ACS_MAIN_STATUS = 0; return; @@ -325,7 +303,7 @@ DRV_Z_EN = 1; DRV_XY_EN = 1; - FCTN_ACS_CNTRLALGO(mag_data,gyro_data,1); + FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x01,0x00,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) @@ -334,7 +312,6 @@ } FCTN_ACS_GENPWM_MAIN(moment) ; - ACS_STATUS = 5; // set ACS_STATUS = ACS_NOMINAL_ONLY ACS_MAIN_STATUS = 0; return; @@ -348,7 +325,7 @@ DRV_Z_EN = 1; DRV_XY_EN = 1; - FCTN_ACS_CNTRLALGO(mag_data,gyro_data,0); + FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x00,ACS_DETUMBLING_ALGO_TYPE); printf("\n\r Moment values returned by control algo \n"); for(int i=0; i<3; i++) { @@ -366,58 +343,7 @@ DRV_Z_EN = 1; DRV_XY_EN = 1; - if(flag_firsttime==1) - { - for(int i=0;i<3;i++) - { - db[i]=0; // Unit: Tesla/Second - } - flag_firsttime=0; - } - - else - { - for(int i=0;i<3;i++) - { - db[i]= (mag_data[i]-b_old[i])/sampling_time; // Unit: Tesla/Second - } - } - - - if (ACS_DETUMBLING_ALGO_TYPE == 0) - { - - for(int i=0;i<3;i++) - { - moment[i]=-kdetumble*(mag_data[(i+1)%3]*gyro_data[(i+2)%3]-mag_data[(i+2)%3]*gyro_data[(i+1)%3]); // Unit: Ampere*Meter^2 - } - - - ACS_STATUS = 6; // set ACS_STATUS = ACS_BOMEGA_CONTROL - } - - else if(ACS_DETUMBLING_ALGO_TYPE == 1) - { - - for(int i=0;i<3;i++) - { - moment[i]=-kdetumble*db[i]; // Unit: Ampere*Meter^2 - } - - ACS_STATUS = 4; // set ACS_STATUS = ACS_BDOT_CONTROL - } - - for(int i=0;i<3;i++) - { - - b_old[i]= mag_data[i]; // Unit: Tesla/Second - } - - printf("\n\r Moment values returned by control algo \n"); - for(int i=0; i<3; i++) - { - printf("%f\t",moment[i]); - } + FCTN_ACS_CNTRLALGO(moment,mag_data,gyro_data,0x00,0x01,ACS_DETUMBLING_ALGO_TYPE); FCTN_ACS_GENPWM_MAIN(moment) ; ACS_MAIN_STATUS = 0;