BAE_QM_MAR9

Team Fox » Code » BAE_QM_MAR9

Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Dependencies: FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

ACS.cpp@34:1b41c34b12ea, 2016-07-04 (annotated)

Committer:: Bragadeesh153
Date:: Mon Jul 04 07:01:26 2016 +0000
Revision:: 34:1b41c34b12ea
Parent:: 20:949d13045431
Child:: 35:7193e581932f

UPDATED SCZ_ANGLE and COMMISSIONING

Who changed what in which revision?

User	Revision	Line number	New contents of line
sakthipriya	0:7b4c00e3912f	1	/*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya	0:7b4c00e3912f	2	-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya	0:7b4c00e3912f	3	#include <mbed.h>
sakthipriya	0:7b4c00e3912f	4	#include <math.h>
sakthipriya	0:7b4c00e3912f	5
sakthipriya	0:7b4c00e3912f	6	#include "pni.h" //pni header file
sakthipriya	0:7b4c00e3912f	7	#include "pin_config.h"
sakthipriya	0:7b4c00e3912f	8	#include "ACS.h"
sakthipriya	6:036d08b62785	9	#include "EPS.h"
lakshya	20:949d13045431	10	/*variables will get get updated value from FLash
lakshya	20:949d13045431	11	in case flash cups while testing i.e initial defaul values are kept as of now
lakshya	20:949d13045431	12	*/
sakthipriya	0:7b4c00e3912f	13	//******************************flags****************************************//
sakthipriya	0:7b4c00e3912f	14	extern uint32_t BAE_STATUS;
sakthipriya	0:7b4c00e3912f	15	extern uint32_t BAE_ENABLE;
Bragadeesh153	13:fb7facaf308b	16	extern uint8_t ACS_INIT_STATUS;
Bragadeesh153	13:fb7facaf308b	17	extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153	13:fb7facaf308b	18	extern uint8_t ACS_ATS_STATUS;
Bragadeesh153	13:fb7facaf308b	19	extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153	13:fb7facaf308b	20	extern uint8_t ACS_STATUS;
lakshya	20:949d13045431	21	extern uint8_t ACS_DETUMBLING_ALGO_TYPE;//////
lakshya	20:949d13045431	22
lakshya	20:949d13045431	23	extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
lakshya	20:949d13045431	24	extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
sakthipriya	0:7b4c00e3912f	25
Bragadeesh153	13:fb7facaf308b	26	extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153	13:fb7facaf308b	27	extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153	13:fb7facaf308b	28	extern uint8_t ACS_STATE;
sakthipriya	0:7b4c00e3912f	29
sakthipriya	0:7b4c00e3912f	30	DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya	0:7b4c00e3912f	31	DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya	0:7b4c00e3912f	32	DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya	0:7b4c00e3912f	33
sakthipriya	0:7b4c00e3912f	34	extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya	0:7b4c00e3912f	35	extern PwmOut PWM2; //y
sakthipriya	0:7b4c00e3912f	36	extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya	0:7b4c00e3912f	37
sakthipriya	0:7b4c00e3912f	38	int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya	0:7b4c00e3912f	39	int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya	0:7b4c00e3912f	40	int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya	0:7b4c00e3912f	41
sakthipriya	0:7b4c00e3912f	42	extern float data[6];
sakthipriya	6:036d08b62785	43	extern BAE_HK_actual actual_data;
sakthipriya	0:7b4c00e3912f	44
lakshya	20:949d13045431	45	//global para
lakshya	20:949d13045431	46	//FUNCTION
lakshya	20:949d13045431	47	float max_invjm [9]= {1.0000,1.0000,1.0000,0.0471,4.6159,4.1582,4.4047,0.0755,4.1582};
lakshya	20:949d13045431	48	float min_invjm[9] = {-1.0000,-1.0000,-1.0000,-0.0471,-4.6159,-4.1582,-4.4047,-0.0755,-4.1582};
lakshya	20:949d13045431	49	float max_jm[9] = {0.3755,0.0176,0.2672,0.4895,0.2174,0.0452,1.0000,0.1209,0.0572};
lakshya	20:949d13045431	50	float min_jm[9] = {-0.2491,-0.0457,0.2271,0.1556,0.2222,0.0175,0.9998,0.0361,0.0922};
lakshya	20:949d13045431	51	//se some other better way
lakshya	20:949d13045431	52	/*
lakshya	20:949d13045431	53	float max_bb[3] = {0,1.0e-040.1633,1.0e-040.1528};
lakshya	20:949d13045431	54	float min_bb[3] = {0,1.0e-04(-0.1736),1.0e-04(-0.1419)};
lakshya	20:949d13045431	55	*/
lakshya	20:949d13045431	56	float max_bb[3] = {0,1.00.00010.1633,1.00.00010.1528};
lakshya	20:949d13045431	57	float min_bb[3] = {0,1.00.0001(-0.1736),1.00.0001(-0.1419)};
sakthipriya	0:7b4c00e3912f	58
lakshya	20:949d13045431	59	//ACS
lakshya	20:949d13045431	60	uint8_t controlmode_mms = 0;
lakshya	20:949d13045431	61	uint8_t ATS1_EVENT_STATUS_RGTR=0x00;
lakshya	20:949d13045431	62	uint8_t ATS1_SENTRAL_STATUS_RGTR=0x00;
lakshya	20:949d13045431	63	uint8_t ATS1_ERROR_RGTR=0x00;
lakshya	20:949d13045431	64	uint8_t ATS2_EVENT_STATUS_RGTR=0x00;
lakshya	20:949d13045431	65	uint8_t ATS2_SENTRAL_STATUS_RGTR=0x00;
lakshya	20:949d13045431	66	uint8_t ATS2_ERROR_RGTR=0x00;
lakshya	20:949d13045431	67	uint8_t invjm_mms[9];
lakshya	20:949d13045431	68	uint8_t jm_mms[9];
lakshya	20:949d13045431	69	uint8_t bb_mms[3];
lakshya	20:949d13045431	70	uint8_t singularity_flag=0;
Bragadeesh153	34:1b41c34b12ea	71	uint8_t B_SCZ_ANGLE = 0x00;
lakshya	20:949d13045431	72
lakshya	20:949d13045431	73	uint8_t ACS_MAG_TIME_DELAY;// = 65;
lakshya	20:949d13045431	74	uint8_t ACS_DEMAG_TIME_DELAY;// = 65;
lakshya	20:949d13045431	75	uint16_t ACS_Z_FIXED_MOMENT;// = 1.3;
lakshya	20:949d13045431	76	uint8_t ACS_TR_Z_SW_STATUS;//=1;
lakshya	20:949d13045431	77	uint8_t ACS_TR_XY_SW_STATUS;//=1;
lakshya	20:949d13045431	78	//GLOBAL PARA
lakshya	20:949d13045431	79	uint8_t ACS_TR_X_PWM; //*
lakshya	20:949d13045431	80	uint8_t ACS_TR_Y_PWM; //*
lakshya	20:949d13045431	81	uint8_t ACS_TR_Z_PWM; //*
lakshya	20:949d13045431	82	//change
lakshya	20:949d13045431	83	uint16_t ACS_MM_X_COMSN = 1;
lakshya	20:949d13045431	84	uint16_t ACS_MM_Y_COMSN = 1;
lakshya	20:949d13045431	85	uint16_t ACS_MG_X_COMSN = 1;
lakshya	20:949d13045431	86	uint16_t ACS_MG_Y_COMSN = 1;
lakshya	20:949d13045431	87	uint16_t ACS_MM_Z_COMSN = 1;
lakshya	20:949d13045431	88	uint16_t ACS_MG_Z_COMSN = 1;
lakshya	20:949d13045431	89
lakshya	20:949d13045431	90	uint8_t float_to_uint8(float min,float max,float val)
lakshya	20:949d13045431	91	{
lakshya	20:949d13045431	92	if(val>max)
lakshya	20:949d13045431	93	{return 0xff;
lakshya	20:949d13045431	94	}
lakshya	20:949d13045431	95	if(val<min)
lakshya	20:949d13045431	96	{return 0x00;
lakshya	20:949d13045431	97	}
lakshya	20:949d13045431	98	float div=max-min;div=(255.0/div);val=((val-min)*div);
lakshya	20:949d13045431	99	return (uint8_t)val;
lakshya	20:949d13045431	100	}
lakshya	20:949d13045431	101
lakshya	20:949d13045431	102
lakshya	20:949d13045431	103	void float_to_uint8_ARRAY(int d1,int d2, float arr,float max[], float min[], uint8_t valarr)
lakshya	20:949d13045431	104	{
lakshya	20:949d13045431	105	for(int i=0;i<d1;i++)
lakshya	20:949d13045431	106	for(int j=0;j<d2;j++)
lakshya	20:949d13045431	107	{
lakshya	20:949d13045431	108	printf("\n\r%f",((arr+(id1))+j));
lakshya	20:949d13045431	109	valarr[id1+j] = (uint8_t)float_to_uint8(min[id1+j],max[id1+j],((arr+(i*d1))+j));
lakshya	20:949d13045431	110	printf("\n\r%d",valarr[i*d1+j]);
lakshya	20:949d13045431	111	}
lakshya	20:949d13045431	112	}
lakshya	20:949d13045431	113
sakthipriya	0:7b4c00e3912f	114
sakthipriya	0:7b4c00e3912f	115
sakthipriya	0:7b4c00e3912f	116	Serial pc_acs(USBTX,USBRX); //for usb communication
lakshya	20:949d13045431	117
lakshya	10:f93407b97750	118	//CONTROL_ALGO
lakshya	20:949d13045431	119	float moment[3]; // Unit: AmpereMeter^2//
lakshya	10:f93407b97750	120	float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
lakshya	20:949d13045431	121	float db[3];//*
lakshya	20:949d13045431	122	uint8_t flag_firsttime=1, alarmmode=0;
lakshya	10:f93407b97750	123
lakshya	10:f93407b97750	124
lakshya	20:949d13045431	125	void controlmodes(float b[3], float db[3], float omega[3], uint8_t controlmode1); //*
lakshya	20:949d13045431	126	float max_array(float arr[3]);
sakthipriya	0:7b4c00e3912f	127	void inverse(float mat[3][3],float inv[3][3]);
lakshya	10:f93407b97750	128
lakshya	10:f93407b97750	129	//CONTROLALGO PARAMETERS
lakshya	20:949d13045431	130	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)
sakthipriya	0:7b4c00e3912f	131	{
lakshya	20:949d13045431	132
lakshya	10:f93407b97750	133	float normalising_fact;
lakshya	20:949d13045431	134	float b_copy[3], omega_copy[3], db_copy[3];
lakshya	10:f93407b97750	135	int i;
lakshya	10:f93407b97750	136	if(flag_firsttime==1)
lakshya	10:f93407b97750	137	{
lakshya	10:f93407b97750	138	for(i=0;i<3;i++)
lakshya	10:f93407b97750	139	{
lakshya	20:949d13045431	140	db[i]=0; // Unit: Tesla/Second
lakshya	10:f93407b97750	141	}
lakshya	10:f93407b97750	142	flag_firsttime=0;
lakshya	10:f93407b97750	143	}
sakthipriya	0:7b4c00e3912f	144	else
sakthipriya	0:7b4c00e3912f	145	{
sakthipriya	0:7b4c00e3912f	146	for(i=0;i<3;i++)
sakthipriya	0:7b4c00e3912f	147	{
lakshya	20:949d13045431	148	db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya	0:7b4c00e3912f	149	}
lakshya	10:f93407b97750	150	}
lakshya	10:f93407b97750	151
lakshya	20:949d13045431	152	if(nominal == 0)
lakshya	20:949d13045431	153
lakshya	20:949d13045431	154	{
lakshya	20:949d13045431	155
lakshya	20:949d13045431	156	if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
lakshya	20:949d13045431	157	{
lakshya	20:949d13045431	158	alarmmode=0;
lakshya	20:949d13045431	159	}
lakshya	20:949d13045431	160	else if(max_array(omega)>OmegaMax&& alarmmode==0)
lakshya	20:949d13045431	161	{
lakshya	20:949d13045431	162	alarmmode=1;
lakshya	20:949d13045431	163	}
lakshya	20:949d13045431	164
lakshya	20:949d13045431	165	}
lakshya	20:949d13045431	166
lakshya	10:f93407b97750	167	for (i=0;i<3;i++)
sakthipriya	0:7b4c00e3912f	168	{
lakshya	20:949d13045431	169	b_copy[i]=b[i];
lakshya	20:949d13045431	170	db_copy[i]=db[i];
lakshya	20:949d13045431	171	omega_copy[i]=omega[i];
sakthipriya	0:7b4c00e3912f	172	}
lakshya	10:f93407b97750	173
lakshya	20:949d13045431	174	if(((alarmmode==0)\|\| (nominal == 1))&&(detumbling==0))
lakshya	10:f93407b97750	175	{
lakshya	20:949d13045431	176	controlmode_mms = 0;
lakshya	20:949d13045431	177	controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE);
lakshya	20:949d13045431	178	for (i=0;i<3;i++)
lakshya	20:949d13045431	179	{
lakshya	20:949d13045431	180	b[i]=b_copy[i];
lakshya	20:949d13045431	181	db[i]=db_copy[i];
lakshya	20:949d13045431	182	omega[i]=omega_copy[i];
lakshya	20:949d13045431	183	}
lakshya	10:f93407b97750	184	if(max_array(moment)>MmntMax)
lakshya	10:f93407b97750	185	{
lakshya	20:949d13045431	186	controlmode_mms = 1;
lakshya	20:949d13045431	187	controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
lakshya	20:949d13045431	188	for (i=0;i<3;i++)
lakshya	20:949d13045431	189	{
lakshya	20:949d13045431	190	b[i]=b_copy[i];
lakshya	20:949d13045431	191	db[i]=db_copy[i];
lakshya	20:949d13045431	192	omega[i]=omega_copy[i];
lakshya	20:949d13045431	193	}
lakshya	10:f93407b97750	194	if(max_array(moment)>MmntMax)
lakshya	10:f93407b97750	195	{
lakshya	10:f93407b97750	196	normalising_fact=max_array(moment)/MmntMax;
lakshya	10:f93407b97750	197	for(i=0;i<3;i++)
lakshya	20:949d13045431	198	{
lakshya	10:f93407b97750	199	moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya	20:949d13045431	200	}
lakshya	10:f93407b97750	201	}
lakshya	10:f93407b97750	202	}
lakshya	20:949d13045431	203	ACS_STATUS = 5;//*is this changed now
lakshya	10:f93407b97750	204	}
lakshya	10:f93407b97750	205	else
lakshya	10:f93407b97750	206	{
lakshya	20:949d13045431	207	controlmode_mms = 1;
lakshya	20:949d13045431	208	controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
lakshya	20:949d13045431	209	for (i=0;i<3;i++)
lakshya	20:949d13045431	210	{
lakshya	20:949d13045431	211	b[i]=b_copy[i];
lakshya	20:949d13045431	212	db[i]=db_copy[i];
lakshya	20:949d13045431	213	omega[i]=omega_copy[i];
lakshya	20:949d13045431	214	}
lakshya	10:f93407b97750	215	if(max_array(moment)>MmntMax)
lakshya	10:f93407b97750	216	{
lakshya	10:f93407b97750	217	normalising_fact=max_array(moment)/MmntMax;
lakshya	10:f93407b97750	218	for(i=0;i<3;i++)
lakshya	20:949d13045431	219	{
lakshya	10:f93407b97750	220	moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya	20:949d13045431	221	}
lakshya	10:f93407b97750	222	}
lakshya	10:f93407b97750	223
lakshya	10:f93407b97750	224	}
lakshya	10:f93407b97750	225	for (i=0;i<3;i++)
sakthipriya	0:7b4c00e3912f	226	{
lakshya	20:949d13045431	227	b_old[i]=b[i];
sakthipriya	0:7b4c00e3912f	228	}
sakthipriya	0:7b4c00e3912f	229	}
lakshya	10:f93407b97750	230
lakshya	20:949d13045431	231	void inverse(float mat[3][3],float inv[3][3],uint8_t &singularity_flag)
sakthipriya	0:7b4c00e3912f	232	{
sakthipriya	0:7b4c00e3912f	233	int i,j;
sakthipriya	0:7b4c00e3912f	234	float det=0;
sakthipriya	0:7b4c00e3912f	235	for(i=0;i<3;i++)
sakthipriya	0:7b4c00e3912f	236	{
sakthipriya	0:7b4c00e3912f	237	for(j=0;j<3;j++)
lakshya	10:f93407b97750	238	{
sakthipriya	0:7b4c00e3912f	239	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]);
lakshya	10:f93407b97750	240	}
sakthipriya	0:7b4c00e3912f	241	}
sakthipriya	0:7b4c00e3912f	242	det+=(mat[0][0]inv[0][0])+(mat[0][1]inv[1][0])+(mat[0][2]*inv[2][0]);
lakshya	20:949d13045431	243	if (det==0)
lakshya	20:949d13045431	244	{
lakshya	20:949d13045431	245	singularity_flag=1;
lakshya	20:949d13045431	246	}
lakshya	20:949d13045431	247	else
lakshya	10:f93407b97750	248	{
lakshya	20:949d13045431	249	singularity_flag=0;
lakshya	20:949d13045431	250	for(i=0;i<3;i++)
lakshya	10:f93407b97750	251	{
lakshya	20:949d13045431	252	for(j=0;j<3;j++)
lakshya	20:949d13045431	253	{
lakshya	20:949d13045431	254	inv[i][j]/=det;
lakshya	20:949d13045431	255	}
lakshya	10:f93407b97750	256	}
sakthipriya	0:7b4c00e3912f	257	}
sakthipriya	0:7b4c00e3912f	258	}
sakthipriya	0:7b4c00e3912f	259
lakshya	10:f93407b97750	260	float max_array(float arr[3])
lakshya	10:f93407b97750	261	{
lakshya	10:f93407b97750	262	int i;
lakshya	10:f93407b97750	263	float temp_max=fabs(arr[0]);
lakshya	10:f93407b97750	264	for(i=1;i<3;i++)
lakshya	10:f93407b97750	265	{
lakshya	10:f93407b97750	266	if(fabs(arr[i])>temp_max)
lakshya	10:f93407b97750	267	{
lakshya	10:f93407b97750	268	temp_max=fabs(arr[i]);
lakshya	10:f93407b97750	269	}
lakshya	10:f93407b97750	270	}
lakshya	10:f93407b97750	271	return temp_max;
lakshya	10:f93407b97750	272	}
lakshya	10:f93407b97750	273
lakshya	20:949d13045431	274	uint8_t singularity_flag_mms=0;
lakshya	10:f93407b97750	275
lakshya	20:949d13045431	276	void controlmodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
lakshya	10:f93407b97750	277	{
lakshya	10:f93407b97750	278	float bb[3]={0,0,0};
lakshya	10:f93407b97750	279	float d[3]={0,0,0};
lakshya	10:f93407b97750	280	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
lakshya	10:f93407b97750	281	float den=0,den2;
lakshya	10:f93407b97750	282	float bcopy[3];
lakshya	10:f93407b97750	283	int i, j;//temporary variables
lakshya	10:f93407b97750	284	float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya	10:f93407b97750	285	float invJm[3][3];
lakshya	20:949d13045431	286	float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
lakshya	20:949d13045431	287	//uint8_t singularity_flag=0;
lakshya	10:f93407b97750	288
lakshya	10:f93407b97750	289	if(controlmode1==0)
lakshya	10:f93407b97750	290	{
lakshya	10:f93407b97750	291	den=sqrt((b[0]b[0])+(b[1]b[1])+(b[2]*b[2]));
lakshya	10:f93407b97750	292	den2=(b[0]db[0])+(b[1]db[1])+(b[2]*db[2]);
lakshya	20:949d13045431	293	if (den==0)
lakshya	10:f93407b97750	294	{
lakshya	20:949d13045431	295	singularity_flag_mms=1;
lakshya	10:f93407b97750	296	}
lakshya	20:949d13045431	297	if (singularity_flag_mms==0)
lakshya	10:f93407b97750	298	{
lakshya	20:949d13045431	299	for(i=0;i<3;i++)
lakshya	20:949d13045431	300	{
lakshya	20:949d13045431	301	db[i]=((db[i]denden)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
lakshya	20:949d13045431	302	}
lakshya	20:949d13045431	303	for(i=0;i<3;i++)
lakshya	20:949d13045431	304	{
lakshya	20:949d13045431	305	b[i]/=den; // Mormalized b. Hence no unit.
lakshya	20:949d13045431	306	}
lakshya	20:949d13045431	307	if(b[2]>0.9 \|\| b[2]<-0.9)
lakshya	20:949d13045431	308	{
lakshya	20:949d13045431	309	kz=kz2;
lakshya	20:949d13045431	310	kmu=kmu2;
lakshya	20:949d13045431	311	gamma=gamma2;
lakshya	20:949d13045431	312	}
lakshya	20:949d13045431	313	for(i=0;i<2;i++)
lakshya	20:949d13045431	314	{
lakshya	20:949d13045431	315	Mu[i]=b[i];
lakshya	20:949d13045431	316	v[i]=-kmu*Mu[i];
lakshya	20:949d13045431	317	dv[i]=-kmu*db[i];
lakshya	20:949d13045431	318	z[i]=db[i]-v[i];
lakshya	20:949d13045431	319	u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
lakshya	20:949d13045431	320	}
lakshya	20:949d13045431	321	inverse(Jm,invJm,singularity_flag_mms);
lakshya	20:949d13045431	322	for(i=0;i<3;i++)
lakshya	20:949d13045431	323	{
lakshya	20:949d13045431	324	for(j=0;j<3;j++)
lakshya	20:949d13045431	325	{
lakshya	20:949d13045431	326	bb[i]+=omega[j](omega[(i+1)%3]Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
lakshya	20:949d13045431	327	}
lakshya	20:949d13045431	328	}
lakshya	20:949d13045431	329	for(i=0;i<3;i++)
lakshya	10:f93407b97750	330	{
lakshya	20:949d13045431	331	for(j=0;j<3;j++)
lakshya	20:949d13045431	332	{
lakshya	20:949d13045431	333	d[i]+=bb[j]*invJm[i][j];
lakshya	20:949d13045431	334	}
lakshya	20:949d13045431	335	}
lakshya	20:949d13045431	336	bb[1]=u[0]-(d[1]b[2])+(d[2]b[1])-(omega[1]db[2])+(omega[2]db[1]);
lakshya	20:949d13045431	337	bb[2]=u[1]-(d[2]b[0])+(d[0]b[2])-(omega[2]db[0])+(omega[0]db[2]);
lakshya	20:949d13045431	338	bb[0]=0;
lakshya	20:949d13045431	339	for(i=0;i<3;i++)
lakshya	20:949d13045431	340	{
lakshya	20:949d13045431	341	d[i]=invJm[2][i];
lakshya	20:949d13045431	342	invJm[1][i]=-b[2]invJm[1][i]+b[1]d[i];
lakshya	20:949d13045431	343	invJm[2][i]=b[2]invJm[0][i]-b[0]d[i];
lakshya	20:949d13045431	344	invJm[0][i]=b[i];
lakshya	10:f93407b97750	345	}
lakshya	20:949d13045431	346	inverse(invJm,Jm,singularity_flag_mms);
lakshya	20:949d13045431	347
lakshya	20:949d13045431	348	//00000
lakshya	20:949d13045431	349	float_to_uint8_ARRAY(3,3, (float*)invJm,max_invjm, min_invjm, invjm_mms);
lakshya	20:949d13045431	350	float_to_uint8_ARRAY(3,3, (float*)Jm,max_jm, min_jm, jm_mms);
lakshya	20:949d13045431	351	float_to_uint8_ARRAY(1,3, (float*)bb,max_bb, min_bb, bb_mms);
lakshya	20:949d13045431	352
lakshya	20:949d13045431	353	if (singularity_flag_mms==0)
lakshya	10:f93407b97750	354	{
lakshya	20:949d13045431	355	for(i=0;i<3;i++)
lakshya	20:949d13045431	356	{
lakshya	20:949d13045431	357	for(j=0;j<3;j++)
lakshya	20:949d13045431	358	{
lakshya	20:949d13045431	359	tauc[i]+=Jm[i][j]bb[j]; // Unit: NewtonMeter^2
lakshya	20:949d13045431	360	}
lakshya	20:949d13045431	361	}
lakshya	20:949d13045431	362	for(i=0;i<3;i++)
lakshya	20:949d13045431	363	{
lakshya	20:949d13045431	364	bcopy[i]=b[i]*den;
lakshya	20:949d13045431	365	}
lakshya	20:949d13045431	366	for(i=0;i<3;i++)
lakshya	20:949d13045431	367	{
lakshya	20:949d13045431	368	Mmnt[i]=bcopy[(i+1)%3]tauc[(i+2)%3]-bcopy[(i+2)%3]tauc[(i+1)%3];
lakshya	20:949d13045431	369	Mmnt[i]/=(denden); // Unit: AmpereMeter^2
lakshya	20:949d13045431	370	}
lakshya	10:f93407b97750	371	}
lakshya	10:f93407b97750	372	}
lakshya	20:949d13045431	373	if (singularity_flag_mms==1)
lakshya	10:f93407b97750	374	{
lakshya	20:949d13045431	375	for (i=0;i<3;i++)
lakshya	10:f93407b97750	376	{
lakshya	20:949d13045431	377	Mmnt[i]=2*MmntMax;
lakshya	10:f93407b97750	378	}
lakshya	10:f93407b97750	379	}
lakshya	20:949d13045431	380	ACS_STATUS = 5;
lakshya	10:f93407b97750	381	}
lakshya	10:f93407b97750	382	else if(controlmode1==1)
lakshya	10:f93407b97750	383	{
lakshya	20:949d13045431	384	if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
lakshya	10:f93407b97750	385	{
lakshya	20:949d13045431	386	for(i=0;i<3;i++)
lakshya	20:949d13045431	387	{
lakshya	20:949d13045431	388	Mmnt[i]=-kdetumble(b[(i+1)%3]omega[(i+2)%3]-b[(i+2)%3]omega[(i+1)%3]); // Unit: AmpereMeter^2
lakshya	20:949d13045431	389	}
lakshya	20:949d13045431	390	ACS_STATUS = 6;
lakshya	20:949d13045431	391	}
lakshya	20:949d13045431	392	else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
lakshya	20:949d13045431	393	{
lakshya	20:949d13045431	394	for(i=0;i<3;i++)
lakshya	20:949d13045431	395	{
lakshya	20:949d13045431	396	Mmnt[i]=-kdetumble*db[i];
lakshya	20:949d13045431	397	}
lakshya	20:949d13045431	398	ACS_STATUS = 4;
lakshya	10:f93407b97750	399	}
lakshya	10:f93407b97750	400	}
lakshya	10:f93407b97750	401	for(i=0;i<3;i++)
lakshya	10:f93407b97750	402	{
lakshya	10:f93407b97750	403	moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya	10:f93407b97750	404	}
lakshya	10:f93407b97750	405	}
sakthipriya	0:7b4c00e3912f	406
sakthipriya	0:7b4c00e3912f	407	I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya	0:7b4c00e3912f	408
lakshya	20:949d13045431	409	int FCTN_ACS_INIT(); //initialization of registers happens
lakshya	20:949d13045431	410	int SENSOR_INIT();
lakshya	20:949d13045431	411	int FCTN_ATS_DATA_ACQ(); //data is obtained
lakshya	20:949d13045431	412	int SENSOR_DATA_ACQ();
lakshya	20:949d13045431	413	//void T_OUT(); //timeout function to stop infinite loop
lakshya	20:949d13045431	414
lakshya	20:949d13045431	415	int CONFIG_UPLOAD();
lakshya	20:949d13045431	416	//Timeout to; //Timeout variable to
sakthipriya	0:7b4c00e3912f	417	int toFlag;
sakthipriya	0:7b4c00e3912f	418
sakthipriya	0:7b4c00e3912f	419	int count =0; // Time for which the BAE uC is running (in seconds)
lakshya	20:949d13045431	420	//void T_OUT()
lakshya	20:949d13045431	421	//{
lakshya	20:949d13045431	422	// toFlag=0; //as T_OUT function gets called the while loop gets terminated
lakshya	20:949d13045431	423	//}
sakthipriya	0:7b4c00e3912f	424
sakthipriya	0:7b4c00e3912f	425
sakthipriya	0:7b4c00e3912f	426	//DEFINING VARIABLES
sakthipriya	0:7b4c00e3912f	427	char cmd[2];
sakthipriya	0:7b4c00e3912f	428	char raw_gyro[6];
sakthipriya	0:7b4c00e3912f	429	char raw_mag[6];
lakshya	20:949d13045431	430	char reg_data[24];
sakthipriya	0:7b4c00e3912f	431	char store,status;
lakshya	20:949d13045431	432	//int16_t bit_data done in actual_data structure itself;
sakthipriya	0:7b4c00e3912f	433
lakshya	20:949d13045431	434	uint16_t time_data;
lakshya	20:949d13045431	435	float gyro_data[3], mag_data[3];
lakshya	20:949d13045431	436	//float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
lakshya	20:949d13045431	437
lakshya	20:949d13045431	438	int ack;
lakshya	20:949d13045431	439	int CONFIG_UPLOAD()
lakshya	20:949d13045431	440	{
lakshya	20:949d13045431	441	cmd[0]=RESETREQ;
sakthipriya	0:7b4c00e3912f	442	cmd[1]=BIT_RESREQ;
sakthipriya	0:7b4c00e3912f	443	i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
lakshya	20:949d13045431	444	wait_ms(575);
lakshya	20:949d13045431	445
lakshya	20:949d13045431	446	//Verify magic number
lakshya	20:949d13045431	447
lakshya	20:949d13045431	448	cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
lakshya	20:949d13045431	449	cmd[1]=BIT_HOST_UPLD_ENB;
lakshya	20:949d13045431	450	i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	451	wait_ms(20);
lakshya	20:949d13045431	452
lakshya	20:949d13045431	453	cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
lakshya	20:949d13045431	454	cmd[1]=0x0000;
lakshya	20:949d13045431	455	i2c.write(SLAVE_ADDR,cmd,3);
lakshya	20:949d13045431	456	wait_ms(20);
lakshya	20:949d13045431	457
lakshya	20:949d13045431	458	cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
lakshya	20:949d13045431	459	cmd[1]=0x00;
lakshya	20:949d13045431	460	i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	461	wait_ms(20);
lakshya	20:949d13045431	462
lakshya	20:949d13045431	463	return 0;
lakshya	20:949d13045431	464	}
lakshya	20:949d13045431	465
lakshya	20:949d13045431	466	int SENSOR_INIT()
lakshya	20:949d13045431	467	{
lakshya	20:949d13045431	468	/// pc_acs.printf("Entered sensor init\n \r");
lakshya	20:949d13045431	469	cmd[0]=RESETREQ;
lakshya	20:949d13045431	470	cmd[1]=BIT_RESREQ;
lakshya	20:949d13045431	471	ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
lakshya	20:949d13045431	472	//wait_ms(575); //waiting for loading configuration file stored in EEPROM
lakshya	20:949d13045431	473
lakshya	20:949d13045431	474	/// pc_acs.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
lakshya	20:949d13045431	475
lakshya	20:949d13045431	476	if( ack!=0)
lakshya	20:949d13045431	477	{
lakshya	20:949d13045431	478	cmd[0]=RESETREQ;
lakshya	20:949d13045431	479	cmd[1]=BIT_RESREQ;
lakshya	20:949d13045431	480	ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
lakshya	20:949d13045431	481	if(ack !=0)
lakshya	20:949d13045431	482	return 0;
lakshya	20:949d13045431	483	}
lakshya	20:949d13045431	484
lakshya	20:949d13045431	485	wait_ms(575);
lakshya	20:949d13045431	486
sakthipriya	0:7b4c00e3912f	487	cmd[0]=SENTRALSTATUS;
lakshya	20:949d13045431	488	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	489
lakshya	20:949d13045431	490	if( ack!=0)
lakshya	20:949d13045431	491	{
lakshya	20:949d13045431	492	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	493	if(ack!=0)
lakshya	20:949d13045431	494	return 0;
lakshya	20:949d13045431	495	}
lakshya	20:949d13045431	496
lakshya	20:949d13045431	497	ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	498
lakshya	20:949d13045431	499	if( ack!=0)
lakshya	20:949d13045431	500	{
lakshya	20:949d13045431	501	ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	502	if(ack!=0)
lakshya	20:949d13045431	503	return 0;
lakshya	20:949d13045431	504	}
lakshya	20:949d13045431	505
lakshya	20:949d13045431	506	/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
lakshya	20:949d13045431	507
lakshya	20:949d13045431	508	//to check whether EEPROM is uploaded properly
sakthipriya	0:7b4c00e3912f	509	switch((int)store) {
lakshya	20:949d13045431	510	case(3): {
sakthipriya	0:7b4c00e3912f	511	break;
sakthipriya	0:7b4c00e3912f	512	}
sakthipriya	0:7b4c00e3912f	513	case(11): {
sakthipriya	0:7b4c00e3912f	514	break;
sakthipriya	0:7b4c00e3912f	515	}
sakthipriya	0:7b4c00e3912f	516	default: {
sakthipriya	0:7b4c00e3912f	517	cmd[0]=RESETREQ;
sakthipriya	0:7b4c00e3912f	518	cmd[1]=BIT_RESREQ;
lakshya	20:949d13045431	519	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	520	if( ack!=0)
lakshya	20:949d13045431	521	{
lakshya	20:949d13045431	522	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	523	if(ack!=0)
lakshya	20:949d13045431	524	return 0;
lakshya	20:949d13045431	525	}
lakshya	20:949d13045431	526	wait_ms(575);//should be 600
lakshya	20:949d13045431	527
lakshya	20:949d13045431	528	cmd[0]=SENTRALSTATUS;
lakshya	20:949d13045431	529	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	530	if( ack!=0)
lakshya	20:949d13045431	531	{
lakshya	20:949d13045431	532	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	533	if(ack!=0)
lakshya	20:949d13045431	534	return 0;
lakshya	20:949d13045431	535	}
lakshya	20:949d13045431	536	ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	537	if( ack!=0)
lakshya	20:949d13045431	538	{
lakshya	20:949d13045431	539	ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	540	if(ack!=0)
lakshya	20:949d13045431	541	return 0;
lakshya	20:949d13045431	542	}
lakshya	20:949d13045431	543	/// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
lakshya	20:949d13045431	544
sakthipriya	0:7b4c00e3912f	545	}
sakthipriya	0:7b4c00e3912f	546	}
lakshya	20:949d13045431	547
lakshya	20:949d13045431	548	int manual=0;
lakshya	20:949d13045431	549	if( ((int)store != 11 )&&((int)store != 3))
lakshya	20:949d13045431	550	{
lakshya	20:949d13045431	551
lakshya	20:949d13045431	552	cmd[0]=RESETREQ;
lakshya	20:949d13045431	553	cmd[1]=BIT_RESREQ;
lakshya	20:949d13045431	554	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	555	if( ack!=0)
lakshya	20:949d13045431	556	{
lakshya	20:949d13045431	557	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	558	if(ack!=0)
lakshya	20:949d13045431	559	return 0;
lakshya	20:949d13045431	560	}
lakshya	20:949d13045431	561	wait_ms(575);
lakshya	20:949d13045431	562
lakshya	20:949d13045431	563	manual = CONFIG_UPLOAD();
lakshya	20:949d13045431	564
lakshya	20:949d13045431	565	if(manual == 0)
lakshya	20:949d13045431	566	{
lakshya	20:949d13045431	567	//MANUAL CONFIGURATION FAILED
lakshya	20:949d13045431	568	return 0;
lakshya	20:949d13045431	569	}
lakshya	20:949d13045431	570
lakshya	20:949d13045431	571	}
lakshya	20:949d13045431	572	cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
lakshya	20:949d13045431	573	cmd[1]=BIT_RUN_ENB;
lakshya	20:949d13045431	574	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	575	if( ack!=0)
lakshya	20:949d13045431	576	{
lakshya	20:949d13045431	577	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	578	if(ack!=0)
lakshya	20:949d13045431	579	return 0;
lakshya	20:949d13045431	580	}
lakshya	20:949d13045431	581
lakshya	20:949d13045431	582	cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
lakshya	20:949d13045431	583	cmd[1]=BIT_MAGODR;
lakshya	20:949d13045431	584	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	585	if( ack!=0)
lakshya	20:949d13045431	586	{
lakshya	20:949d13045431	587	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	588	if(ack!=0)
lakshya	20:949d13045431	589	return 0;
lakshya	20:949d13045431	590	}
lakshya	20:949d13045431	591
lakshya	20:949d13045431	592	cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
lakshya	20:949d13045431	593	cmd[1]=BIT_GYROODR;
lakshya	20:949d13045431	594	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	595	if( ack!=0)
lakshya	20:949d13045431	596	{
lakshya	20:949d13045431	597	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	598	if(ack!=0)
lakshya	20:949d13045431	599	return 0;
lakshya	20:949d13045431	600	}
lakshya	20:949d13045431	601
lakshya	20:949d13045431	602	cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
lakshya	20:949d13045431	603	cmd[1]=0x00;
lakshya	20:949d13045431	604	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	605	if( ack!=0)
lakshya	20:949d13045431	606	{
lakshya	20:949d13045431	607	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	608	if(ack!=0)
lakshya	20:949d13045431	609	return 0;
lakshya	20:949d13045431	610	}
lakshya	20:949d13045431	611	//wait_ms(20);
lakshya	20:949d13045431	612	cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
lakshya	20:949d13045431	613	cmd[1]=0x00;
lakshya	20:949d13045431	614	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	615	if( ack!=0)
lakshya	20:949d13045431	616	{
lakshya	20:949d13045431	617	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	618	if(ack!=0)
lakshya	20:949d13045431	619	return 0;
lakshya	20:949d13045431	620	}
lakshya	20:949d13045431	621
lakshya	20:949d13045431	622	cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
lakshya	20:949d13045431	623	cmd[1]=BIT_EVT_ENB;
lakshya	20:949d13045431	624	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	625	if( ack!=0)
lakshya	20:949d13045431	626	{
lakshya	20:949d13045431	627	ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya	20:949d13045431	628	if(ack!=0)
lakshya	20:949d13045431	629	return 0;
lakshya	20:949d13045431	630	}
lakshya	20:949d13045431	631
lakshya	20:949d13045431	632	cmd[0]=SENTRALSTATUS;
lakshya	20:949d13045431	633	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	634	if( ack!=0)
lakshya	20:949d13045431	635	{
lakshya	20:949d13045431	636	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	637	if(ack!=0)
lakshya	20:949d13045431	638	return 0;
lakshya	20:949d13045431	639	}
lakshya	20:949d13045431	640
lakshya	20:949d13045431	641	ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	642	if( ack!=0)
lakshya	20:949d13045431	643	{
lakshya	20:949d13045431	644	ack= i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya	20:949d13045431	645	if(ack!=0)
lakshya	20:949d13045431	646	return 0;
lakshya	20:949d13045431	647	}
lakshya	20:949d13045431	648
lakshya	20:949d13045431	649	/// pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
lakshya	20:949d13045431	650
lakshya	20:949d13045431	651	if( (int)store == 3) //Check if initialised properly and not in idle state
lakshya	20:949d13045431	652	{
lakshya	20:949d13045431	653	/// pc_acs.printf("Exited sensor init successfully\n \r");
lakshya	20:949d13045431	654	return 1;
lakshya	20:949d13045431	655	}
lakshya	20:949d13045431	656
lakshya	20:949d13045431	657
lakshya	20:949d13045431	658	//// pc_acs.printf("Sensor init failed \n \r") ;
lakshya	20:949d13045431	659	return 0;
lakshya	20:949d13045431	660	}
lakshya	20:949d13045431	661
lakshya	20:949d13045431	662	int FCTN_ACS_INIT()
lakshya	20:949d13045431	663	{
lakshya	20:949d13045431	664	ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
lakshya	20:949d13045431	665
lakshya	17:fc782f7548c6	666
lakshya	20:949d13045431	667	int working=0;
lakshya	20:949d13045431	668
lakshya	20:949d13045431	669	/// pc_acs.printf("Attitude sensor init called \n \r");
lakshya	20:949d13045431	670	/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	17:fc782f7548c6	671
lakshya	17:fc782f7548c6	672
lakshya	20:949d13045431	673	if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
lakshya	20:949d13045431	674	{
lakshya	20:949d13045431	675
lakshya	20:949d13045431	676	/// pc_acs.printf("Sensor 1 marked working \n \r");
lakshya	20:949d13045431	677	working = SENSOR_INIT();
lakshya	20:949d13045431	678	if(working ==1)
lakshya	20:949d13045431	679	{
lakshya	20:949d13045431	680	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x70;
lakshya	20:949d13045431	681	/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
lakshya	20:949d13045431	682	/// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
lakshya	20:949d13045431	683	ACS_INIT_STATUS = 0;
lakshya	20:949d13045431	684	return 1;
lakshya	20:949d13045431	685	}
lakshya	20:949d13045431	686
lakshya	20:949d13045431	687
lakshya	20:949d13045431	688
lakshya	20:949d13045431	689	/// pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
lakshya	20:949d13045431	690	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	691	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0xC0;
lakshya	20:949d13045431	692
lakshya	20:949d13045431	693	}
lakshya	17:fc782f7548c6	694
lakshya	20:949d13045431	695	/// pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
lakshya	17:fc782f7548c6	696
lakshya	20:949d13045431	697	if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
lakshya	20:949d13045431	698	{
lakshya	20:949d13045431	699
lakshya	20:949d13045431	700
lakshya	20:949d13045431	701	ATS2_SW_ENABLE = 0;
lakshya	20:949d13045431	702	wait_ms(5);
lakshya	20:949d13045431	703	working = SENSOR_INIT();
lakshya	20:949d13045431	704	if(working ==1)
lakshya	20:949d13045431	705	{
lakshya	20:949d13045431	706	/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	707	/// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
lakshya	20:949d13045431	708	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x07;
lakshya	20:949d13045431	709	ACS_INIT_STATUS = 0;
lakshya	20:949d13045431	710	return 2;
lakshya	20:949d13045431	711	}
lakshya	20:949d13045431	712
lakshya	20:949d13045431	713	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	714	wait_ms(5);
lakshya	20:949d13045431	715
lakshya	20:949d13045431	716	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x0C;
lakshya	20:949d13045431	717
lakshya	20:949d13045431	718
lakshya	20:949d13045431	719	}
lakshya	17:fc782f7548c6	720
lakshya	20:949d13045431	721	/// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	722	/// pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
lakshya	17:fc782f7548c6	723
lakshya	20:949d13045431	724	ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
lakshya	20:949d13045431	725	return 0;
sakthipriya	0:7b4c00e3912f	726	}
sakthipriya	0:7b4c00e3912f	727
lakshya	20:949d13045431	728
lakshya	20:949d13045431	729	int SENSOR_DATA_ACQ()
sakthipriya	0:7b4c00e3912f	730	{
lakshya	20:949d13045431	731	//int mag_only=0;
lakshya	20:949d13045431	732	/// pc_acs.printf("Entering Sensor data acq.\n \r");
lakshya	20:949d13045431	733	char status;
lakshya	20:949d13045431	734	int sentral;
lakshya	20:949d13045431	735	int event;
lakshya	20:949d13045431	736	int sensor;
lakshya	20:949d13045431	737	int error;
lakshya	20:949d13045431	738	int init;
lakshya	20:949d13045431	739
lakshya	20:949d13045431	740	uint8_t gyro_error=0;
lakshya	20:949d13045431	741	uint8_t mag_error=0;
lakshya	20:949d13045431	742
lakshya	20:949d13045431	743	//int ack1;
lakshya	20:949d13045431	744	//int ack2;
lakshya	20:949d13045431	745
sakthipriya	0:7b4c00e3912f	746	cmd[0]=EVT_STATUS;
lakshya	20:949d13045431	747	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	748	if(ack!=0)
lakshya	20:949d13045431	749	{
lakshya	20:949d13045431	750	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	751	if(ack!=0)
lakshya	20:949d13045431	752	return 0;
lakshya	20:949d13045431	753	}
lakshya	20:949d13045431	754
lakshya	20:949d13045431	755	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	756	if(ack!=0)
lakshya	20:949d13045431	757	{
lakshya	20:949d13045431	758	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	759	if(ack!=0)
lakshya	20:949d13045431	760	return 0;
lakshya	20:949d13045431	761	}
lakshya	17:fc782f7548c6	762
lakshya	20:949d13045431	763	event = (int)status;
lakshya	20:949d13045431	764
lakshya	20:949d13045431	765	if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya	20:949d13045431	766	{
lakshya	20:949d13045431	767	ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya	20:949d13045431	768	}
lakshya	20:949d13045431	769	else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya	20:949d13045431	770	{
lakshya	20:949d13045431	771	ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya	20:949d13045431	772	}
lakshya	20:949d13045431	773
lakshya	20:949d13045431	774	cmd[0]=SENTRALSTATUS;
lakshya	20:949d13045431	775	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	776	if(ack!=0)
lakshya	20:949d13045431	777	{
lakshya	20:949d13045431	778	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	779	if(ack!=0)
lakshya	20:949d13045431	780	return 0;
lakshya	20:949d13045431	781	}
lakshya	20:949d13045431	782
lakshya	20:949d13045431	783	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	784	if(ack!=0)
lakshya	20:949d13045431	785	{
lakshya	20:949d13045431	786	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	787	if(ack!=0)
lakshya	20:949d13045431	788	return 0;
lakshya	20:949d13045431	789	}
lakshya	20:949d13045431	790
lakshya	17:fc782f7548c6	791
lakshya	20:949d13045431	792	sentral = (int) status;
lakshya	20:949d13045431	793
lakshya	20:949d13045431	794	if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya	20:949d13045431	795	{
lakshya	20:949d13045431	796	ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya	20:949d13045431	797	}
lakshya	20:949d13045431	798	else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya	20:949d13045431	799	{
lakshya	20:949d13045431	800	ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya	20:949d13045431	801	}
lakshya	20:949d13045431	802
lakshya	20:949d13045431	803	/// pc_acs.printf("Event Status is %x\n \r",event);
lakshya	20:949d13045431	804	/// pc_acs.printf("Sentral Status is %x\n \r",sentral);
lakshya	20:949d13045431	805
lakshya	17:fc782f7548c6	806
lakshya	17:fc782f7548c6	807
lakshya	20:949d13045431	808	if ( (event & 0x40 != 0x40 ) \|\| (event & 0x08 != 0x08 ) \|\| (event & 0x01 == 0x01 )\|\| (event & 0x02 == 0x02 )\|\| (sentral!= 3)) //check for any error in event status register
lakshya	20:949d13045431	809	{
lakshya	20:949d13045431	810
lakshya	20:949d13045431	811
lakshya	20:949d13045431	812	init = SENSOR_INIT();
lakshya	20:949d13045431	813
lakshya	20:949d13045431	814	cmd[0]=EVT_STATUS;
lakshya	20:949d13045431	815	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	816	if(ack!=0)
lakshya	20:949d13045431	817	{
lakshya	20:949d13045431	818	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	819	if(ack!=0)
lakshya	20:949d13045431	820	return 0;
lakshya	20:949d13045431	821	}
lakshya	20:949d13045431	822
lakshya	20:949d13045431	823	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	824	if(ack!=0)
lakshya	20:949d13045431	825	{
lakshya	20:949d13045431	826	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	827	if(ack!=0)
lakshya	20:949d13045431	828	return 0;
lakshya	20:949d13045431	829	}
lakshya	20:949d13045431	830
lakshya	20:949d13045431	831	event = (int)status;
lakshya	20:949d13045431	832
lakshya	20:949d13045431	833	cmd[0]=SENTRALSTATUS;
lakshya	20:949d13045431	834	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	835	if(ack!=0)
lakshya	20:949d13045431	836	{
lakshya	20:949d13045431	837	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	838	if(ack!=0)
lakshya	20:949d13045431	839	return 0;
lakshya	20:949d13045431	840	}
lakshya	20:949d13045431	841
lakshya	20:949d13045431	842	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	843	if(ack!=0)
lakshya	20:949d13045431	844	{
lakshya	20:949d13045431	845	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	846	if(ack!=0)
lakshya	20:949d13045431	847	return 0;
lakshya	20:949d13045431	848	}
lakshya	20:949d13045431	849
lakshya	20:949d13045431	850	sentral = (int)status;
lakshya	20:949d13045431	851
lakshya	20:949d13045431	852	/// pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
lakshya	20:949d13045431	853
lakshya	20:949d13045431	854	if ( (event & 0x40 != 0x40 ) \|\| (event & 0x08 != 0x08) \|\| (event & 0x01 == 0x01 )\|\| (event & 0x02 == 0x02 ) \|\| (init == 0)\|\|(sentral != 3)) //check for any error in event status
lakshya	20:949d13045431	855	{
lakshya	20:949d13045431	856
lakshya	20:949d13045431	857	cmd[0]=ERROR;
lakshya	20:949d13045431	858	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	859	if(ack!=0)
lakshya	20:949d13045431	860	{
lakshya	20:949d13045431	861	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	862	if(ack!=0)
lakshya	20:949d13045431	863	return 0;
lakshya	20:949d13045431	864	}
lakshya	20:949d13045431	865
lakshya	20:949d13045431	866	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	867	if(ack!=0)
lakshya	20:949d13045431	868	{
lakshya	20:949d13045431	869
lakshya	20:949d13045431	870	if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya	20:949d13045431	871	{
lakshya	20:949d13045431	872	ATS1_ERROR_RGTR = 0x01;
lakshya	20:949d13045431	873	}
lakshya	20:949d13045431	874	else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya	20:949d13045431	875	{
lakshya	20:949d13045431	876	ATS2_ERROR_RGTR = 0x01;
lakshya	20:949d13045431	877	}
lakshya	20:949d13045431	878	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	879	if(ack!=0)
lakshya	20:949d13045431	880	return 0;
lakshya	20:949d13045431	881	}
lakshya	20:949d13045431	882
lakshya	20:949d13045431	883	error = (int)status;
lakshya	20:949d13045431	884
lakshya	20:949d13045431	885	if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya	20:949d13045431	886	{
lakshya	20:949d13045431	887	ATS1_ERROR_RGTR = (uint8_t)error;
lakshya	20:949d13045431	888	}
lakshya	20:949d13045431	889	else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya	20:949d13045431	890	{
lakshya	20:949d13045431	891	ATS2_ERROR_RGTR = (uint8_t)error;
lakshya	20:949d13045431	892	}
lakshya	20:949d13045431	893
lakshya	20:949d13045431	894	cmd[0]=SENSORSTATUS;
lakshya	20:949d13045431	895	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	896	if(ack!=0)
lakshya	20:949d13045431	897	{
lakshya	20:949d13045431	898	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	899	if(ack!=0)
lakshya	20:949d13045431	900	return 0;
lakshya	20:949d13045431	901	}
lakshya	20:949d13045431	902
lakshya	20:949d13045431	903	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	904	if(ack!=0)
lakshya	20:949d13045431	905	{
lakshya	20:949d13045431	906	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	907	if(ack!=0)
lakshya	20:949d13045431	908	return 0;
lakshya	20:949d13045431	909	}
lakshya	20:949d13045431	910
lakshya	20:949d13045431	911	sensor = (int)status;
lakshya	20:949d13045431	912
lakshya	20:949d13045431	913
lakshya	20:949d13045431	914	if((error!=0) \|\| (sensor!=0))
lakshya	20:949d13045431	915	{
lakshya	20:949d13045431	916	if( (error&1 == 1) \|\| (sensor&1 == 1) \|\| (sensor&16 == 16) )
lakshya	20:949d13045431	917	{
lakshya	20:949d13045431	918	pc_acs.printf("error in gyro alone..\n \r");
lakshya	20:949d13045431	919	gyro_error = 1;
lakshya	20:949d13045431	920	}
lakshya	20:949d13045431	921
lakshya	20:949d13045431	922	if( (error&4 == 4) \|\| (sensor&4 == 4) \|\| (sensor&64 == 64) )
lakshya	20:949d13045431	923	{
lakshya	20:949d13045431	924
lakshya	20:949d13045431	925	pc_acs.printf("error in mag alone.Exiting.\n \r");
lakshya	20:949d13045431	926	mag_error = 1;
lakshya	20:949d13045431	927	}
lakshya	20:949d13045431	928	if( (gyro_error!=1)&&(mag_error!=1))
lakshya	20:949d13045431	929	{
lakshya	20:949d13045431	930	pc_acs.printf("error in something else.Exiting.\n \r");
lakshya	20:949d13045431	931	return 0;
lakshya	20:949d13045431	932
lakshya	20:949d13045431	933	}
lakshya	20:949d13045431	934	}
lakshya	20:949d13045431	935
lakshya	20:949d13045431	936
lakshya	20:949d13045431	937	if((event & 1 == 1 ))
lakshya	20:949d13045431	938	{
lakshya	20:949d13045431	939	/// pc_acs.printf("error in CPU Reset.\n \r");
lakshya	20:949d13045431	940	return 0;
lakshya	20:949d13045431	941
lakshya	20:949d13045431	942	}
lakshya	20:949d13045431	943
lakshya	20:949d13045431	944	if((event & 8 != 8 )\|\|(event & 32 != 32 ))
lakshya	20:949d13045431	945	{
lakshya	20:949d13045431	946	pc_acs.printf("Data not ready waiting...\n \r");
lakshya	20:949d13045431	947	//POLL
lakshya	20:949d13045431	948	wait_ms(200);
lakshya	20:949d13045431	949
lakshya	20:949d13045431	950	cmd[0]=EVT_STATUS;
lakshya	20:949d13045431	951
lakshya	20:949d13045431	952	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	953	if(ack!=0)
lakshya	20:949d13045431	954	{
lakshya	20:949d13045431	955	ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya	20:949d13045431	956	if(ack!=0)
lakshya	20:949d13045431	957	return 0;
lakshya	20:949d13045431	958	}
lakshya	20:949d13045431	959
lakshya	20:949d13045431	960	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	961	if(ack!=0)
lakshya	20:949d13045431	962	{
lakshya	20:949d13045431	963	ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya	20:949d13045431	964	if(ack!=0)
lakshya	20:949d13045431	965	return 0;
lakshya	20:949d13045431	966	}
lakshya	20:949d13045431	967
lakshya	20:949d13045431	968	event = (int)status;
lakshya	20:949d13045431	969	if(event & 32 != 32 )
lakshya	20:949d13045431	970	{
lakshya	20:949d13045431	971
lakshya	20:949d13045431	972	pc_acs.printf("Mag data only ready.Read..\n \r");
lakshya	20:949d13045431	973	gyro_error = 1;
lakshya	20:949d13045431	974
lakshya	20:949d13045431	975	}
lakshya	20:949d13045431	976
lakshya	20:949d13045431	977	if(event & 8 != 8 )
lakshya	20:949d13045431	978	{
lakshya	20:949d13045431	979	pc_acs.printf("Both data still not ready.Exiting..\n \r");
lakshya	20:949d13045431	980	mag_error=1;
lakshya	20:949d13045431	981	}
lakshya	20:949d13045431	982
lakshya	20:949d13045431	983
lakshya	20:949d13045431	984	}
lakshya	20:949d13045431	985
lakshya	20:949d13045431	986
lakshya	20:949d13045431	987	}
lakshya	20:949d13045431	988
lakshya	20:949d13045431	989	if((mag_error !=1)&&(gyro_error!=1))
lakshya	20:949d13045431	990	{
lakshya	20:949d13045431	991	pc_acs.printf("Error in something else.Exiting.\n \r");
lakshya	20:949d13045431	992	return 0;
lakshya	20:949d13045431	993	}
lakshya	20:949d13045431	994
lakshya	20:949d13045431	995	if((mag_error ==1)&&(gyro_error==1))
lakshya	20:949d13045431	996	{
lakshya	20:949d13045431	997	pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
lakshya	20:949d13045431	998	return 0;
lakshya	20:949d13045431	999	}
lakshya	20:949d13045431	1000
lakshya	20:949d13045431	1001	}
lakshya	20:949d13045431	1002
lakshya	20:949d13045431	1003
lakshya	20:949d13045431	1004	cmd[0]=MAG_XOUT_H; //LSB of x
lakshya	20:949d13045431	1005	i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya	20:949d13045431	1006	ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya	20:949d13045431	1007	if(ack != 0)
lakshya	20:949d13045431	1008	{
lakshya	20:949d13045431	1009	cmd[0]=MAG_XOUT_H; //LSB of x
lakshya	20:949d13045431	1010	i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya	20:949d13045431	1011	ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya	20:949d13045431	1012	if(ack !=1)
lakshya	20:949d13045431	1013	return 0;
lakshya	20:949d13045431	1014
lakshya	20:949d13045431	1015	}
lakshya	20:949d13045431	1016
lakshya	20:949d13045431	1017
lakshya	20:949d13045431	1018	// pc_acs.printf("\nGyro Values:\n");
lakshya	20:949d13045431	1019	if (gyro_error!=1)
lakshya	20:949d13045431	1020	{
lakshya	20:949d13045431	1021	for(int i=0; i<3; i++) {
lakshya	20:949d13045431	1022	//concatenating gyro LSB and MSB to get 16 bit signed data values
lakshya	20:949d13045431	1023	actual_data.bit_data_acs_mg[i]= ((int16_t)reg_data[16+2i+1]<<8)\|(int16_t)reg_data[16+2i];
lakshya	20:949d13045431	1024	gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
lakshya	20:949d13045431	1025	gyro_data[i]=gyro_data[i]/senstivity_gyro;
lakshya	20:949d13045431	1026	actual_data.AngularSpeed_actual[i] = gyro_data[i];
lakshya	20:949d13045431	1027	}
lakshya	20:949d13045431	1028	}
lakshya	20:949d13045431	1029
lakshya	20:949d13045431	1030	if(mag_error!=1)
lakshya	20:949d13045431	1031	{
lakshya	20:949d13045431	1032	for(int i=0; i<3; i++) {
lakshya	20:949d13045431	1033	//concatenating mag LSB and MSB to get 16 bit signed data values Extract data
lakshya	20:949d13045431	1034	actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2i+1]<<8)\|(int16_t)reg_data[2i];
lakshya	20:949d13045431	1035
lakshya	20:949d13045431	1036	mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
lakshya	20:949d13045431	1037	mag_data[i]=mag_data[i]/senstivity_mag;
lakshya	20:949d13045431	1038	actual_data.Bvalue_actual[i] = mag_data[i];
lakshya	20:949d13045431	1039	}
lakshya	20:949d13045431	1040	}
lakshya	20:949d13045431	1041
lakshya	20:949d13045431	1042
lakshya	20:949d13045431	1043	if(mag_error == 1)
lakshya	20:949d13045431	1044	{
lakshya	20:949d13045431	1045
lakshya	20:949d13045431	1046	pc_acs.printf("Gyro only successful.\n \r");
lakshya	20:949d13045431	1047	return 1;
lakshya	20:949d13045431	1048	}
lakshya	20:949d13045431	1049	if(gyro_error == 1)
lakshya	20:949d13045431	1050	{
lakshya	20:949d13045431	1051	pc_acs.printf("Mag only successful.\n \r");
lakshya	20:949d13045431	1052	return 2;
lakshya	20:949d13045431	1053	}
lakshya	17:fc782f7548c6	1054
lakshya	20:949d13045431	1055	pc_acs.printf("Reading data success.\n \r");
lakshya	20:949d13045431	1056	return 3;
lakshya	20:949d13045431	1057	}
lakshya	20:949d13045431	1058
lakshya	20:949d13045431	1059
lakshya	20:949d13045431	1060	int FCTN_ATS_DATA_ACQ()
lakshya	20:949d13045431	1061	{
lakshya	20:949d13045431	1062	for(int i=0; i<3; i++) {
lakshya	20:949d13045431	1063	actual_data.AngularSpeed_actual[i] = 0;
lakshya	20:949d13045431	1064	actual_data.Bvalue_actual[i] = 0;
lakshya	20:949d13045431	1065	}
lakshya	20:949d13045431	1066
lakshya	20:949d13045431	1067	int acq;
lakshya	20:949d13045431	1068	int init;
lakshya	20:949d13045431	1069
lakshya	20:949d13045431	1070	//// pc_acs.printf("DATA_ACQ called \n \r");
lakshya	20:949d13045431	1071	//// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	1072
lakshya	20:949d13045431	1073
lakshya	20:949d13045431	1074	if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
lakshya	20:949d13045431	1075	{
lakshya	20:949d13045431	1076
lakshya	20:949d13045431	1077	acq = SENSOR_DATA_ACQ();
lakshya	20:949d13045431	1078	if(acq == 3)
lakshya	20:949d13045431	1079	{
lakshya	20:949d13045431	1080
lakshya	20:949d13045431	1081	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x70;
lakshya	20:949d13045431	1082
lakshya	20:949d13045431	1083	//??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya	20:949d13045431	1084	//// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	1085	//// pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya	20:949d13045431	1086	return 3;
lakshya	20:949d13045431	1087	}
lakshya	20:949d13045431	1088	else if((acq == 2)\|\|(acq==1))
lakshya	20:949d13045431	1089	{
lakshya	20:949d13045431	1090	pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya	20:949d13045431	1091	if( (ACS_ATS_STATUS & 0x0F == 0x03) \|\|((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))\|\|((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya	20:949d13045431	1092	{
lakshya	20:949d13045431	1093	//other sensor both working, off or
lakshya	20:949d13045431	1094	//other sensor gyro working, this sensor not working , off
lakshya	20:949d13045431	1095	//other sensor mag working, this sensor not working,off
lakshya	20:949d13045431	1096
lakshya	20:949d13045431	1097	ATS1_SW_ENABLE = 1; //switch off sensor 1
lakshya	20:949d13045431	1098	wait_ms(5);
lakshya	20:949d13045431	1099	if(acq == 1)
lakshya	20:949d13045431	1100	{
lakshya	20:949d13045431	1101	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x10; //Update sensor 1 status
lakshya	20:949d13045431	1102	}
lakshya	20:949d13045431	1103	if(acq==2)
lakshya	20:949d13045431	1104	{
lakshya	20:949d13045431	1105	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x20;
lakshya	20:949d13045431	1106	}
lakshya	20:949d13045431	1107
lakshya	20:949d13045431	1108	ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya	20:949d13045431	1109	wait_ms(5);
lakshya	20:949d13045431	1110
lakshya	20:949d13045431	1111	init = SENSOR_INIT(); //sensor 2 init
lakshya	20:949d13045431	1112	if( init == 0)
lakshya	20:949d13045431	1113	{
lakshya	20:949d13045431	1114	pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya	20:949d13045431	1115	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1116	wait_ms(5);
lakshya	20:949d13045431	1117	ATS1_SW_ENABLE = 0;
lakshya	20:949d13045431	1118	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x0C; //Update not working and switch back to 1
lakshya	20:949d13045431	1119	if(acq == 1)
lakshya	20:949d13045431	1120	{
lakshya	20:949d13045431	1121	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x50; //Update sensor 1 status
lakshya	20:949d13045431	1122	}
lakshya	20:949d13045431	1123	if(acq==2)
lakshya	20:949d13045431	1124	{
lakshya	20:949d13045431	1125	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x60;
lakshya	20:949d13045431	1126	}
lakshya	20:949d13045431	1127	return acq;
lakshya	20:949d13045431	1128	}
lakshya	20:949d13045431	1129
lakshya	20:949d13045431	1130	int acq2;
lakshya	20:949d13045431	1131	acq2 = SENSOR_DATA_ACQ();
lakshya	20:949d13045431	1132	if(acq2 == 3)
lakshya	20:949d13045431	1133	{
lakshya	20:949d13045431	1134	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x07;
lakshya	20:949d13045431	1135	pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya	20:949d13045431	1136	return 3;
lakshya	20:949d13045431	1137	}
lakshya	20:949d13045431	1138	else if(acq2 == 1)
lakshya	20:949d13045431	1139	{
lakshya	20:949d13045431	1140	if(acq==2)
lakshya	20:949d13045431	1141	{
lakshya	20:949d13045431	1142	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1143	wait_ms(5);
lakshya	20:949d13045431	1144	ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
lakshya	20:949d13045431	1145	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x01;
lakshya	20:949d13045431	1146	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x60;
lakshya	20:949d13045431	1147	return 3;
lakshya	20:949d13045431	1148	}
lakshya	20:949d13045431	1149	else
lakshya	20:949d13045431	1150	{
lakshya	20:949d13045431	1151	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya	20:949d13045431	1152	return 1;
lakshya	20:949d13045431	1153	}
lakshya	20:949d13045431	1154	}
lakshya	20:949d13045431	1155
lakshya	20:949d13045431	1156	else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya	20:949d13045431	1157	{
lakshya	20:949d13045431	1158	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x06;
lakshya	20:949d13045431	1159	return 2;
lakshya	20:949d13045431	1160	}
lakshya	20:949d13045431	1161	else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya	20:949d13045431	1162	{
lakshya	20:949d13045431	1163	pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya	20:949d13045431	1164	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1165	wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
lakshya	20:949d13045431	1166	ATS1_SW_ENABLE = 0;
lakshya	20:949d13045431	1167	wait_ms(5);
lakshya	20:949d13045431	1168	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)\|0x40;
lakshya	20:949d13045431	1169	return acq;
lakshya	20:949d13045431	1170	}
lakshya	20:949d13045431	1171
lakshya	20:949d13045431	1172	}
lakshya	20:949d13045431	1173	else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya	20:949d13045431	1174	{
lakshya	20:949d13045431	1175	if(acq == 1)
lakshya	20:949d13045431	1176	{
lakshya	20:949d13045431	1177	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x50; //return Sensor 2 status and update acq
lakshya	20:949d13045431	1178	return 1;
lakshya	20:949d13045431	1179	}
lakshya	20:949d13045431	1180	if(acq==2)
lakshya	20:949d13045431	1181	{
lakshya	20:949d13045431	1182	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x60;
lakshya	20:949d13045431	1183	return 2;
lakshya	20:949d13045431	1184	}
lakshya	20:949d13045431	1185	pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya	20:949d13045431	1186	return acq;
lakshya	20:949d13045431	1187
lakshya	20:949d13045431	1188	}
sakthipriya	0:7b4c00e3912f	1189	}
lakshya	20:949d13045431	1190
lakshya	20:949d13045431	1191	else if(acq == 0)
lakshya	20:949d13045431	1192	{
lakshya	20:949d13045431	1193	pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
lakshya	20:949d13045431	1194	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	1195	wait_ms(5); //Switch ON sensor 2
lakshya	20:949d13045431	1196	ATS2_SW_ENABLE = 0;
lakshya	20:949d13045431	1197	wait_ms(5);
lakshya	20:949d13045431	1198	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0xC0;
lakshya	20:949d13045431	1199	if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya	20:949d13045431	1200	{
lakshya	20:949d13045431	1201	init = SENSOR_INIT();
lakshya	20:949d13045431	1202	if( init == 0)
lakshya	20:949d13045431	1203	{
lakshya	20:949d13045431	1204	pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
lakshya	20:949d13045431	1205	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1206	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x0C; //Sensor 2 also not working exit
lakshya	20:949d13045431	1207	return 0;
lakshya	20:949d13045431	1208	}
lakshya	20:949d13045431	1209
lakshya	20:949d13045431	1210	int acq2;
lakshya	20:949d13045431	1211	acq2 = SENSOR_DATA_ACQ();
lakshya	20:949d13045431	1212	if(acq2 == 3)
lakshya	20:949d13045431	1213	{
lakshya	20:949d13045431	1214	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x07;
lakshya	20:949d13045431	1215	pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya	20:949d13045431	1216	return 3;
lakshya	20:949d13045431	1217	}
lakshya	20:949d13045431	1218	else if(acq2 == 1)
lakshya	20:949d13045431	1219	{
lakshya	20:949d13045431	1220	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x05;
lakshya	20:949d13045431	1221	return 1;
lakshya	20:949d13045431	1222	}
lakshya	20:949d13045431	1223	else if(acq2 == 2)
lakshya	20:949d13045431	1224	{
lakshya	20:949d13045431	1225	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x06;
lakshya	20:949d13045431	1226	return 2;
lakshya	20:949d13045431	1227	}
lakshya	20:949d13045431	1228	else if(acq2 == 0)
lakshya	20:949d13045431	1229	{
lakshya	20:949d13045431	1230	pc_acs.printf(" Sensor 2 data acq failure..\n \r");
lakshya	20:949d13045431	1231	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1232
lakshya	20:949d13045431	1233	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x0C;
lakshya	20:949d13045431	1234	return 0;
lakshya	20:949d13045431	1235	}
lakshya	20:949d13045431	1236
lakshya	20:949d13045431	1237	}
lakshya	20:949d13045431	1238
sakthipriya	0:7b4c00e3912f	1239	}
lakshya	20:949d13045431	1240
lakshya	20:949d13045431	1241
sakthipriya	0:7b4c00e3912f	1242	}
lakshya	20:949d13045431	1243
lakshya	20:949d13045431	1244	if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
lakshya	20:949d13045431	1245	{
lakshya	20:949d13045431	1246	acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
lakshya	20:949d13045431	1247	if(acq == 3) //Both available read and exit
lakshya	20:949d13045431	1248	{
lakshya	20:949d13045431	1249	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x07;
lakshya	20:949d13045431	1250	pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	1251	pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya	20:949d13045431	1252	return 3;
lakshya	20:949d13045431	1253	}
lakshya	20:949d13045431	1254	else if((acq == 2)\|\|(acq==1)) //Only mag or only gyro
lakshya	20:949d13045431	1255	{
lakshya	20:949d13045431	1256	pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya	20:949d13045431	1257	if((ACS_ATS_STATUS & 0xF0 == 0x30) \|\|((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))\|\|((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya	20:949d13045431	1258	{
lakshya	20:949d13045431	1259	//other sensor both working, off or
lakshya	20:949d13045431	1260	//other sensor gyro working, this sensor not working , off
lakshya	20:949d13045431	1261	//other sensor mag working, this sensor not working,off
lakshya	20:949d13045431	1262	ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya	20:949d13045431	1263	wait_ms(5);
lakshya	20:949d13045431	1264	if(acq == 1)
lakshya	20:949d13045431	1265	{
lakshya	20:949d13045431	1266	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x01; //Update sensor 2 status
lakshya	20:949d13045431	1267	}
lakshya	20:949d13045431	1268	if(acq==2)
lakshya	20:949d13045431	1269	{
lakshya	20:949d13045431	1270	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x02;
lakshya	20:949d13045431	1271	}
lakshya	20:949d13045431	1272
lakshya	20:949d13045431	1273	ATS1_SW_ENABLE = 0; //switch on sensor 1
lakshya	20:949d13045431	1274	wait_ms(5);
lakshya	20:949d13045431	1275	init = SENSOR_INIT(); //sensor 2 init
lakshya	20:949d13045431	1276
lakshya	20:949d13045431	1277	if( init == 0)
lakshya	20:949d13045431	1278	{
lakshya	20:949d13045431	1279	pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
lakshya	20:949d13045431	1280	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	1281	wait_ms(5);
lakshya	20:949d13045431	1282	ATS2_SW_ENABLE = 0;
lakshya	20:949d13045431	1283	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0xC0; //Update not working and switch back to 2
lakshya	20:949d13045431	1284	if(acq == 1)
lakshya	20:949d13045431	1285	{
lakshya	20:949d13045431	1286	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x05; //Update sensor 1 status
lakshya	20:949d13045431	1287	}
lakshya	20:949d13045431	1288	if(acq==2)
lakshya	20:949d13045431	1289	{
lakshya	20:949d13045431	1290	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x06;
lakshya	20:949d13045431	1291	}
lakshya	20:949d13045431	1292	return acq;
lakshya	20:949d13045431	1293	}
lakshya	20:949d13045431	1294
lakshya	20:949d13045431	1295	int acq2;
lakshya	20:949d13045431	1296	acq2 = SENSOR_DATA_ACQ();
lakshya	20:949d13045431	1297
lakshya	20:949d13045431	1298	if(acq2 == 3)
lakshya	20:949d13045431	1299	{
lakshya	20:949d13045431	1300	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x70;
lakshya	20:949d13045431	1301	pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya	20:949d13045431	1302	return 3;
lakshya	20:949d13045431	1303	}
lakshya	20:949d13045431	1304
lakshya	20:949d13045431	1305	else if(acq2 == 1)
lakshya	20:949d13045431	1306	{
lakshya	20:949d13045431	1307	if(acq==2)
lakshya	20:949d13045431	1308	{
lakshya	20:949d13045431	1309	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	1310	wait_ms(5);
lakshya	20:949d13045431	1311	ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya	20:949d13045431	1312	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x10;
lakshya	20:949d13045431	1313	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x06;
lakshya	20:949d13045431	1314	return 3;
lakshya	20:949d13045431	1315	}
lakshya	20:949d13045431	1316	else
lakshya	20:949d13045431	1317	{
lakshya	20:949d13045431	1318	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya	20:949d13045431	1319	return 1;
lakshya	20:949d13045431	1320	}
lakshya	20:949d13045431	1321	}
lakshya	20:949d13045431	1322
lakshya	20:949d13045431	1323	else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya	20:949d13045431	1324	{
lakshya	20:949d13045431	1325	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x60;
lakshya	20:949d13045431	1326	return 2;
lakshya	20:949d13045431	1327	}
lakshya	20:949d13045431	1328	else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya	20:949d13045431	1329	{
lakshya	20:949d13045431	1330	pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
lakshya	20:949d13045431	1331	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	1332	wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya	20:949d13045431	1333	ATS2_SW_ENABLE = 0;
lakshya	20:949d13045431	1334	wait_ms(5);
lakshya	20:949d13045431	1335	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)\|0x04;
lakshya	20:949d13045431	1336	return acq;
lakshya	20:949d13045431	1337	}
lakshya	20:949d13045431	1338
lakshya	20:949d13045431	1339	}
lakshya	20:949d13045431	1340	else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya	20:949d13045431	1341	{
lakshya	20:949d13045431	1342	if(acq == 1)
lakshya	20:949d13045431	1343	{
lakshya	20:949d13045431	1344	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x05; //return Sensor 1 status and update acq
lakshya	20:949d13045431	1345	return 1;
lakshya	20:949d13045431	1346	}
lakshya	20:949d13045431	1347	if(acq==2)
lakshya	20:949d13045431	1348	{
lakshya	20:949d13045431	1349	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x06;
lakshya	20:949d13045431	1350	return 2;
lakshya	20:949d13045431	1351	}
lakshya	20:949d13045431	1352	pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya	20:949d13045431	1353	return acq;
lakshya	20:949d13045431	1354
lakshya	20:949d13045431	1355	}
lakshya	20:949d13045431	1356	}
lakshya	20:949d13045431	1357	else if(acq == 0)
lakshya	20:949d13045431	1358	{
lakshya	20:949d13045431	1359	pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya	20:949d13045431	1360	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1361	wait_ms(5); //Switch ON sensor 1
lakshya	20:949d13045431	1362	ATS1_SW_ENABLE = 0;
lakshya	20:949d13045431	1363	wait_ms(5);
lakshya	20:949d13045431	1364	ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)\|0x0C;
lakshya	20:949d13045431	1365	if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya	20:949d13045431	1366	{
lakshya	20:949d13045431	1367	init = SENSOR_INIT();
lakshya	20:949d13045431	1368	if( init == 0)
lakshya	20:949d13045431	1369	{
lakshya	20:949d13045431	1370	pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
lakshya	20:949d13045431	1371	ATS2_SW_ENABLE = 1;
lakshya	20:949d13045431	1372	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0xC0; //Sensor 1 also not working exit
lakshya	20:949d13045431	1373	return 0;
lakshya	20:949d13045431	1374	}
lakshya	20:949d13045431	1375
lakshya	20:949d13045431	1376	int acq2;
lakshya	20:949d13045431	1377	acq2 = SENSOR_DATA_ACQ();
lakshya	20:949d13045431	1378	if(acq2 == 3)
lakshya	20:949d13045431	1379	{
lakshya	20:949d13045431	1380	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x70;
lakshya	20:949d13045431	1381	pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya	20:949d13045431	1382	return 3;
lakshya	20:949d13045431	1383	}
lakshya	20:949d13045431	1384	else if(acq2 == 1)
lakshya	20:949d13045431	1385	{
lakshya	20:949d13045431	1386	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x50;
lakshya	20:949d13045431	1387	return 1;
lakshya	20:949d13045431	1388	}
lakshya	20:949d13045431	1389	else if(acq2 == 2)
lakshya	20:949d13045431	1390	{
lakshya	20:949d13045431	1391	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0x60;
lakshya	20:949d13045431	1392	return 2;
lakshya	20:949d13045431	1393	}
lakshya	20:949d13045431	1394	else if(acq2 == 0)
lakshya	20:949d13045431	1395	{
lakshya	20:949d13045431	1396	pc_acs.printf(" Sensor 1 data acq failure..\n \r");
lakshya	20:949d13045431	1397	ATS1_SW_ENABLE = 1;
lakshya	20:949d13045431	1398	ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)\|0xC0;
lakshya	20:949d13045431	1399	return 0;
lakshya	20:949d13045431	1400	}
lakshya	20:949d13045431	1401	}
lakshya	20:949d13045431	1402	}
lakshya	20:949d13045431	1403	}
lakshya	20:949d13045431	1404	pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya	20:949d13045431	1405	pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
lakshya	20:949d13045431	1406	return 0;
sakthipriya	0:7b4c00e3912f	1407	}
sakthipriya	0:7b4c00e3912f	1408
sakthipriya	0:7b4c00e3912f	1409	void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya	0:7b4c00e3912f	1410	{
lakshya	20:949d13045431	1411	//// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya	0:7b4c00e3912f	1412
sakthipriya	0:7b4c00e3912f	1413	float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya	0:7b4c00e3912f	1414	float l_current_x=0; //Current sent in x TR's
sakthipriya	0:7b4c00e3912f	1415	float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya	0:7b4c00e3912f	1416	float l_current_y=0; //Current sent in y TR's
sakthipriya	0:7b4c00e3912f	1417	float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya	0:7b4c00e3912f	1418	float l_current_z=0; //Current sent in z TR's
sakthipriya	0:7b4c00e3912f	1419
sakthipriya	0:7b4c00e3912f	1420
lakshya	20:949d13045431	1421	//// printf("\r\r");
sakthipriya	0:7b4c00e3912f	1422
sakthipriya	0:7b4c00e3912f	1423	//----------------------------- x-direction TR --------------------------------------------//
sakthipriya	0:7b4c00e3912f	1424
sakthipriya	0:7b4c00e3912f	1425
sakthipriya	0:7b4c00e3912f	1426	float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya	0:7b4c00e3912f	1427
sakthipriya	0:7b4c00e3912f	1428	phase_TR_x = 1; // setting the default current direction
sakthipriya	0:7b4c00e3912f	1429	if (l_moment_x <0)
sakthipriya	0:7b4c00e3912f	1430	{
sakthipriya	0:7b4c00e3912f	1431	phase_TR_x = 0; // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
sakthipriya	0:7b4c00e3912f	1432	l_moment_x = abs(l_moment_x);
sakthipriya	0:7b4c00e3912f	1433	}
sakthipriya	0:7b4c00e3912f	1434
sakthipriya	0:7b4c00e3912f	1435	l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya	20:949d13045431	1436	//// printf("current in trx is %f \r \n",l_current_x);
lakshya	10:f93407b97750	1437	if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya	10:f93407b97750	1438	{
lakshya	10:f93407b97750	1439	l_duty_cycle_x = 310000000pow(l_current_x,3)- 90216pow(l_current_x,2) + 697.78l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1440	//// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya	10:f93407b97750	1441	PWM1.period(TIME_PERIOD);
lakshya	10:f93407b97750	1442	PWM1 = l_duty_cycle_x/100 ;
lakshya	10:f93407b97750	1443	}
lakshya	10:f93407b97750	1444	else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya	10:f93407b97750	1445	{
lakshya	10:f93407b97750	1446	l_duty_cycle_x = - 76880pow(l_current_x,3) + 1280.8pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1447	//// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya	10:f93407b97750	1448	PWM1.period(TIME_PERIOD);
lakshya	10:f93407b97750	1449	PWM1 = l_duty_cycle_x/100 ;
lakshya	10:f93407b97750	1450	}
lakshya	10:f93407b97750	1451	else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya	10:f93407b97750	1452	{
lakshya	10:f93407b97750	1453	l_duty_cycle_x = 275.92pow(l_current_x,2) + 546.13l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1454	//// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya	10:f93407b97750	1455	PWM1.period(TIME_PERIOD);
lakshya	10:f93407b97750	1456	PWM1 = l_duty_cycle_x/100 ;
lakshya	10:f93407b97750	1457	}
lakshya	10:f93407b97750	1458	else if(l_current_x==0)
lakshya	10:f93407b97750	1459	{
lakshya	20:949d13045431	1460	//// printf("\n \r l_current_x====0");
lakshya	10:f93407b97750	1461	l_duty_cycle_x = 0; // default value of duty cycle
lakshya	20:949d13045431	1462	//// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya	10:f93407b97750	1463	PWM1.period(TIME_PERIOD);
lakshya	10:f93407b97750	1464	PWM1 = l_duty_cycle_x/100 ;
lakshya	10:f93407b97750	1465	}
lakshya	10:f93407b97750	1466	else //not necessary
lakshya	10:f93407b97750	1467	{
lakshya	10:f93407b97750	1468	g_err_flag_TR_x = 1;
lakshya	20:949d13045431	1469	}
lakshya	20:949d13045431	1470	pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya	10:f93407b97750	1471
lakshya	10:f93407b97750	1472	//------------------------------------- y-direction TR--------------------------------------//
lakshya	10:f93407b97750	1473
lakshya	10:f93407b97750	1474
lakshya	10:f93407b97750	1475	float l_moment_y = Moment[1]; //Moment in y direction
lakshya	10:f93407b97750	1476
lakshya	10:f93407b97750	1477	phase_TR_y = 1; // setting the default current direction
lakshya	10:f93407b97750	1478	if (l_moment_y <0)
lakshya	10:f93407b97750	1479	{
lakshya	10:f93407b97750	1480	phase_TR_y = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
lakshya	10:f93407b97750	1481	l_moment_y = abs(l_moment_y);
lakshya	10:f93407b97750	1482	}
lakshya	10:f93407b97750	1483
lakshya	10:f93407b97750	1484
lakshya	10:f93407b97750	1485	l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya	20:949d13045431	1486	//// printf("current in try is %f \r \n",l_current_y);
lakshya	10:f93407b97750	1487	if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya	10:f93407b97750	1488	{
lakshya	10:f93407b97750	1489	l_duty_cycle_y = 310000000pow(l_current_y,3)- 90216pow(l_current_y,2) + 697.78l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1490	//// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya	10:f93407b97750	1491	PWM2.period(TIME_PERIOD);
lakshya	10:f93407b97750	1492	PWM2 = l_duty_cycle_y/100 ;
lakshya	10:f93407b97750	1493	}
lakshya	10:f93407b97750	1494	else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya	10:f93407b97750	1495	{
lakshya	10:f93407b97750	1496	l_duty_cycle_y = - 76880pow(l_current_y,3) + 1280.8pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1497	//// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya	10:f93407b97750	1498	PWM2.period(TIME_PERIOD);
lakshya	10:f93407b97750	1499	PWM2 = l_duty_cycle_y/100 ;
lakshya	10:f93407b97750	1500	}
lakshya	10:f93407b97750	1501	else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya	10:f93407b97750	1502	{
lakshya	10:f93407b97750	1503	l_duty_cycle_y = 275.92pow(l_current_y,2) + 546.13l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1504	//// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya	10:f93407b97750	1505	PWM2.period(TIME_PERIOD);
lakshya	10:f93407b97750	1506	PWM2 = l_duty_cycle_y/100 ;
lakshya	10:f93407b97750	1507	}
lakshya	10:f93407b97750	1508	else if(l_current_y==0)
lakshya	10:f93407b97750	1509	{
lakshya	20:949d13045431	1510	//// printf("\n \r l_current_y====0");
lakshya	10:f93407b97750	1511	l_duty_cycle_y = 0; // default value of duty cycle
lakshya	20:949d13045431	1512	//// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya	10:f93407b97750	1513	PWM2.period(TIME_PERIOD);
lakshya	10:f93407b97750	1514	PWM2 = l_duty_cycle_y/100 ;
lakshya	10:f93407b97750	1515	}
lakshya	10:f93407b97750	1516	else // not necessary
lakshya	10:f93407b97750	1517	{
lakshya	10:f93407b97750	1518	g_err_flag_TR_y = 1;
lakshya	10:f93407b97750	1519	}
lakshya	20:949d13045431	1520	pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya	10:f93407b97750	1521	//----------------------------------------------- z-direction TR -------------------------//
lakshya	10:f93407b97750	1522
lakshya	20:949d13045431	1523
lakshya	20:949d13045431	1524
lakshya	10:f93407b97750	1525	float l_moment_z = Moment[2]; //Moment in z direction
lakshya	10:f93407b97750	1526
lakshya	20:949d13045431	1527	phase_TR_z = 1; // setting the default current direction
lakshya	10:f93407b97750	1528	if (l_moment_z <0)
lakshya	10:f93407b97750	1529	{
lakshya	20:949d13045431	1530	phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high
lakshya	10:f93407b97750	1531	l_moment_z = abs(l_moment_z);
lakshya	10:f93407b97750	1532	}
lakshya	10:f93407b97750	1533
lakshya	10:f93407b97750	1534
lakshya	10:f93407b97750	1535	l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya	20:949d13045431	1536	//// printf("current in trz is %f \r \n",l_current_z);
lakshya	20:949d13045431	1537	if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
lakshya	10:f93407b97750	1538	{
lakshya	10:f93407b97750	1539	l_duty_cycle_z = 310000000pow(l_current_z,3)- 90216pow(l_current_z,2) + 697.78l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1540	//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya	10:f93407b97750	1541	PWM3.period(TIME_PERIOD);
lakshya	10:f93407b97750	1542	PWM3 = l_duty_cycle_z/100 ;
lakshya	10:f93407b97750	1543	}
lakshya	10:f93407b97750	1544	else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya	10:f93407b97750	1545	{
lakshya	10:f93407b97750	1546	l_duty_cycle_z = - 76880pow(l_current_z,3) + 1280.8pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1547	//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya	10:f93407b97750	1548	PWM3.period(TIME_PERIOD);
lakshya	10:f93407b97750	1549	PWM3 = l_duty_cycle_z/100 ;
lakshya	10:f93407b97750	1550	}
lakshya	10:f93407b97750	1551	else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya	10:f93407b97750	1552	{
lakshya	10:f93407b97750	1553	l_duty_cycle_z = 275.92pow(l_current_z,2) + 546.13l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya	20:949d13045431	1554	//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya	10:f93407b97750	1555	PWM3.period(TIME_PERIOD);
lakshya	10:f93407b97750	1556	PWM3 = l_duty_cycle_z/100 ;
lakshya	20:949d13045431	1557	}
lakshya	10:f93407b97750	1558	else if(l_current_z==0)
lakshya	10:f93407b97750	1559	{
lakshya	20:949d13045431	1560	//// printf("\n \r l_current_z====0");
lakshya	10:f93407b97750	1561	l_duty_cycle_z = 0; // default value of duty cycle
lakshya	20:949d13045431	1562	//// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya	10:f93407b97750	1563	PWM3.period(TIME_PERIOD);
lakshya	10:f93407b97750	1564	PWM3 = l_duty_cycle_z/100 ;
lakshya	10:f93407b97750	1565	}
lakshya	10:f93407b97750	1566	else // not necessary
lakshya	10:f93407b97750	1567	{
lakshya	20:949d13045431	1568	g_err_flag_TR_z = 1;
lakshya	20:949d13045431	1569	}
lakshya	20:949d13045431	1570	pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya	10:f93407b97750	1571
lakshya	20:949d13045431	1572	//changed
lakshya	20:949d13045431	1573	if(phase_TR_x)
lakshya	20:949d13045431	1574	ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
lakshya	20:949d13045431	1575	else
lakshya	20:949d13045431	1576	ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
lakshya	20:949d13045431	1577	if(phase_TR_y)
lakshya	20:949d13045431	1578	ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
lakshya	20:949d13045431	1579	else
lakshya	20:949d13045431	1580	ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
lakshya	20:949d13045431	1581	if(phase_TR_z)
lakshya	20:949d13045431	1582	ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
lakshya	20:949d13045431	1583	else
lakshya	20:949d13045431	1584	ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM2);
lakshya	20:949d13045431	1585
lakshya	10:f93407b97750	1586	//-----------------------------------------exiting the function-----------------------------------//
lakshya	10:f93407b97750	1587
lakshya	20:949d13045431	1588	//// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya	10:f93407b97750	1589
lakshya	10:f93407b97750	1590	}
lakshya	20:949d13045431	1591

Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	09 Mar 2017
Imports:	0
Forks:	0
Commits:	102
Dependents:	0
Dependencies:	3
Followers:	39

Important changes to repositories hosted on mbed.com

ACS.cpp@34:1b41c34b12ea, 2016-07-04 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

ACS.cpp@34:1b41c34b12ea, 2016-07-04 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning