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@40:c2538d97e78b, 2016-07-05 (annotated)

Committer:: Bragadeesh153
Date:: Tue Jul 05 19:14:06 2016 +0000
Revision:: 40:c2538d97e78b
Parent:: 39:670133e7ffd8
Child:: 41:5df2bed2157d

In out updated , ACK change comm

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

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@40:c2538d97e78b, 2016-07-05 (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@40:c2538d97e78b, 2016-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning