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@20:949d13045431, 2016-07-01 (annotated)

Committer:: lakshya
Date:: Fri Jul 01 17:55:30 2016 +0000
Revision:: 20:949d13045431
Parent:: 19:79e69017c855
Child:: 34:1b41c34b12ea
Child:: 39:670133e7ffd8

BAE final 1.0 (1st july); ; BCN and EPS  code to be upgraded; +; testing and troubleshooting to be done ; +; watchdog to be implemented; ; comparing with flowcharts.

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

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@20:949d13045431, 2016-07-01 (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@20:949d13045431, 2016-07-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning