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:
- 49:61c9f28332ba
- Parent:
- 39:670133e7ffd8
- Child:
- 52:daa685b0e390
--- a/ACS.cpp Fri Jul 08 08:25:39 2016 +0000
+++ b/ACS.cpp Thu Jul 14 23:04:26 2016 +0000
@@ -2,6 +2,12 @@
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
#include <mbed.h>
#include <math.h>
+Timer timer_SENSOR_DATA_ACQ;
+Timer timer_controlmodes;
+Timer timer_SENSOR_INIT;
+Timer timer_CONFIG_UPLOAD;
+
+Serial acs_pc(USBTX,USBRX);
#include "pni.h" //pni header file
#include "pin_config.h"
@@ -21,16 +27,16 @@
extern uint8_t ACS_STATUS;
extern uint8_t ACS_DETUMBLING_ALGO_TYPE;//////
-extern DigitalInOut ATS1_SW_ENABLE; // enable of att sens2 switch
-extern DigitalInOut ATS2_SW_ENABLE; // enable of att sens switch
+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;
extern uint8_t ACS_STATE;
-DigitalInOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
-DigitalInOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
-DigitalInOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
+DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
+DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
+DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
extern PwmOut PWM1; //x //Functions used to generate PWM signal
extern PwmOut PWM2; //y
@@ -68,9 +74,8 @@
uint8_t invjm_mms[9];
uint8_t jm_mms[9];
uint8_t bb_mms[3];
-uint8_t singularity_flag=0;
+uint8_t singularity_flag_mms=0;
uint8_t B_SCZ_ANGLE = 0x00;
-
uint8_t ACS_MAG_TIME_DELAY;// = 65;
uint8_t ACS_DEMAG_TIME_DELAY;// = 65;
uint16_t ACS_Z_FIXED_MOMENT;// = 1.3;
@@ -81,12 +86,12 @@
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;
+uint16_t ACS_MM_X_COMSN = 0;
+uint16_t ACS_MM_Y_COMSN = 0;
+uint16_t ACS_MG_X_COMSN = 0;
+uint16_t ACS_MG_Y_COMSN = 0;
+uint16_t ACS_MM_Z_COMSN = 0;
+uint16_t ACS_MG_Z_COMSN = 0;
uint8_t float_to_uint8(float min,float max,float val)
{
@@ -106,186 +111,145 @@
for(int i=0;i<d1;i++)
for(int j=0;j<d2;j++)
{
- printf("\n\r%f",*((arr+(i*d1))+j));
+ acs_pc.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]);
+ acs_pc.printf("\n\r%d",valarr[i*d1+j]);
}
}
-Serial pc_acs(USBTX,USBRX); //for usb communication
+//Serial pc(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];//*
+//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]);
+//void controllermodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t detumblingalgo);
+void inversec(float mat[3][3],float inv[3][3],int *singularity_flag);
+float max_array(float arr[3]);
-//CONTROLALGO PARAMETERS
-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)
+void FCTN_ACS_CNTRLALGO (float moment[3], float b1[3], float omega1[3], uint8_t nominal , uint8_t detumbling , uint8_t ACS_DETUMBLING_ALGO_TYPE )
{
-
+ float db1[3]; // Unit: Tesla/Second
float normalising_fact;
- float b_copy[3], omega_copy[3], db_copy[3];
- int i;
+ float b1_copy[3], omega1_copy[3], db1_copy[3];
+ int i, j;
if(flag_firsttime==1)
{
for(i=0;i<3;i++)
{
- db[i]=0; // Unit: Tesla/Second
+ db1[i]=0; // Unit: Tesla/Second
}
- flag_firsttime=0;
+ flag_firsttime=0;
}
else
{
for(i=0;i<3;i++)
{
- db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
+ db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
}
}
- if(nominal == 0)
-
- {
-
- if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
- {
- alarmmode=0;
- }
- else if(max_array(omega)>OmegaMax&& alarmmode==0)
- {
- alarmmode=1;
- }
-
- }
-
+ if(nominal == 0)
+ {
+ if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
+ {
+ alarmmode=0;
+ }
+ else if(max_array(omega1)>OmegaMax && alarmmode==0)
+ {
+ alarmmode=1;
+ }
+
+ }
+
for (i=0;i<3;i++)
{
- b_copy[i]=b[i];
- db_copy[i]=db[i];
- omega_copy[i]=omega[i];
+ b1_copy[i]=b1[i];
+ db1_copy[i]=db1[i];
+ omega1_copy[i]=omega1[i];
}
if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
{
- controlmode_mms = 0;
- controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE);
+ //*controlmode=0;
+ controlmode_mms =0;
+ controllermodes(moment,b1,db1,omega1,controlmode_mms,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];
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_copy[i];
}
if(max_array(moment)>MmntMax)
{
+ //*controlmode=1;
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];
- }
+ controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE);
+ for (i=0;i<3;i++)
+ {
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_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
+
+ ACS_STATUS = 5;
}
else
{
+ //*controlmode=1;
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];
- }
+ controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE);
+ for (i=0;i<3;i++)
+ {
+ b1[i]=b1_copy[i];
+ db1[i]=db1_copy[i];
+ omega1[i]=omega1_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]=b[i];
+ b_old[i]=b1[i];
}
}
-void inverse(float mat[3][3],float inv[3][3],uint8_t &singularity_flag)
+void controllermodes(float moment[3], float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
{
- int i,j;
- float det=0;
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
- {
- inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
- }
- }
- det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
- if (det==0)
- {
- singularity_flag=1;
- }
- else
- {
- singularity_flag=0;
- for(i=0;i<3;i++)
- {
- for(j=0;j<3;j++)
- {
- inv[i][j]/=det;
- }
- }
- }
-}
-float max_array(float arr[3])
-{
- int i;
- float temp_max=fabs(arr[0]);
- for(i=1;i<3;i++)
- {
- if(fabs(arr[i])>temp_max)
- {
- temp_max=fabs(arr[i]);
- }
- }
- return temp_max;
-}
-
-uint8_t singularity_flag_mms=0;
-
-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};
- float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure
float den=0,den2;
float bcopy[3];
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;
- //uint8_t singularity_flag=0;
+ float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure
+
+ int singularity_flag = 0;
+ singularity_flag_mms=0;
if(controlmode1==0)
{
@@ -293,9 +257,10 @@
den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
if (den==0)
{
+ singularity_flag = 1;
singularity_flag_mms=1;
}
- if (singularity_flag_mms==0)
+ if (singularity_flag==0)
{
for(i=0;i<3;i++)
{
@@ -319,7 +284,7 @@
z[i]=db[i]-v[i];
u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
}
- inverse(Jm,invJm,singularity_flag_mms);
+ inversec(Jm,invJm,&singularity_flag);
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
@@ -334,8 +299,8 @@
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[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++)
{
@@ -344,14 +309,13 @@
invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
invJm[0][i]=b[i];
}
- inverse(invJm,Jm,singularity_flag_mms);
-
- //00000
+ inversec(invJm,Jm,&singularity_flag);
+
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)
+ singularity_flag_mms = singularity_flag;
+ if (singularity_flag==0)
{
for(i=0;i<3;i++)
{
@@ -371,14 +335,14 @@
}
}
}
- if (singularity_flag_mms==1)
+ if (singularity_flag==1)
{
for (i=0;i<3;i++)
{
Mmnt[i]=2*MmntMax;
}
}
- ACS_STATUS = 5;
+ ACS_STATUS =5;
}
else if(controlmode1==1)
{
@@ -405,6 +369,51 @@
}
}
+void inversec(float mat[3][3],float inv[3][3],int *singularity_flag)
+{
+ int i,j;
+ float det=0;
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
+ }
+ }
+ det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
+ if (det==0)
+ {
+ *singularity_flag=1;
+ }
+ else
+ {
+ *singularity_flag=0;
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<3;j++)
+ {
+ inv[i][j]/=det;
+ }
+ }
+ }
+}
+
+float max_array(float arr[3])
+{
+ int i;
+ float temp_max=fabs(arr[0]);
+ for(i=1;i<3;i++)
+ {
+ if(fabs(arr[i])>temp_max)
+ {
+ temp_max=fabs(arr[i]);
+ }
+ }
+ return temp_max;
+}
+
+
+
I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
int FCTN_ACS_INIT(); //initialization of registers happens
@@ -444,8 +453,10 @@
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(575);
-
+ wait_ms(275);
+ //kick_WDOG();
+ acs_pc.printf("\n\r lvl5");
+ wait_ms(300);
//Verify magic number
@@ -456,14 +467,14 @@
if(value & 0x02)
{
- printf("Sentral already has eeprom firmware loaded.\n");
+ acs_pc.printf("Sentral already has eeprom firmware loaded.\n");
}
/* Write value 0x01 to the ResetReq register, address 0x9B. This will result
in a hard reset of the Sentral. This is unnecessary if the prior event was
a Reset. */
if(!(value & 0x08))
{
- printf("CPU is not in standby, issuing a shutdown request.\n");
+ acs_pc.printf("CPU is not in standby, issuing a shutdown request.\n");
//i2c_write(I2C_SLAVE_ADDR, 0x34, data, 1);
cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to shut down
cmd[1]=0x00;
@@ -490,12 +501,15 @@
i2c.write(SLAVE_ADDR,cmd,2);
wait_ms(20);
+ //kick_WDOG();
+ acs_pc.printf("\n\r lvl6");
+
cmd[0]=UPLOAD_ADDR; //0x0000 is written in RAM register to enable upload
cmd[1]=0x0000;
i2c.write(SLAVE_ADDR,cmd,3);
wait_ms(100);
- printf("Uploading data...\n");
+ acs_pc.printf("Uploading data...\n");
#define TRASACTION_SIZE 3
@@ -538,12 +552,12 @@
if(actualCRC != EEPROMTextCRC)
{
- pc_acs.printf("Program crc (0x%.8X) does not match CRC reported by Sentral (0x%0.8X)\n", EEPROMTextCRC, actualCRC);
+ acs_pc.printf("Program crc (0x%.8X) does not match CRC reported by Sentral (0x%0.8X)\n", EEPROMTextCRC, actualCRC);
return 0;
}
else
{
- pc_acs.printf("Firmware Upload Complete.\n");
+ acs_pc.printf("Firmware Upload Complete.\n");
return 1;
}
@@ -557,13 +571,13 @@
int SENSOR_INIT()
{
-/// pc_acs.printf("Entered sensor init\n \r");
+/// acs_pc.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
+/// acs_pc.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
if( ack!=0)
{
@@ -574,7 +588,11 @@
return 0;
}
- wait_ms(575);
+
+ wait_ms(275);
+ //kick_WDOG();
+ acs_pc.printf("\n\r lvl2");
+ wait_ms(300);
cmd[0]=SENTRALSTATUS;
ack = i2c.write(SLAVE_ADDR,cmd,1);
@@ -595,7 +613,7 @@
return 0;
}
-/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+/// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
//to check whether EEPROM is uploaded properly
switch((int)store) {
@@ -615,8 +633,10 @@
if(ack!=0)
return 0;
}
- wait_ms(575);//should be 600
-
+ wait_ms(275);//should be 600
+ //kick_WDOG();
+ acs_pc.printf("\n\r lvl3");
+ wait_ms(300);
cmd[0]=SENTRALSTATUS;
ack = i2c.write(SLAVE_ADDR,cmd,1);
if( ack!=0)
@@ -632,7 +652,7 @@
if(ack!=0)
return 0;
}
-/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
+/// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
}
}
@@ -650,9 +670,14 @@
if(ack!=0)
return 0;
}
- wait_ms(575);
+ wait_ms(275);
+ //kick_WDOG();
+ acs_pc.printf("\n\r lvl4");
+ wait_ms(300);
+ timer_SENSOR_INIT.start();
manual = CONFIG_UPLOAD();
+ timer_SENSOR_INIT.stop();
if(manual == 0)
{
@@ -738,16 +763,16 @@
return 0;
}
-/// pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
+/// acs_pc.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");
+/// acs_pc.printf("Exited sensor init successfully\n \r");
return 1;
}
-//// pc_acs.printf("Sensor init failed \n \r") ;
+//// acs_pc.printf("Sensor init failed \n \r") ;
return 0;
}
@@ -755,36 +780,37 @@
{
ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
-
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);
+/// acs_pc.printf("Attitude sensor init called \n \r");
+/// acs_pc.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
{
-/// pc_acs.printf("Sensor 1 marked working \n \r");
+/// acs_pc.printf("Sensor 1 marked working \n \r");
+ timer_SENSOR_INIT.start();
working = SENSOR_INIT();
+ timer_SENSOR_INIT.stop();
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_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
+/// acs_pc.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
+/// acs_pc.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");
+/// acs_pc.printf("Sensor 1 not working. Trying Sensor 2\n \r");
if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
{
@@ -792,11 +818,15 @@
ATS2_SW_ENABLE = 0;
wait_ms(5);
+ timer_SENSOR_INIT.reset();
+ timer_SENSOR_INIT.start();
working = SENSOR_INIT();
+ timer_SENSOR_INIT.stop();
+
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_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+/// acs_pc.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;
@@ -810,8 +840,8 @@
}
-/// 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_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+/// acs_pc.printf("Sensor 2 also not working.Exit init.\n \r");
ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
return 0;
@@ -821,7 +851,7 @@
int SENSOR_DATA_ACQ()
{
//int mag_only=0;
-/// pc_acs.printf("Entering Sensor data acq.\n \r");
+/// acs_pc.printf("Entering Sensor data acq.\n \r");
char status;
int sentral;
int event;
@@ -892,16 +922,18 @@
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);
+/// acs_pc.printf("Event Status is %x\n \r",event);
+/// acs_pc.printf("Sentral Status is %x\n \r",sentral);
- if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )|| (sentral!= 3)) //check for any error in event status register
+ if ( ((event & 0x20 )!= 0x20 ) || ((event & 0x08) != 0x08 ) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 )|| (sentral!= 3)) //check for any error in event status register
{
-
+ timer_SENSOR_INIT.reset();
+ timer_SENSOR_INIT.start();
init = SENSOR_INIT();
+ timer_SENSOR_INIT.stop();
cmd[0]=EVT_STATUS;
ack = i2c.write(SLAVE_ADDR,cmd,1);
@@ -940,10 +972,10 @@
}
sentral = (int)status;
+ int poll_status;
+/// acs_pc.printf("Event Status after resetting and init is %x\n \r",event);
-/// 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
+ if ( ((event & 0x20) != 0x20 ) || ((event & 0x08) != 0x08) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status
{
cmd[0]=ERROR;
@@ -1007,35 +1039,38 @@
{
if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
{
- pc_acs.printf("error in gyro alone..\n \r");
+ acs_pc.printf("error in gyro.\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");
+ acs_pc.printf("error in mag.Exiting.\n \r");
mag_error = 1;
}
if( (gyro_error!=1)&&(mag_error!=1))
{
- pc_acs.printf("error in something else.Exiting.\n \r");
+ acs_pc.printf("error in something else.Exiting.\n \r");
return 0;
}
}
- if((event & 1 == 1 ))
+ if(((event & 1) == 1 ))
{
-/// pc_acs.printf("error in CPU Reset.\n \r");
+/// acs_pc.printf("error in CPU Reset.\n \r");
return 0;
}
-
- if((event & 8 != 8 )||(event & 32 != 32 ))
+ int poll =0;
+ poll_status =1;
+ while((poll<15)&&(((event & 8) != 8 )||((event & 32) != 32 )))
+ //if(((event & 8) != 8 )||((event & 32) != 32 ))
{
- pc_acs.printf("Data not ready waiting...\n \r");
+ poll++;
+ acs_pc.printf("Data not ready waiting...POLL#%d\n \r",poll);
//POLL
wait_ms(200);
@@ -1058,35 +1093,48 @@
}
event = (int)status;
- if(event & 32 != 32 )
+ if((event & 32) != 32 )
{
- pc_acs.printf("Mag data only ready.Read..\n \r");
+ acs_pc.printf("Gyro not ready..\n \r");
gyro_error = 1;
}
+ else
+ {
+ gyro_error = 0;
+ }
- if(event & 8 != 8 )
+ if((event & 8) != 8 )
{
- pc_acs.printf("Both data still not ready.Exiting..\n \r");
+ acs_pc.printf("Mag not ready..\n \r");
mag_error=1;
}
+ else
+ {
+ mag_error=0;
+ }
}
+
+ if(poll!=15)
+ {
+ poll_status = 0;
+ }
}
- if((mag_error !=1)&&(gyro_error!=1))
+ if(((mag_error !=1)&&(gyro_error!=1))&&(poll_status!=0))
{
- pc_acs.printf("Error in something else.Exiting.\n \r");
+ acs_pc.printf("Error in something else.Exiting.\n \r");
return 0;
}
- if((mag_error ==1)&&(gyro_error==1))
+ if(((mag_error ==1)&&(gyro_error==1))&&(poll_status!=0))
{
- pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
+ acs_pc.printf("Error in both gyro and mag.Exiting.\n \r");
return 0;
}
@@ -1107,7 +1155,7 @@
}
- // pc_acs.printf("\nGyro Values:\n");
+ // acs_pc.printf("\nGyro Values:\n");
if (gyro_error!=1)
{
for(int i=0; i<3; i++) {
@@ -1135,16 +1183,16 @@
if(mag_error == 1)
{
- pc_acs.printf("Gyro only successful.\n \r");
+ acs_pc.printf("Gyro only successful.\n \r");
return 1;
}
if(gyro_error == 1)
- {
- pc_acs.printf("Mag only successful.\n \r");
+ {
+ acs_pc.printf("Mag only successful.\n \r");
return 2;
}
- //pc_acs.printf("Reading data success.\n \r");
+ //acs_pc.printf("Reading data success.\n \r");
return 3;
}
@@ -1159,27 +1207,31 @@
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);
+//// acs_pc.printf("DATA_ACQ called \n \r");
+//// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
{
-
+ timer_SENSOR_DATA_ACQ.start();
acq = SENSOR_DATA_ACQ();
+ timer_SENSOR_DATA_ACQ.stop();
+ //pc.pritnf("\n\r timer_SENSOR_DATA_ACQ is %f",timer_SENSOR_DATA_ACQ.read());
+
+
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");
+//// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+//// acs_pc.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");
+ acs_pc.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
@@ -1199,11 +1251,15 @@
ATS2_SW_ENABLE = 0; //switch on sensor 2
wait_ms(5);
-
- init = SENSOR_INIT(); //sensor 2 init
+
+ timer_SENSOR_INIT.reset();
+ timer_SENSOR_INIT.start();
+ init = SENSOR_INIT();
+ timer_SENSOR_INIT.stop();
+ //sensor 2 init
if( init == 0)
{
- pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
+ acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
ATS2_SW_ENABLE = 1;
wait_ms(5);
ATS1_SW_ENABLE = 0;
@@ -1224,7 +1280,7 @@
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
+ acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
return 3;
}
else if(acq2 == 1)
@@ -1252,7 +1308,7 @@
}
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");
+ acs_pc.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;
@@ -1274,7 +1330,7 @@
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");
+ acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
return acq;
}
@@ -1282,7 +1338,7 @@
else if(acq == 0)
{
- pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
+ acs_pc.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;
@@ -1293,7 +1349,7 @@
init = SENSOR_INIT();
if( init == 0)
{
- pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
+ acs_pc.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;
@@ -1304,7 +1360,7 @@
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
+ acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
return 3;
}
else if(acq2 == 1)
@@ -1319,7 +1375,7 @@
}
else if(acq2 == 0)
{
- pc_acs.printf(" Sensor 2 data acq failure..\n \r");
+ acs_pc.printf(" Sensor 2 data acq failure..\n \r");
ATS2_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
@@ -1339,13 +1395,13 @@
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");
+ acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+ acs_pc.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");
+ acs_pc.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
@@ -1368,7 +1424,7 @@
if( init == 0)
{
- pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
+ acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
ATS1_SW_ENABLE = 1;
wait_ms(5);
ATS2_SW_ENABLE = 0;
@@ -1390,7 +1446,7 @@
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
+ acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
return 3;
}
@@ -1419,7 +1475,7 @@
}
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");
+ acs_pc.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;
@@ -1441,14 +1497,14 @@
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");
+ acs_pc.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
+ acs_pc.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;
@@ -1459,7 +1515,7 @@
init = SENSOR_INIT();
if( init == 0)
{
- pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
+ acs_pc.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;
@@ -1470,7 +1526,7 @@
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
+ acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
return 3;
}
else if(acq2 == 1)
@@ -1485,7 +1541,7 @@
}
else if(acq2 == 0)
{
- pc_acs.printf(" Sensor 1 data acq failure..\n \r");
+ acs_pc.printf(" Sensor 1 data acq failure..\n \r");
ATS1_SW_ENABLE = 1;
ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
return 0;
@@ -1493,8 +1549,350 @@
}
}
}
- 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");
+
+
+
+ if(( (ACS_ATS_STATUS & 0xC0) == 0x00))
+ {
+
+ ATS2_SW_ENABLE = 0;
+ wait_ms(10);
+ 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
+//// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+//// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
+ return 3;
+ }
+ else if((acq == 2)||(acq==1))
+ {
+ acs_pc.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)
+ {
+ acs_pc.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;
+ acs_pc.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
+ {
+ acs_pc.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;
+ }
+ acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
+ return acq;
+
+ }
+ }
+
+ else if(acq == 0)
+ {
+ acs_pc.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)
+ {
+ acs_pc.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;
+ acs_pc.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)
+ {
+ acs_pc.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) == 0x00))
+ {
+ ATS2_SW_ENABLE = 0;
+ wait_ms(10);
+ acq = SENSOR_DATA_ACQ(); //make ATS2 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;
+ acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+ acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
+ return 3;
+ }
+ else if((acq == 2)||(acq==1)) //Only mag or only gyro
+ {
+ acs_pc.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)
+ {
+ acs_pc.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;
+ acs_pc.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
+ {
+ acs_pc.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;
+ }
+ acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
+ return acq;
+
+ }
+ }
+ else if(acq == 0)
+ {
+ acs_pc.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)
+ {
+ acs_pc.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;
+ acs_pc.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)
+ {
+ acs_pc.printf(" Sensor 1 data acq failure..\n \r");
+ ATS1_SW_ENABLE = 1;
+ ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
+ return 0;
+ }
+ }
+ }
+ }
+
+
+
+
+
+ acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
+ acs_pc.printf(" Both sensors data acq failure.Exiting.\n \r");
return 0;
}
@@ -1559,7 +1957,7 @@
{
g_err_flag_TR_x = 1;
}
- pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
+ acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x);
//------------------------------------- y-direction TR--------------------------------------//
@@ -1609,7 +2007,7 @@
{
g_err_flag_TR_y = 1;
}
- pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
+ acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
//----------------------------------------------- z-direction TR -------------------------//
@@ -1659,7 +2057,7 @@
{
g_err_flag_TR_z = 1;
}
- pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
+ acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
//changed
if(phase_TR_x)