Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

ACS.cpp@60:bd1498f03319, 2016-10-08 (annotated)

Committer:
Bragadeesh153
Date:
Sat Oct 08 09:19:13 2016 +0000
Revision:
60:bd1498f03319
Parent:
59:0fc0bfafaa9f
Child:
63:55d32e7dcad7
minor change to gyrorange

Who changed what in which revision?

UserRevisionLine numberNew 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>
lakshya 49:61c9f28332ba 5 Timer timer_SENSOR_DATA_ACQ;
lakshya 49:61c9f28332ba 6 Timer timer_controlmodes;
lakshya 49:61c9f28332ba 7 Timer timer_SENSOR_INIT;
lakshya 49:61c9f28332ba 8 Timer timer_CONFIG_UPLOAD;
lakshya 49:61c9f28332ba 9
lakshya 49:61c9f28332ba 10 Serial acs_pc(USBTX,USBRX);
sakthipriya 0:7b4c00e3912f 11
sakthipriya 0:7b4c00e3912f 12 #include "pni.h" //pni header file
sakthipriya 0:7b4c00e3912f 13 #include "pin_config.h"
lakshya 39:670133e7ffd8 14 #include "configuration.h"
sakthipriya 0:7b4c00e3912f 15 #include "ACS.h"
sakthipriya 6:036d08b62785 16 #include "EPS.h"
lakshya 20:949d13045431 17 /*variables will get get updated value from FLash
lakshya 20:949d13045431 18 in case flash cups while testing i.e initial defaul values are kept as of now
lakshya 20:949d13045431 19 */
sakthipriya 0:7b4c00e3912f 20 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 21 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 22 extern uint32_t BAE_ENABLE;
Bragadeesh153 13:fb7facaf308b 23 extern uint8_t ACS_INIT_STATUS;
Bragadeesh153 13:fb7facaf308b 24 extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153 13:fb7facaf308b 25 extern uint8_t ACS_ATS_STATUS;
Bragadeesh153 13:fb7facaf308b 26 extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153 13:fb7facaf308b 27 extern uint8_t ACS_STATUS;
lakshya 20:949d13045431 28 extern uint8_t ACS_DETUMBLING_ALGO_TYPE;//////
lakshya 20:949d13045431 29
lakshya 49:61c9f28332ba 30 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
lakshya 49:61c9f28332ba 31 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
sakthipriya 0:7b4c00e3912f 32
Bragadeesh153 13:fb7facaf308b 33 extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153 13:fb7facaf308b 34 extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153 13:fb7facaf308b 35 extern uint8_t ACS_STATE;
sakthipriya 0:7b4c00e3912f 36
lakshya 49:61c9f28332ba 37 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
lakshya 49:61c9f28332ba 38 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
lakshya 49:61c9f28332ba 39 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 40
sakthipriya 0:7b4c00e3912f 41 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 42 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 43 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 44
sakthipriya 0:7b4c00e3912f 45 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 46 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 47 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 48
sakthipriya 0:7b4c00e3912f 49 extern float data[6];
sakthipriya 6:036d08b62785 50 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 51
lakshya 20:949d13045431 52 //global para
lakshya 20:949d13045431 53 //FUNCTION
lakshya 20:949d13045431 54 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 55 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 56 float max_jm[9] = {0.3755,0.0176,0.2672,0.4895,0.2174,0.0452,1.0000,0.1209,0.0572};
lakshya 53:459b71b1861c 57 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 58 //se some other better way
lakshya 20:949d13045431 59 /*
lakshya 20:949d13045431 60 float max_bb[3] = {0,1.0*e-04*0.1633,1.0*e-04*0.1528};
lakshya 20:949d13045431 61 float min_bb[3] = {0,1.0*e-04*(-0.1736),1.0*e-04*(-0.1419)};
lakshya 20:949d13045431 62 */
lakshya 20:949d13045431 63 float max_bb[3] = {0,1.0*0.0001*0.1633,1.0*0.0001*0.1528};
lakshya 20:949d13045431 64 float min_bb[3] = {0,1.0*0.0001*(-0.1736),1.0*0.0001*(-0.1419)};
sakthipriya 0:7b4c00e3912f 65
lakshya 20:949d13045431 66 //ACS
lakshya 20:949d13045431 67 uint8_t controlmode_mms = 0;
lakshya 20:949d13045431 68 uint8_t ATS1_EVENT_STATUS_RGTR=0x00;
lakshya 20:949d13045431 69 uint8_t ATS1_SENTRAL_STATUS_RGTR=0x00;
lakshya 20:949d13045431 70 uint8_t ATS1_ERROR_RGTR=0x00;
lakshya 20:949d13045431 71 uint8_t ATS2_EVENT_STATUS_RGTR=0x00;
lakshya 20:949d13045431 72 uint8_t ATS2_SENTRAL_STATUS_RGTR=0x00;
lakshya 20:949d13045431 73 uint8_t ATS2_ERROR_RGTR=0x00;
lakshya 20:949d13045431 74 uint8_t invjm_mms[9];
lakshya 20:949d13045431 75 uint8_t jm_mms[9];
lakshya 20:949d13045431 76 uint8_t bb_mms[3];
lakshya 49:61c9f28332ba 77 uint8_t singularity_flag_mms=0;
lakshya 39:670133e7ffd8 78 uint8_t B_SCZ_ANGLE = 0x00;
lakshya 20:949d13045431 79 uint8_t ACS_MAG_TIME_DELAY;// = 65;
lakshya 20:949d13045431 80 uint8_t ACS_DEMAG_TIME_DELAY;// = 65;
lakshya 20:949d13045431 81 uint16_t ACS_Z_FIXED_MOMENT;// = 1.3;
lakshya 20:949d13045431 82 uint8_t ACS_TR_Z_SW_STATUS;//=1;
lakshya 20:949d13045431 83 uint8_t ACS_TR_XY_SW_STATUS;//=1;
lakshya 20:949d13045431 84 //GLOBAL PARA
lakshya 20:949d13045431 85 uint8_t ACS_TR_X_PWM; //*
lakshya 20:949d13045431 86 uint8_t ACS_TR_Y_PWM; //*
lakshya 20:949d13045431 87 uint8_t ACS_TR_Z_PWM; //*
lakshya 20:949d13045431 88 //change
lakshya 58:c4203e162d12 89 uint16_t ACS_MM_X_COMSN = 40;
lakshya 58:c4203e162d12 90 uint16_t ACS_MM_Y_COMSN = 50;
lakshya 58:c4203e162d12 91 uint16_t ACS_MG_X_COMSN = 40;
lakshya 58:c4203e162d12 92 uint16_t ACS_MG_Y_COMSN = 50;
lakshya 58:c4203e162d12 93 uint16_t ACS_MM_Z_COMSN = 60;
lakshya 58:c4203e162d12 94 uint16_t ACS_MG_Z_COMSN = 60;
lakshya 20:949d13045431 95
lakshya 20:949d13045431 96 uint8_t float_to_uint8(float min,float max,float val)
lakshya 20:949d13045431 97 {
lakshya 20:949d13045431 98 if(val>max)
lakshya 20:949d13045431 99 {return 0xff;
lakshya 20:949d13045431 100 }
lakshya 20:949d13045431 101 if(val<min)
lakshya 20:949d13045431 102 {return 0x00;
lakshya 20:949d13045431 103 }
lakshya 20:949d13045431 104 float div=max-min;div=(255.0/div);val=((val-min)*div);
Bragadeesh153 59:0fc0bfafaa9f 105 //printf("\n\n\n\rthe algo value is %d",val);
lakshya 20:949d13045431 106 return (uint8_t)val;
lakshya 20:949d13045431 107 }
lakshya 20:949d13045431 108
lakshya 20:949d13045431 109
lakshya 20:949d13045431 110 void float_to_uint8_ARRAY(int d1,int d2, float *arr,float max[], float min[], uint8_t *valarr)
lakshya 20:949d13045431 111 {
lakshya 20:949d13045431 112 for(int i=0;i<d1;i++)
lakshya 20:949d13045431 113 for(int j=0;j<d2;j++)
lakshya 20:949d13045431 114 {
lakshya 49:61c9f28332ba 115 acs_pc.printf("\n\r%f",*((arr+(i*d1))+j));
lakshya 20:949d13045431 116 valarr[i*d1+j] = (uint8_t)float_to_uint8(min[i*d1+j],max[i*d1+j],*((arr+(i*d1))+j));
lakshya 49:61c9f28332ba 117 acs_pc.printf("\n\r%d",valarr[i*d1+j]);
lakshya 20:949d13045431 118 }
lakshya 20:949d13045431 119 }
lakshya 20:949d13045431 120
sakthipriya 0:7b4c00e3912f 121
sakthipriya 0:7b4c00e3912f 122
lakshya 49:61c9f28332ba 123 //Serial pc(USBTX,USBRX); //for usb communication
lakshya 20:949d13045431 124
lakshya 10:f93407b97750 125 //CONTROL_ALGO
lakshya 20:949d13045431 126 float moment[3]; // Unit: Ampere*Meter^2//*
lakshya 10:f93407b97750 127 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
lakshya 49:61c9f28332ba 128 //float db[3];//*
lakshya 20:949d13045431 129 uint8_t flag_firsttime=1, alarmmode=0;
lakshya 10:f93407b97750 130
lakshya 49:61c9f28332ba 131 //void controllermodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t detumblingalgo);
lakshya 49:61c9f28332ba 132 void inversec(float mat[3][3],float inv[3][3],int *singularity_flag);
lakshya 49:61c9f28332ba 133 float max_array(float arr[3]);
lakshya 10:f93407b97750 134
lakshya 49:61c9f28332ba 135 void FCTN_ACS_CNTRLALGO (float moment[3], float b1[3], float omega1[3], uint8_t nominal , uint8_t detumbling , uint8_t ACS_DETUMBLING_ALGO_TYPE )
sakthipriya 0:7b4c00e3912f 136 {
lakshya 49:61c9f28332ba 137 float db1[3]; // Unit: Tesla/Second
lakshya 10:f93407b97750 138 float normalising_fact;
lakshya 49:61c9f28332ba 139 float b1_copy[3], omega1_copy[3], db1_copy[3];
lakshya 49:61c9f28332ba 140 int i, j;
lakshya 10:f93407b97750 141 if(flag_firsttime==1)
lakshya 10:f93407b97750 142 {
lakshya 10:f93407b97750 143 for(i=0;i<3;i++)
lakshya 10:f93407b97750 144 {
lakshya 49:61c9f28332ba 145 db1[i]=0; // Unit: Tesla/Second
lakshya 10:f93407b97750 146 }
lakshya 49:61c9f28332ba 147 flag_firsttime=0;
lakshya 10:f93407b97750 148 }
sakthipriya 0:7b4c00e3912f 149 else
sakthipriya 0:7b4c00e3912f 150 {
sakthipriya 0:7b4c00e3912f 151 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 152 {
lakshya 49:61c9f28332ba 153 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya 0:7b4c00e3912f 154 }
lakshya 10:f93407b97750 155 }
lakshya 10:f93407b97750 156
lakshya 49:61c9f28332ba 157 if(nominal == 0)
lakshya 49:61c9f28332ba 158 {
lakshya 49:61c9f28332ba 159 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
lakshya 49:61c9f28332ba 160 {
lakshya 49:61c9f28332ba 161 alarmmode=0;
lakshya 49:61c9f28332ba 162 }
lakshya 49:61c9f28332ba 163 else if(max_array(omega1)>OmegaMax && alarmmode==0)
lakshya 49:61c9f28332ba 164 {
lakshya 49:61c9f28332ba 165 alarmmode=1;
lakshya 49:61c9f28332ba 166 }
lakshya 49:61c9f28332ba 167
lakshya 49:61c9f28332ba 168 }
lakshya 49:61c9f28332ba 169
lakshya 10:f93407b97750 170 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 171 {
lakshya 49:61c9f28332ba 172 b1_copy[i]=b1[i];
lakshya 49:61c9f28332ba 173 db1_copy[i]=db1[i];
lakshya 49:61c9f28332ba 174 omega1_copy[i]=omega1[i];
sakthipriya 0:7b4c00e3912f 175 }
lakshya 10:f93407b97750 176
lakshya 20:949d13045431 177 if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
lakshya 10:f93407b97750 178 {
lakshya 49:61c9f28332ba 179 //*controlmode=0;
lakshya 49:61c9f28332ba 180 controlmode_mms =0;
lakshya 49:61c9f28332ba 181 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE);
lakshya 20:949d13045431 182 for (i=0;i<3;i++)
lakshya 20:949d13045431 183 {
lakshya 49:61c9f28332ba 184 b1[i]=b1_copy[i];
lakshya 49:61c9f28332ba 185 db1[i]=db1_copy[i];
lakshya 49:61c9f28332ba 186 omega1[i]=omega1_copy[i];
lakshya 20:949d13045431 187 }
lakshya 10:f93407b97750 188 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 189 {
lakshya 49:61c9f28332ba 190 //*controlmode=1;
lakshya 20:949d13045431 191 controlmode_mms = 1;
lakshya 49:61c9f28332ba 192 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE);
lakshya 49:61c9f28332ba 193 for (i=0;i<3;i++)
lakshya 49:61c9f28332ba 194 {
lakshya 49:61c9f28332ba 195 b1[i]=b1_copy[i];
lakshya 49:61c9f28332ba 196 db1[i]=db1_copy[i];
lakshya 49:61c9f28332ba 197 omega1[i]=omega1_copy[i];
lakshya 49:61c9f28332ba 198 }
lakshya 10:f93407b97750 199 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 200 {
lakshya 10:f93407b97750 201 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 202 for(i=0;i<3;i++)
lakshya 49:61c9f28332ba 203 {
lakshya 10:f93407b97750 204 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 49:61c9f28332ba 205 }
lakshya 10:f93407b97750 206 }
lakshya 10:f93407b97750 207 }
lakshya 49:61c9f28332ba 208
lakshya 49:61c9f28332ba 209 ACS_STATUS = 5;
lakshya 10:f93407b97750 210 }
lakshya 10:f93407b97750 211 else
lakshya 10:f93407b97750 212 {
lakshya 49:61c9f28332ba 213 //*controlmode=1;
lakshya 20:949d13045431 214 controlmode_mms = 1;
lakshya 49:61c9f28332ba 215 controllermodes(moment,b1,db1,omega1,controlmode_mms,ACS_DETUMBLING_ALGO_TYPE);
lakshya 49:61c9f28332ba 216 for (i=0;i<3;i++)
lakshya 49:61c9f28332ba 217 {
lakshya 49:61c9f28332ba 218 b1[i]=b1_copy[i];
lakshya 49:61c9f28332ba 219 db1[i]=db1_copy[i];
lakshya 49:61c9f28332ba 220 omega1[i]=omega1_copy[i];
lakshya 49:61c9f28332ba 221 }
lakshya 10:f93407b97750 222 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 223 {
lakshya 10:f93407b97750 224 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 225 for(i=0;i<3;i++)
lakshya 49:61c9f28332ba 226 {
lakshya 10:f93407b97750 227 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 49:61c9f28332ba 228 }
lakshya 10:f93407b97750 229 }
lakshya 10:f93407b97750 230
lakshya 10:f93407b97750 231 }
lakshya 10:f93407b97750 232 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 233 {
lakshya 49:61c9f28332ba 234 b_old[i]=b1[i];
sakthipriya 0:7b4c00e3912f 235 }
sakthipriya 0:7b4c00e3912f 236 }
lakshya 10:f93407b97750 237
lakshya 49:61c9f28332ba 238 void controllermodes(float moment[3], float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
sakthipriya 0:7b4c00e3912f 239 {
sakthipriya 0:7b4c00e3912f 240
lakshya 10:f93407b97750 241 float bb[3]={0,0,0};
lakshya 10:f93407b97750 242 float d[3]={0,0,0};
lakshya 10:f93407b97750 243 float den=0,den2;
lakshya 10:f93407b97750 244 float bcopy[3];
lakshya 10:f93407b97750 245 int i, j;//temporary variables
lakshya 10:f93407b97750 246 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya 10:f93407b97750 247 float invJm[3][3];
lakshya 49:61c9f28332ba 248
lakshya 20:949d13045431 249 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
lakshya 49:61c9f28332ba 250 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 49:61c9f28332ba 251
lakshya 49:61c9f28332ba 252 int singularity_flag = 0;
lakshya 49:61c9f28332ba 253 singularity_flag_mms=0;
lakshya 10:f93407b97750 254
lakshya 10:f93407b97750 255 if(controlmode1==0)
lakshya 10:f93407b97750 256 {
lakshya 10:f93407b97750 257 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
lakshya 10:f93407b97750 258 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
lakshya 20:949d13045431 259 if (den==0)
lakshya 10:f93407b97750 260 {
lakshya 49:61c9f28332ba 261 singularity_flag = 1;
lakshya 20:949d13045431 262 singularity_flag_mms=1;
lakshya 10:f93407b97750 263 }
lakshya 49:61c9f28332ba 264 if (singularity_flag==0)
lakshya 10:f93407b97750 265 {
lakshya 20:949d13045431 266 for(i=0;i<3;i++)
lakshya 20:949d13045431 267 {
lakshya 20:949d13045431 268 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
lakshya 20:949d13045431 269 }
lakshya 20:949d13045431 270 for(i=0;i<3;i++)
lakshya 20:949d13045431 271 {
lakshya 20:949d13045431 272 b[i]/=den; // Mormalized b. Hence no unit.
lakshya 20:949d13045431 273 }
lakshya 20:949d13045431 274 if(b[2]>0.9 || b[2]<-0.9)
lakshya 20:949d13045431 275 {
lakshya 20:949d13045431 276 kz=kz2;
lakshya 20:949d13045431 277 kmu=kmu2;
lakshya 20:949d13045431 278 gamma=gamma2;
lakshya 20:949d13045431 279 }
lakshya 20:949d13045431 280 for(i=0;i<2;i++)
lakshya 20:949d13045431 281 {
lakshya 20:949d13045431 282 Mu[i]=b[i];
lakshya 20:949d13045431 283 v[i]=-kmu*Mu[i];
lakshya 20:949d13045431 284 dv[i]=-kmu*db[i];
lakshya 20:949d13045431 285 z[i]=db[i]-v[i];
lakshya 20:949d13045431 286 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
lakshya 20:949d13045431 287 }
lakshya 49:61c9f28332ba 288 inversec(Jm,invJm,&singularity_flag);
lakshya 20:949d13045431 289 for(i=0;i<3;i++)
lakshya 20:949d13045431 290 {
lakshya 20:949d13045431 291 for(j=0;j<3;j++)
lakshya 20:949d13045431 292 {
lakshya 20:949d13045431 293 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 294 }
lakshya 20:949d13045431 295 }
lakshya 20:949d13045431 296 for(i=0;i<3;i++)
lakshya 10:f93407b97750 297 {
lakshya 20:949d13045431 298 for(j=0;j<3;j++)
lakshya 20:949d13045431 299 {
lakshya 20:949d13045431 300 d[i]+=bb[j]*invJm[i][j];
lakshya 20:949d13045431 301 }
lakshya 20:949d13045431 302 }
lakshya 49:61c9f28332ba 303 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])+(omega[1]*db[2])-(omega[2]*db[1]);
lakshya 49:61c9f28332ba 304 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])+(omega[2]*db[0])-(omega[0]*db[2]);
lakshya 20:949d13045431 305 bb[0]=0;
lakshya 20:949d13045431 306 for(i=0;i<3;i++)
lakshya 20:949d13045431 307 {
lakshya 20:949d13045431 308 d[i]=invJm[2][i];
lakshya 20:949d13045431 309 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
lakshya 20:949d13045431 310 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
lakshya 20:949d13045431 311 invJm[0][i]=b[i];
lakshya 10:f93407b97750 312 }
lakshya 49:61c9f28332ba 313 inversec(invJm,Jm,&singularity_flag);
lakshya 49:61c9f28332ba 314
lakshya 20:949d13045431 315 float_to_uint8_ARRAY(3,3, (float*)invJm,max_invjm, min_invjm, invjm_mms);
lakshya 20:949d13045431 316 float_to_uint8_ARRAY(3,3, (float*)Jm,max_jm, min_jm, jm_mms);
lakshya 20:949d13045431 317 float_to_uint8_ARRAY(1,3, (float*)bb,max_bb, min_bb, bb_mms);
lakshya 49:61c9f28332ba 318 singularity_flag_mms = singularity_flag;
lakshya 49:61c9f28332ba 319 if (singularity_flag==0)
lakshya 10:f93407b97750 320 {
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 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
lakshya 20:949d13045431 326 }
lakshya 20:949d13045431 327 }
lakshya 20:949d13045431 328 for(i=0;i<3;i++)
lakshya 20:949d13045431 329 {
lakshya 20:949d13045431 330 bcopy[i]=b[i]*den;
lakshya 20:949d13045431 331 }
lakshya 20:949d13045431 332 for(i=0;i<3;i++)
lakshya 20:949d13045431 333 {
lakshya 20:949d13045431 334 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
lakshya 20:949d13045431 335 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
lakshya 20:949d13045431 336 }
lakshya 10:f93407b97750 337 }
lakshya 10:f93407b97750 338 }
lakshya 49:61c9f28332ba 339 if (singularity_flag==1)
lakshya 10:f93407b97750 340 {
lakshya 20:949d13045431 341 for (i=0;i<3;i++)
lakshya 10:f93407b97750 342 {
lakshya 20:949d13045431 343 Mmnt[i]=2*MmntMax;
lakshya 10:f93407b97750 344 }
lakshya 10:f93407b97750 345 }
lakshya 49:61c9f28332ba 346 ACS_STATUS =5;
lakshya 10:f93407b97750 347 }
lakshya 10:f93407b97750 348 else if(controlmode1==1)
lakshya 10:f93407b97750 349 {
lakshya 20:949d13045431 350 if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
lakshya 10:f93407b97750 351 {
lakshya 20:949d13045431 352 for(i=0;i<3;i++)
lakshya 20:949d13045431 353 {
lakshya 20:949d13045431 354 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
lakshya 20:949d13045431 355 }
lakshya 20:949d13045431 356 ACS_STATUS = 6;
lakshya 20:949d13045431 357 }
lakshya 20:949d13045431 358 else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
lakshya 20:949d13045431 359 {
lakshya 20:949d13045431 360 for(i=0;i<3;i++)
lakshya 20:949d13045431 361 {
lakshya 20:949d13045431 362 Mmnt[i]=-kdetumble*db[i];
lakshya 20:949d13045431 363 }
lakshya 20:949d13045431 364 ACS_STATUS = 4;
lakshya 10:f93407b97750 365 }
lakshya 10:f93407b97750 366 }
lakshya 10:f93407b97750 367 for(i=0;i<3;i++)
lakshya 10:f93407b97750 368 {
lakshya 10:f93407b97750 369 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 370 }
lakshya 10:f93407b97750 371 }
sakthipriya 0:7b4c00e3912f 372
lakshya 49:61c9f28332ba 373 void inversec(float mat[3][3],float inv[3][3],int *singularity_flag)
lakshya 49:61c9f28332ba 374 {
lakshya 49:61c9f28332ba 375 int i,j;
lakshya 49:61c9f28332ba 376 float det=0;
lakshya 49:61c9f28332ba 377 for(i=0;i<3;i++)
lakshya 49:61c9f28332ba 378 {
lakshya 49:61c9f28332ba 379 for(j=0;j<3;j++)
lakshya 49:61c9f28332ba 380 {
lakshya 49:61c9f28332ba 381 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 49:61c9f28332ba 382 }
lakshya 49:61c9f28332ba 383 }
lakshya 49:61c9f28332ba 384 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
lakshya 49:61c9f28332ba 385 if (det==0)
lakshya 49:61c9f28332ba 386 {
lakshya 49:61c9f28332ba 387 *singularity_flag=1;
lakshya 49:61c9f28332ba 388 }
lakshya 49:61c9f28332ba 389 else
lakshya 49:61c9f28332ba 390 {
lakshya 49:61c9f28332ba 391 *singularity_flag=0;
lakshya 49:61c9f28332ba 392 for(i=0;i<3;i++)
lakshya 49:61c9f28332ba 393 {
lakshya 49:61c9f28332ba 394 for(j=0;j<3;j++)
lakshya 49:61c9f28332ba 395 {
lakshya 49:61c9f28332ba 396 inv[i][j]/=det;
lakshya 49:61c9f28332ba 397 }
lakshya 49:61c9f28332ba 398 }
lakshya 49:61c9f28332ba 399 }
lakshya 49:61c9f28332ba 400 }
lakshya 49:61c9f28332ba 401
lakshya 49:61c9f28332ba 402 float max_array(float arr[3])
lakshya 49:61c9f28332ba 403 {
lakshya 49:61c9f28332ba 404 int i;
lakshya 49:61c9f28332ba 405 float temp_max=fabs(arr[0]);
lakshya 49:61c9f28332ba 406 for(i=1;i<3;i++)
lakshya 49:61c9f28332ba 407 {
lakshya 49:61c9f28332ba 408 if(fabs(arr[i])>temp_max)
lakshya 49:61c9f28332ba 409 {
lakshya 49:61c9f28332ba 410 temp_max=fabs(arr[i]);
lakshya 49:61c9f28332ba 411 }
lakshya 49:61c9f28332ba 412 }
lakshya 49:61c9f28332ba 413 return temp_max;
lakshya 49:61c9f28332ba 414 }
lakshya 49:61c9f28332ba 415
lakshya 49:61c9f28332ba 416
lakshya 49:61c9f28332ba 417
sakthipriya 0:7b4c00e3912f 418 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 419
lakshya 20:949d13045431 420 int FCTN_ACS_INIT(); //initialization of registers happens
lakshya 20:949d13045431 421 int SENSOR_INIT();
lakshya 20:949d13045431 422 int FCTN_ATS_DATA_ACQ(); //data is obtained
lakshya 20:949d13045431 423 int SENSOR_DATA_ACQ();
lakshya 20:949d13045431 424 //void T_OUT(); //timeout function to stop infinite loop
lakshya 20:949d13045431 425
lakshya 20:949d13045431 426 int CONFIG_UPLOAD();
lakshya 20:949d13045431 427 //Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 428 int toFlag;
sakthipriya 0:7b4c00e3912f 429
sakthipriya 0:7b4c00e3912f 430 int count =0; // Time for which the BAE uC is running (in seconds)
lakshya 20:949d13045431 431 //void T_OUT()
lakshya 20:949d13045431 432 //{
lakshya 20:949d13045431 433 // toFlag=0; //as T_OUT function gets called the while loop gets terminated
lakshya 20:949d13045431 434 //}
sakthipriya 0:7b4c00e3912f 435
sakthipriya 0:7b4c00e3912f 436
sakthipriya 0:7b4c00e3912f 437 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 438 char cmd[2];
sakthipriya 0:7b4c00e3912f 439 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 440 char raw_mag[6];
lakshya 20:949d13045431 441 char reg_data[24];
sakthipriya 0:7b4c00e3912f 442 char store,status;
lakshya 20:949d13045431 443 //int16_t bit_data done in actual_data structure itself;
sakthipriya 0:7b4c00e3912f 444
lakshya 20:949d13045431 445 uint16_t time_data;
lakshya 20:949d13045431 446 float gyro_data[3], mag_data[3];
lakshya 20:949d13045431 447 //float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
lakshya 20:949d13045431 448
lakshya 20:949d13045431 449 int ack;
lakshya 20:949d13045431 450 int CONFIG_UPLOAD()
lakshya 39:670133e7ffd8 451 {
lakshya 39:670133e7ffd8 452 uint8_t value;
lakshya 39:670133e7ffd8 453
lakshya 20:949d13045431 454 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 455 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 456 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
lakshya 49:61c9f28332ba 457 wait_ms(275);
lakshya 49:61c9f28332ba 458 //kick_WDOG();
lakshya 49:61c9f28332ba 459 acs_pc.printf("\n\r lvl5");
lakshya 49:61c9f28332ba 460 wait_ms(300);
lakshya 20:949d13045431 461 //Verify magic number
lakshya 20:949d13045431 462
lakshya 39:670133e7ffd8 463
lakshya 39:670133e7ffd8 464 cmd[0]=SENTRALSTATUS;
lakshya 39:670133e7ffd8 465 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 39:670133e7ffd8 466 i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 39:670133e7ffd8 467 value = (uint8_t)store;
lakshya 39:670133e7ffd8 468
lakshya 39:670133e7ffd8 469 if(value & 0x02)
lakshya 39:670133e7ffd8 470 {
lakshya 49:61c9f28332ba 471 acs_pc.printf("Sentral already has eeprom firmware loaded.\n");
lakshya 39:670133e7ffd8 472 }
lakshya 39:670133e7ffd8 473 /* Write value 0x01 to the ResetReq register, address 0x9B. This will result
lakshya 39:670133e7ffd8 474 in a hard reset of the Sentral. This is unnecessary if the prior event was
lakshya 39:670133e7ffd8 475 a Reset. */
lakshya 39:670133e7ffd8 476 if(!(value & 0x08))
lakshya 39:670133e7ffd8 477 {
lakshya 49:61c9f28332ba 478 acs_pc.printf("CPU is not in standby, issuing a shutdown request.\n");
lakshya 39:670133e7ffd8 479 //i2c_write(I2C_SLAVE_ADDR, 0x34, data, 1);
lakshya 39:670133e7ffd8 480 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to shut down
lakshya 39:670133e7ffd8 481 cmd[1]=0x00;
lakshya 39:670133e7ffd8 482 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 39:670133e7ffd8 483
lakshya 39:670133e7ffd8 484 int cnt=0;
lakshya 39:670133e7ffd8 485 do {
lakshya 39:670133e7ffd8 486 cmd[0]=SENTRALSTATUS;
lakshya 39:670133e7ffd8 487 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 39:670133e7ffd8 488 i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 39:670133e7ffd8 489 value = (uint8_t)store;
lakshya 39:670133e7ffd8 490 wait_ms(100);
lakshya 39:670133e7ffd8 491 cnt++;
lakshya 39:670133e7ffd8 492 } while((!(value & 0x08))&&(cnt<4));
lakshya 39:670133e7ffd8 493
lakshya 39:670133e7ffd8 494 if(cnt==4)
lakshya 39:670133e7ffd8 495 {
lakshya 39:670133e7ffd8 496 return 0;
lakshya 39:670133e7ffd8 497 }
lakshya 39:670133e7ffd8 498 }
lakshya 39:670133e7ffd8 499
lakshya 20:949d13045431 500 cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
lakshya 20:949d13045431 501 cmd[1]=BIT_HOST_UPLD_ENB;
lakshya 20:949d13045431 502 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 503 wait_ms(20);
lakshya 20:949d13045431 504
lakshya 49:61c9f28332ba 505 //kick_WDOG();
lakshya 49:61c9f28332ba 506 acs_pc.printf("\n\r lvl6");
lakshya 49:61c9f28332ba 507
lakshya 39:670133e7ffd8 508 cmd[0]=UPLOAD_ADDR; //0x0000 is written in RAM register to enable upload
lakshya 20:949d13045431 509 cmd[1]=0x0000;
lakshya 20:949d13045431 510 i2c.write(SLAVE_ADDR,cmd,3);
lakshya 39:670133e7ffd8 511 wait_ms(100);
lakshya 39:670133e7ffd8 512
lakshya 49:61c9f28332ba 513 acs_pc.printf("Uploading data...\n");
lakshya 39:670133e7ffd8 514
lakshya 39:670133e7ffd8 515 #define TRASACTION_SIZE 3
lakshya 39:670133e7ffd8 516
lakshya 39:670133e7ffd8 517
lakshya 39:670133e7ffd8 518 for(int i = 0; i < EEPROMTextLength; i += TRASACTION_SIZE * 4)
lakshya 39:670133e7ffd8 519 {
lakshya 39:670133e7ffd8 520
lakshya 39:670133e7ffd8 521 char* data = new char[TRASACTION_SIZE * 4];
lakshya 39:670133e7ffd8 522 data[0]=0x96;
lakshya 39:670133e7ffd8 523 for(int j = 0; j < TRASACTION_SIZE; j++)
lakshya 39:670133e7ffd8 524 {
lakshya 39:670133e7ffd8 525 data[j * 4 + 1] = configdata[i + j * 4 + 3];
lakshya 39:670133e7ffd8 526 data[j * 4 + 2] = configdata[i + j * 4 + 2];
lakshya 39:670133e7ffd8 527 data[j * 4 + 3] = configdata[i + j * 4 + 1];
lakshya 39:670133e7ffd8 528 data[j * 4 + 4] = configdata[i + j * 4 + 0];
lakshya 39:670133e7ffd8 529 }
lakshya 39:670133e7ffd8 530
lakshya 39:670133e7ffd8 531 if(EEPROMTextLength < (i + (TRASACTION_SIZE * 4)))
lakshya 39:670133e7ffd8 532 {
lakshya 39:670133e7ffd8 533 uint32_t bytes = EEPROMTextLength - i;
lakshya 39:670133e7ffd8 534 i2c.write(SLAVE_ADDR,data,bytes+1);
lakshya 39:670133e7ffd8 535 }
lakshya 39:670133e7ffd8 536
lakshya 39:670133e7ffd8 537 else
lakshya 39:670133e7ffd8 538 {
lakshya 39:670133e7ffd8 539 /* Write the Configuration File to Sentral’s program RAM. The file is sent
lakshya 39:670133e7ffd8 540 one byte at a time, using the UploadData register, register address 0x96. */
lakshya 39:670133e7ffd8 541 i2c.write(SLAVE_ADDR,data,13);
lakshya 39:670133e7ffd8 542 }
lakshya 39:670133e7ffd8 543 delete data;
lakshya 39:670133e7ffd8 544 }
lakshya 39:670133e7ffd8 545
lakshya 39:670133e7ffd8 546 char crc[4];
lakshya 39:670133e7ffd8 547 cmd[0]=0x97;
lakshya 39:670133e7ffd8 548 i2c.write(SLAVE_ADDR,cmd,1);
lakshya 39:670133e7ffd8 549 i2c.read(SLAVE_ADDR_READ,crc,4);
lakshya 39:670133e7ffd8 550 value = (uint8_t)store;
lakshya 39:670133e7ffd8 551
lakshya 39:670133e7ffd8 552 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 553
lakshya 39:670133e7ffd8 554 if(actualCRC != EEPROMTextCRC)
lakshya 39:670133e7ffd8 555 {
lakshya 49:61c9f28332ba 556 acs_pc.printf("Program crc (0x%.8X) does not match CRC reported by Sentral (0x%0.8X)\n", EEPROMTextCRC, actualCRC);
lakshya 39:670133e7ffd8 557 return 0;
lakshya 39:670133e7ffd8 558 }
lakshya 39:670133e7ffd8 559 else
lakshya 39:670133e7ffd8 560 {
lakshya 49:61c9f28332ba 561 acs_pc.printf("Firmware Upload Complete.\n");
lakshya 39:670133e7ffd8 562 return 1;
lakshya 39:670133e7ffd8 563 }
lakshya 39:670133e7ffd8 564
lakshya 20:949d13045431 565 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
lakshya 20:949d13045431 566 cmd[1]=0x00;
lakshya 20:949d13045431 567 i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 568 wait_ms(20);
lakshya 20:949d13045431 569
lakshya 20:949d13045431 570 return 0;
lakshya 20:949d13045431 571 }
lakshya 20:949d13045431 572
Bragadeesh153 60:bd1498f03319 573 int PARAMETER_TRANSFER_GYRO(int gyro_rng)
Bragadeesh153 60:bd1498f03319 574 {
Bragadeesh153 60:bd1498f03319 575
Bragadeesh153 60:bd1498f03319 576 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 577 if(gyro_rng ==0)
Bragadeesh153 60:bd1498f03319 578 return 0;
Bragadeesh153 60:bd1498f03319 579 char range[5];
Bragadeesh153 60:bd1498f03319 580
Bragadeesh153 60:bd1498f03319 581
Bragadeesh153 60:bd1498f03319 582 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 583 range[1] = 0x4B;
Bragadeesh153 60:bd1498f03319 584 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 585 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 586
Bragadeesh153 60:bd1498f03319 587 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 588 range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 589 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 590
Bragadeesh153 60:bd1498f03319 591 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 592 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 593 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 594
Bragadeesh153 60:bd1498f03319 595 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 596
Bragadeesh153 60:bd1498f03319 597 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 598 {
Bragadeesh153 60:bd1498f03319 599 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 600 return 0;
Bragadeesh153 60:bd1498f03319 601 }
Bragadeesh153 60:bd1498f03319 602
Bragadeesh153 60:bd1498f03319 603 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 604 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 605 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 606
Bragadeesh153 60:bd1498f03319 607 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]);
Bragadeesh153 60:bd1498f03319 608
Bragadeesh153 60:bd1498f03319 609 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 610 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 611 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 612
Bragadeesh153 60:bd1498f03319 613 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 614 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 615 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 616 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 617
Bragadeesh153 60:bd1498f03319 618
Bragadeesh153 60:bd1498f03319 619
Bragadeesh153 60:bd1498f03319 620 range[0] = 0x60;
Bragadeesh153 60:bd1498f03319 621
Bragadeesh153 60:bd1498f03319 622
Bragadeesh153 60:bd1498f03319 623 range[1] = (int)(gyro_rng%256); //1000
Bragadeesh153 60:bd1498f03319 624 range[2] = (int)(gyro_rng/256);
Bragadeesh153 60:bd1498f03319 625
Bragadeesh153 60:bd1498f03319 626 //range[1]= 0x88;
Bragadeesh153 60:bd1498f03319 627 //range[2] = 0x13; //5000
Bragadeesh153 60:bd1498f03319 628
Bragadeesh153 60:bd1498f03319 629 range[3] = 0x00;
Bragadeesh153 60:bd1498f03319 630 range[4] = 0x00;
Bragadeesh153 60:bd1498f03319 631 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 632
Bragadeesh153 60:bd1498f03319 633 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 634 range[1] = 0xCB;
Bragadeesh153 60:bd1498f03319 635 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 636
Bragadeesh153 60:bd1498f03319 637
Bragadeesh153 60:bd1498f03319 638 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 639 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 640 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 641
Bragadeesh153 60:bd1498f03319 642 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 643 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 644 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 645
Bragadeesh153 60:bd1498f03319 646 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 647
Bragadeesh153 60:bd1498f03319 648 if(range[0] != ((char)(0xCB)))
Bragadeesh153 60:bd1498f03319 649 {
Bragadeesh153 60:bd1498f03319 650 printf("Parameter transfer failed \r\n");
Bragadeesh153 60:bd1498f03319 651 return 0;
Bragadeesh153 60:bd1498f03319 652 }
Bragadeesh153 60:bd1498f03319 653
Bragadeesh153 60:bd1498f03319 654 printf("Parameter transfer success\r\n");
Bragadeesh153 60:bd1498f03319 655 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 656 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 657 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 658 //i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 659 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 660
Bragadeesh153 60:bd1498f03319 661 // range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 662 // range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 663 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 664
Bragadeesh153 60:bd1498f03319 665
Bragadeesh153 60:bd1498f03319 666 printf("Reading parameter\r\n");
Bragadeesh153 60:bd1498f03319 667 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 668
Bragadeesh153 60:bd1498f03319 669 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 670 range[1] = 0x4B;
Bragadeesh153 60:bd1498f03319 671 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 672 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 673
Bragadeesh153 60:bd1498f03319 674 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 675 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 676 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 677
Bragadeesh153 60:bd1498f03319 678 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 679 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 680 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 681
Bragadeesh153 60:bd1498f03319 682 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 683
Bragadeesh153 60:bd1498f03319 684 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 685 {
Bragadeesh153 60:bd1498f03319 686 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 687 return 0;
Bragadeesh153 60:bd1498f03319 688 }
Bragadeesh153 60:bd1498f03319 689
Bragadeesh153 60:bd1498f03319 690 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 691 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 692 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 693
Bragadeesh153 60:bd1498f03319 694 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]);
Bragadeesh153 60:bd1498f03319 695
Bragadeesh153 60:bd1498f03319 696 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 697 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 698 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 699
Bragadeesh153 60:bd1498f03319 700 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 701 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 702 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 703
Bragadeesh153 60:bd1498f03319 704
Bragadeesh153 60:bd1498f03319 705 return 1;
Bragadeesh153 60:bd1498f03319 706
Bragadeesh153 60:bd1498f03319 707 }
Bragadeesh153 60:bd1498f03319 708
Bragadeesh153 60:bd1498f03319 709 int PARAMETER_TRANSFER_MAG(int mag_rng)
Bragadeesh153 60:bd1498f03319 710 {
Bragadeesh153 60:bd1498f03319 711
Bragadeesh153 60:bd1498f03319 712 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 713 if(mag_rng ==0)
Bragadeesh153 60:bd1498f03319 714 return 0;
Bragadeesh153 60:bd1498f03319 715 char range[5];
Bragadeesh153 60:bd1498f03319 716
Bragadeesh153 60:bd1498f03319 717
Bragadeesh153 60:bd1498f03319 718 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 719 range[1] = 0x4A;
Bragadeesh153 60:bd1498f03319 720 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 721 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 722
Bragadeesh153 60:bd1498f03319 723 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 724 range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 725 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 726
Bragadeesh153 60:bd1498f03319 727 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 728 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 729 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 730
Bragadeesh153 60:bd1498f03319 731 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 732
Bragadeesh153 60:bd1498f03319 733 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 734 {
Bragadeesh153 60:bd1498f03319 735 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 736 return 0;
Bragadeesh153 60:bd1498f03319 737 }
Bragadeesh153 60:bd1498f03319 738
Bragadeesh153 60:bd1498f03319 739 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 740 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 741 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 742
Bragadeesh153 60:bd1498f03319 743 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]);
Bragadeesh153 60:bd1498f03319 744
Bragadeesh153 60:bd1498f03319 745 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 746 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 747 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 748
Bragadeesh153 60:bd1498f03319 749 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 750 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 751 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 752 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 753
Bragadeesh153 60:bd1498f03319 754
Bragadeesh153 60:bd1498f03319 755
Bragadeesh153 60:bd1498f03319 756 range[0] = 0x60;
Bragadeesh153 60:bd1498f03319 757
Bragadeesh153 60:bd1498f03319 758
Bragadeesh153 60:bd1498f03319 759 range[1] = (int)(mag_rng%256); //1000
Bragadeesh153 60:bd1498f03319 760 range[2] = (int)(mag_rng/256);
Bragadeesh153 60:bd1498f03319 761
Bragadeesh153 60:bd1498f03319 762 //range[1]= 0x88;
Bragadeesh153 60:bd1498f03319 763 //range[2] = 0x13; //5000
Bragadeesh153 60:bd1498f03319 764
Bragadeesh153 60:bd1498f03319 765 range[3] = 0x00;
Bragadeesh153 60:bd1498f03319 766 range[4] = 0x00;
Bragadeesh153 60:bd1498f03319 767 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 768
Bragadeesh153 60:bd1498f03319 769 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 770 range[1] = 0xCA;
Bragadeesh153 60:bd1498f03319 771 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 772
Bragadeesh153 60:bd1498f03319 773
Bragadeesh153 60:bd1498f03319 774 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 775 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 776 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 777
Bragadeesh153 60:bd1498f03319 778 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 779 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 780 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 781
Bragadeesh153 60:bd1498f03319 782 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 783
Bragadeesh153 60:bd1498f03319 784 if(range[0] != ((char)(0xCB)))
Bragadeesh153 60:bd1498f03319 785 {
Bragadeesh153 60:bd1498f03319 786 printf("Parameter transfer failed \r\n");
Bragadeesh153 60:bd1498f03319 787 return 0;
Bragadeesh153 60:bd1498f03319 788 }
Bragadeesh153 60:bd1498f03319 789
Bragadeesh153 60:bd1498f03319 790 printf("Parameter transfer success\r\n");
Bragadeesh153 60:bd1498f03319 791 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 792 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 793 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 794 //i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 795 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 796
Bragadeesh153 60:bd1498f03319 797 // range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 798 // range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 799 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 800
Bragadeesh153 60:bd1498f03319 801
Bragadeesh153 60:bd1498f03319 802 printf("Reading parameter\r\n");
Bragadeesh153 60:bd1498f03319 803 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 804
Bragadeesh153 60:bd1498f03319 805 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 806 range[1] = 0x4A;
Bragadeesh153 60:bd1498f03319 807 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 808 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 809
Bragadeesh153 60:bd1498f03319 810 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 811 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 812 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 813
Bragadeesh153 60:bd1498f03319 814 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 815 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 816 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 817
Bragadeesh153 60:bd1498f03319 818 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 819
Bragadeesh153 60:bd1498f03319 820 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 821 {
Bragadeesh153 60:bd1498f03319 822 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 823 return 0;
Bragadeesh153 60:bd1498f03319 824 }
Bragadeesh153 60:bd1498f03319 825
Bragadeesh153 60:bd1498f03319 826 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 827 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 828 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 829
Bragadeesh153 60:bd1498f03319 830 printf("range[0] range[1] range[2] range[3] : %.02X %.02X %.02X %.02X \r\n",range[0] ,range[1] , range[2] ,range[3]);
Bragadeesh153 60:bd1498f03319 831
Bragadeesh153 60:bd1498f03319 832 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 833 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 834 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 835
Bragadeesh153 60:bd1498f03319 836 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 837 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 838 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 839
Bragadeesh153 60:bd1498f03319 840
Bragadeesh153 60:bd1498f03319 841 return 1;
Bragadeesh153 60:bd1498f03319 842
Bragadeesh153 60:bd1498f03319 843 }
Bragadeesh153 60:bd1498f03319 844
lakshya 20:949d13045431 845 int SENSOR_INIT()
lakshya 20:949d13045431 846 {
lakshya 49:61c9f28332ba 847 /// acs_pc.printf("Entered sensor init\n \r");
lakshya 20:949d13045431 848 cmd[0]=RESETREQ;
lakshya 20:949d13045431 849 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 850 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 851 //wait_ms(575); //waiting for loading configuration file stored in EEPROM
lakshya 20:949d13045431 852
lakshya 49:61c9f28332ba 853 /// acs_pc.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
lakshya 20:949d13045431 854
lakshya 20:949d13045431 855 if( ack!=0)
lakshya 20:949d13045431 856 {
lakshya 20:949d13045431 857 cmd[0]=RESETREQ;
lakshya 20:949d13045431 858 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 859 ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
lakshya 20:949d13045431 860 if(ack !=0)
lakshya 20:949d13045431 861 return 0;
lakshya 20:949d13045431 862 }
lakshya 20:949d13045431 863
lakshya 49:61c9f28332ba 864
lakshya 49:61c9f28332ba 865 wait_ms(275);
lakshya 49:61c9f28332ba 866 //kick_WDOG();
lakshya 49:61c9f28332ba 867 acs_pc.printf("\n\r lvl2");
lakshya 49:61c9f28332ba 868 wait_ms(300);
lakshya 20:949d13045431 869
sakthipriya 0:7b4c00e3912f 870 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 871 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 872
lakshya 20:949d13045431 873 if( ack!=0)
lakshya 20:949d13045431 874 {
lakshya 20:949d13045431 875 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 876 if(ack!=0)
lakshya 20:949d13045431 877 return 0;
lakshya 20:949d13045431 878 }
lakshya 20:949d13045431 879
lakshya 20:949d13045431 880 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 881
lakshya 20:949d13045431 882 if( ack!=0)
lakshya 20:949d13045431 883 {
lakshya 20:949d13045431 884 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 885 if(ack!=0)
lakshya 20:949d13045431 886 return 0;
lakshya 20:949d13045431 887 }
lakshya 20:949d13045431 888
lakshya 49:61c9f28332ba 889 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
lakshya 20:949d13045431 890
lakshya 20:949d13045431 891 //to check whether EEPROM is uploaded properly
sakthipriya 0:7b4c00e3912f 892 switch((int)store) {
lakshya 20:949d13045431 893 case(3): {
sakthipriya 0:7b4c00e3912f 894 break;
sakthipriya 0:7b4c00e3912f 895 }
sakthipriya 0:7b4c00e3912f 896 case(11): {
sakthipriya 0:7b4c00e3912f 897 break;
sakthipriya 0:7b4c00e3912f 898 }
sakthipriya 0:7b4c00e3912f 899 default: {
sakthipriya 0:7b4c00e3912f 900 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 901 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 902 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 903 if( ack!=0)
lakshya 20:949d13045431 904 {
lakshya 20:949d13045431 905 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 906 if(ack!=0)
lakshya 20:949d13045431 907 return 0;
lakshya 20:949d13045431 908 }
lakshya 49:61c9f28332ba 909 wait_ms(275);//should be 600
lakshya 49:61c9f28332ba 910 //kick_WDOG();
lakshya 49:61c9f28332ba 911 acs_pc.printf("\n\r lvl3");
lakshya 49:61c9f28332ba 912 wait_ms(300);
lakshya 20:949d13045431 913 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 914 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 915 if( ack!=0)
lakshya 20:949d13045431 916 {
lakshya 20:949d13045431 917 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 918 if(ack!=0)
lakshya 20:949d13045431 919 return 0;
lakshya 20:949d13045431 920 }
lakshya 20:949d13045431 921 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 922 if( ack!=0)
lakshya 20:949d13045431 923 {
lakshya 20:949d13045431 924 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 925 if(ack!=0)
lakshya 20:949d13045431 926 return 0;
lakshya 20:949d13045431 927 }
lakshya 49:61c9f28332ba 928 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
lakshya 20:949d13045431 929
sakthipriya 0:7b4c00e3912f 930 }
sakthipriya 0:7b4c00e3912f 931 }
lakshya 20:949d13045431 932
lakshya 20:949d13045431 933 int manual=0;
lakshya 20:949d13045431 934 if( ((int)store != 11 )&&((int)store != 3))
lakshya 20:949d13045431 935 {
lakshya 20:949d13045431 936
lakshya 20:949d13045431 937 cmd[0]=RESETREQ;
lakshya 20:949d13045431 938 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 939 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 940 if( ack!=0)
lakshya 20:949d13045431 941 {
lakshya 20:949d13045431 942 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 943 if(ack!=0)
lakshya 20:949d13045431 944 return 0;
lakshya 20:949d13045431 945 }
lakshya 49:61c9f28332ba 946 wait_ms(275);
lakshya 49:61c9f28332ba 947 //kick_WDOG();
lakshya 49:61c9f28332ba 948 acs_pc.printf("\n\r lvl4");
lakshya 49:61c9f28332ba 949 wait_ms(300);
lakshya 20:949d13045431 950
lakshya 49:61c9f28332ba 951 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 952 manual = CONFIG_UPLOAD();
lakshya 49:61c9f28332ba 953 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 954
lakshya 20:949d13045431 955 if(manual == 0)
lakshya 20:949d13045431 956 {
lakshya 20:949d13045431 957 //MANUAL CONFIGURATION FAILED
lakshya 20:949d13045431 958 return 0;
lakshya 20:949d13045431 959 }
lakshya 20:949d13045431 960
lakshya 20:949d13045431 961 }
lakshya 20:949d13045431 962 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
lakshya 20:949d13045431 963 cmd[1]=BIT_RUN_ENB;
lakshya 20:949d13045431 964 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 965 if( ack!=0)
lakshya 20:949d13045431 966 {
lakshya 20:949d13045431 967 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 968 if(ack!=0)
lakshya 20:949d13045431 969 return 0;
lakshya 20:949d13045431 970 }
lakshya 20:949d13045431 971
lakshya 20:949d13045431 972 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
lakshya 20:949d13045431 973 cmd[1]=BIT_MAGODR;
lakshya 20:949d13045431 974 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 975 if( ack!=0)
lakshya 20:949d13045431 976 {
lakshya 20:949d13045431 977 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 978 if(ack!=0)
lakshya 20:949d13045431 979 return 0;
lakshya 20:949d13045431 980 }
lakshya 20:949d13045431 981
lakshya 20:949d13045431 982 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
lakshya 20:949d13045431 983 cmd[1]=BIT_GYROODR;
lakshya 20:949d13045431 984 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 985 if( ack!=0)
lakshya 20:949d13045431 986 {
lakshya 20:949d13045431 987 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 988 if(ack!=0)
lakshya 20:949d13045431 989 return 0;
lakshya 20:949d13045431 990 }
lakshya 20:949d13045431 991
lakshya 20:949d13045431 992 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
lakshya 20:949d13045431 993 cmd[1]=0x00;
lakshya 20:949d13045431 994 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 995 if( ack!=0)
lakshya 20:949d13045431 996 {
lakshya 20:949d13045431 997 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 998 if(ack!=0)
lakshya 20:949d13045431 999 return 0;
lakshya 20:949d13045431 1000 }
lakshya 20:949d13045431 1001 //wait_ms(20);
lakshya 20:949d13045431 1002 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
lakshya 20:949d13045431 1003 cmd[1]=0x00;
lakshya 20:949d13045431 1004 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1005 if( ack!=0)
lakshya 20:949d13045431 1006 {
lakshya 20:949d13045431 1007 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1008 if(ack!=0)
lakshya 20:949d13045431 1009 return 0;
lakshya 20:949d13045431 1010 }
lakshya 20:949d13045431 1011
lakshya 20:949d13045431 1012 cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
lakshya 20:949d13045431 1013 cmd[1]=BIT_EVT_ENB;
lakshya 20:949d13045431 1014 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1015 if( ack!=0)
lakshya 20:949d13045431 1016 {
lakshya 20:949d13045431 1017 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1018 if(ack!=0)
lakshya 20:949d13045431 1019 return 0;
lakshya 20:949d13045431 1020 }
lakshya 20:949d13045431 1021
Bragadeesh153 60:bd1498f03319 1022 PARAMETER_TRANSFER_GYRO(0); // dont set range //PARAMETER of function sets range of gyroscope
Bragadeesh153 60:bd1498f03319 1023 PARAMETER_TRANSFER_MAG(0); // dont set range //PARAMETER of function sets range of magnetometer
Bragadeesh153 60:bd1498f03319 1024
lakshya 20:949d13045431 1025 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1026 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1027 if( ack!=0)
lakshya 20:949d13045431 1028 {
lakshya 20:949d13045431 1029 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1030 if(ack!=0)
lakshya 20:949d13045431 1031 return 0;
lakshya 20:949d13045431 1032 }
lakshya 20:949d13045431 1033
lakshya 20:949d13045431 1034 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 1035 if( ack!=0)
lakshya 20:949d13045431 1036 {
lakshya 20:949d13045431 1037 ack= i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 1038 if(ack!=0)
lakshya 20:949d13045431 1039 return 0;
lakshya 20:949d13045431 1040 }
lakshya 20:949d13045431 1041
lakshya 49:61c9f28332ba 1042 /// acs_pc.printf("Sentral Status after initialising is %x\n \r",(int)store);
lakshya 20:949d13045431 1043
lakshya 20:949d13045431 1044 if( (int)store == 3) //Check if initialised properly and not in idle state
lakshya 20:949d13045431 1045 {
lakshya 49:61c9f28332ba 1046 /// acs_pc.printf("Exited sensor init successfully\n \r");
lakshya 20:949d13045431 1047 return 1;
lakshya 20:949d13045431 1048 }
lakshya 20:949d13045431 1049
lakshya 20:949d13045431 1050
lakshya 49:61c9f28332ba 1051 //// acs_pc.printf("Sensor init failed \n \r") ;
lakshya 20:949d13045431 1052 return 0;
lakshya 20:949d13045431 1053 }
lakshya 20:949d13045431 1054
lakshya 20:949d13045431 1055 int FCTN_ACS_INIT()
lakshya 20:949d13045431 1056 {
lakshya 20:949d13045431 1057 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
lakshya 20:949d13045431 1058
lakshya 20:949d13045431 1059 int working=0;
lakshya 20:949d13045431 1060
lakshya 49:61c9f28332ba 1061 /// acs_pc.printf("Attitude sensor init called \n \r");
lakshya 49:61c9f28332ba 1062 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 17:fc782f7548c6 1063
lakshya 17:fc782f7548c6 1064
lakshya 20:949d13045431 1065 if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 1066 {
lakshya 20:949d13045431 1067
lakshya 49:61c9f28332ba 1068 /// acs_pc.printf("Sensor 1 marked working \n \r");
lakshya 49:61c9f28332ba 1069 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1070 working = SENSOR_INIT();
lakshya 49:61c9f28332ba 1071 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 1072 if(working ==1)
lakshya 20:949d13045431 1073 {
lakshya 20:949d13045431 1074 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1075 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
lakshya 49:61c9f28332ba 1076 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
lakshya 20:949d13045431 1077 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 1078 return 1;
lakshya 20:949d13045431 1079 }
lakshya 20:949d13045431 1080
lakshya 20:949d13045431 1081
lakshya 20:949d13045431 1082
lakshya 49:61c9f28332ba 1083 /// acs_pc.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
Bragadeesh153 52:daa685b0e390 1084 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1085 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1086
lakshya 20:949d13045431 1087 }
lakshya 17:fc782f7548c6 1088
lakshya 49:61c9f28332ba 1089 /// acs_pc.printf("Sensor 1 not working. Trying Sensor 2\n \r");
lakshya 17:fc782f7548c6 1090
lakshya 20:949d13045431 1091 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 1092 {
lakshya 20:949d13045431 1093
lakshya 20:949d13045431 1094
lakshya 20:949d13045431 1095 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1096 wait_ms(5);
lakshya 49:61c9f28332ba 1097 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1098 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1099 working = SENSOR_INIT();
lakshya 49:61c9f28332ba 1100 timer_SENSOR_INIT.stop();
lakshya 49:61c9f28332ba 1101
lakshya 20:949d13045431 1102 if(working ==1)
lakshya 20:949d13045431 1103 {
lakshya 49:61c9f28332ba 1104 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1105 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
lakshya 20:949d13045431 1106 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 1107 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 1108 return 2;
lakshya 20:949d13045431 1109 }
lakshya 20:949d13045431 1110
lakshya 20:949d13045431 1111 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1112 wait_ms(5);
lakshya 20:949d13045431 1113
lakshya 20:949d13045431 1114 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1115
lakshya 20:949d13045431 1116
lakshya 20:949d13045431 1117 }
lakshya 17:fc782f7548c6 1118
lakshya 49:61c9f28332ba 1119 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1120 /// acs_pc.printf("Sensor 2 also not working.Exit init.\n \r");
lakshya 17:fc782f7548c6 1121
lakshya 20:949d13045431 1122 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
lakshya 20:949d13045431 1123 return 0;
sakthipriya 0:7b4c00e3912f 1124 }
sakthipriya 0:7b4c00e3912f 1125
lakshya 20:949d13045431 1126
lakshya 20:949d13045431 1127 int SENSOR_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 1128 {
lakshya 20:949d13045431 1129 //int mag_only=0;
lakshya 49:61c9f28332ba 1130 /// acs_pc.printf("Entering Sensor data acq.\n \r");
lakshya 20:949d13045431 1131 char status;
lakshya 20:949d13045431 1132 int sentral;
lakshya 20:949d13045431 1133 int event;
lakshya 20:949d13045431 1134 int sensor;
lakshya 20:949d13045431 1135 int error;
lakshya 20:949d13045431 1136 int init;
lakshya 20:949d13045431 1137
lakshya 20:949d13045431 1138 uint8_t gyro_error=0;
lakshya 20:949d13045431 1139 uint8_t mag_error=0;
lakshya 20:949d13045431 1140
lakshya 20:949d13045431 1141 //int ack1;
lakshya 20:949d13045431 1142 //int ack2;
lakshya 20:949d13045431 1143
sakthipriya 0:7b4c00e3912f 1144 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1145 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1146 if(ack!=0)
lakshya 20:949d13045431 1147 {
lakshya 20:949d13045431 1148 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1149 if(ack!=0)
lakshya 20:949d13045431 1150 return 0;
lakshya 20:949d13045431 1151 }
lakshya 20:949d13045431 1152
lakshya 20:949d13045431 1153 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1154 if(ack!=0)
lakshya 20:949d13045431 1155 {
lakshya 20:949d13045431 1156 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1157 if(ack!=0)
lakshya 20:949d13045431 1158 return 0;
lakshya 20:949d13045431 1159 }
lakshya 17:fc782f7548c6 1160
lakshya 20:949d13045431 1161 event = (int)status;
lakshya 20:949d13045431 1162
Bragadeesh153 59:0fc0bfafaa9f 1163 //if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1164 if(1)
lakshya 20:949d13045431 1165 {
lakshya 20:949d13045431 1166 ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 1167 }
lakshya 20:949d13045431 1168 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1169 {
lakshya 20:949d13045431 1170 ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 1171 }
lakshya 20:949d13045431 1172
lakshya 20:949d13045431 1173 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1174 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1175 if(ack!=0)
lakshya 20:949d13045431 1176 {
lakshya 20:949d13045431 1177 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1178 if(ack!=0)
lakshya 20:949d13045431 1179 return 0;
lakshya 20:949d13045431 1180 }
lakshya 20:949d13045431 1181
lakshya 20:949d13045431 1182 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1183 if(ack!=0)
lakshya 20:949d13045431 1184 {
lakshya 20:949d13045431 1185 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1186 if(ack!=0)
lakshya 20:949d13045431 1187 return 0;
lakshya 20:949d13045431 1188 }
lakshya 20:949d13045431 1189
lakshya 17:fc782f7548c6 1190
lakshya 20:949d13045431 1191 sentral = (int) status;
lakshya 20:949d13045431 1192
Bragadeesh153 59:0fc0bfafaa9f 1193 // if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1194 if(1)
lakshya 20:949d13045431 1195 {
lakshya 20:949d13045431 1196 ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 1197 }
lakshya 20:949d13045431 1198 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1199 {
lakshya 20:949d13045431 1200 ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 1201 }
lakshya 20:949d13045431 1202
lakshya 49:61c9f28332ba 1203 /// acs_pc.printf("Event Status is %x\n \r",event);
lakshya 49:61c9f28332ba 1204 /// acs_pc.printf("Sentral Status is %x\n \r",sentral);
lakshya 20:949d13045431 1205
lakshya 17:fc782f7548c6 1206
lakshya 17:fc782f7548c6 1207
lakshya 49:61c9f28332ba 1208 if ( ((event & 0x20 )!= 0x20 ) || ((event & 0x08) != 0x08 ) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 )|| (sentral!= 3)) //check for any error in event status register
lakshya 20:949d13045431 1209 {
lakshya 20:949d13045431 1210
lakshya 49:61c9f28332ba 1211 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1212 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1213 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1214 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 1215
lakshya 20:949d13045431 1216 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1217 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1218 if(ack!=0)
lakshya 20:949d13045431 1219 {
lakshya 20:949d13045431 1220 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1221 if(ack!=0)
lakshya 20:949d13045431 1222 return 0;
lakshya 20:949d13045431 1223 }
lakshya 20:949d13045431 1224
lakshya 20:949d13045431 1225 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1226 if(ack!=0)
lakshya 20:949d13045431 1227 {
lakshya 20:949d13045431 1228 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1229 if(ack!=0)
lakshya 20:949d13045431 1230 return 0;
lakshya 20:949d13045431 1231 }
lakshya 20:949d13045431 1232
lakshya 20:949d13045431 1233 event = (int)status;
lakshya 20:949d13045431 1234
lakshya 20:949d13045431 1235 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1236 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1237 if(ack!=0)
lakshya 20:949d13045431 1238 {
lakshya 20:949d13045431 1239 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1240 if(ack!=0)
lakshya 20:949d13045431 1241 return 0;
lakshya 20:949d13045431 1242 }
lakshya 20:949d13045431 1243
lakshya 20:949d13045431 1244 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1245 if(ack!=0)
lakshya 20:949d13045431 1246 {
lakshya 20:949d13045431 1247 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1248 if(ack!=0)
lakshya 20:949d13045431 1249 return 0;
lakshya 20:949d13045431 1250 }
lakshya 20:949d13045431 1251
lakshya 20:949d13045431 1252 sentral = (int)status;
lakshya 49:61c9f28332ba 1253 int poll_status;
lakshya 49:61c9f28332ba 1254 /// acs_pc.printf("Event Status after resetting and init is %x\n \r",event);
lakshya 20:949d13045431 1255
lakshya 49:61c9f28332ba 1256 if ( ((event & 0x20) != 0x20 ) || ((event & 0x08) != 0x08) || ((event & 0x01) == 0x01 )|| ((event & 0x02) == 0x02 ) || (init == 0)||(sentral != 3)) //check for any error in event status
lakshya 20:949d13045431 1257 {
lakshya 20:949d13045431 1258
lakshya 20:949d13045431 1259 cmd[0]=ERROR;
lakshya 20:949d13045431 1260 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1261 if(ack!=0)
lakshya 20:949d13045431 1262 {
lakshya 20:949d13045431 1263 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1264 if(ack!=0)
lakshya 20:949d13045431 1265 return 0;
lakshya 20:949d13045431 1266 }
lakshya 20:949d13045431 1267
lakshya 20:949d13045431 1268 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1269 if(ack!=0)
lakshya 20:949d13045431 1270 {
lakshya 20:949d13045431 1271
lakshya 20:949d13045431 1272 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 1273 {
lakshya 20:949d13045431 1274 ATS1_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 1275 }
lakshya 20:949d13045431 1276 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1277 {
lakshya 20:949d13045431 1278 ATS2_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 1279 }
lakshya 20:949d13045431 1280 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1281 if(ack!=0)
lakshya 20:949d13045431 1282 return 0;
lakshya 20:949d13045431 1283 }
lakshya 20:949d13045431 1284
lakshya 20:949d13045431 1285 error = (int)status;
lakshya 20:949d13045431 1286
Bragadeesh153 59:0fc0bfafaa9f 1287 //if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1288 if(1)
lakshya 20:949d13045431 1289 {
lakshya 20:949d13045431 1290 ATS1_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 1291 }
lakshya 20:949d13045431 1292 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1293 {
lakshya 20:949d13045431 1294 ATS2_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 1295 }
lakshya 20:949d13045431 1296
lakshya 20:949d13045431 1297 cmd[0]=SENSORSTATUS;
lakshya 20:949d13045431 1298 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1299 if(ack!=0)
lakshya 20:949d13045431 1300 {
lakshya 20:949d13045431 1301 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1302 if(ack!=0)
lakshya 20:949d13045431 1303 return 0;
lakshya 20:949d13045431 1304 }
lakshya 20:949d13045431 1305
lakshya 20:949d13045431 1306 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1307 if(ack!=0)
lakshya 20:949d13045431 1308 {
lakshya 20:949d13045431 1309 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1310 if(ack!=0)
lakshya 20:949d13045431 1311 return 0;
lakshya 20:949d13045431 1312 }
lakshya 20:949d13045431 1313
lakshya 20:949d13045431 1314 sensor = (int)status;
lakshya 20:949d13045431 1315
lakshya 20:949d13045431 1316
lakshya 20:949d13045431 1317 if((error!=0) || (sensor!=0))
lakshya 20:949d13045431 1318 {
lakshya 20:949d13045431 1319 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
lakshya 20:949d13045431 1320 {
lakshya 49:61c9f28332ba 1321 acs_pc.printf("error in gyro.\n \r");
lakshya 20:949d13045431 1322 gyro_error = 1;
lakshya 20:949d13045431 1323 }
lakshya 20:949d13045431 1324
lakshya 20:949d13045431 1325 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
lakshya 20:949d13045431 1326 {
lakshya 20:949d13045431 1327
lakshya 49:61c9f28332ba 1328 acs_pc.printf("error in mag.Exiting.\n \r");
lakshya 20:949d13045431 1329 mag_error = 1;
lakshya 20:949d13045431 1330 }
lakshya 20:949d13045431 1331 if( (gyro_error!=1)&&(mag_error!=1))
lakshya 20:949d13045431 1332 {
lakshya 49:61c9f28332ba 1333 acs_pc.printf("error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1334 return 0;
lakshya 20:949d13045431 1335
lakshya 20:949d13045431 1336 }
lakshya 20:949d13045431 1337 }
lakshya 20:949d13045431 1338
lakshya 20:949d13045431 1339
lakshya 49:61c9f28332ba 1340 if(((event & 1) == 1 ))
lakshya 20:949d13045431 1341 {
lakshya 49:61c9f28332ba 1342 /// acs_pc.printf("error in CPU Reset.\n \r");
lakshya 20:949d13045431 1343 return 0;
lakshya 20:949d13045431 1344
lakshya 20:949d13045431 1345 }
lakshya 49:61c9f28332ba 1346 int poll =0;
lakshya 49:61c9f28332ba 1347 poll_status =1;
lakshya 49:61c9f28332ba 1348 while((poll<15)&&(((event & 8) != 8 )||((event & 32) != 32 )))
lakshya 49:61c9f28332ba 1349 //if(((event & 8) != 8 )||((event & 32) != 32 ))
lakshya 20:949d13045431 1350 {
lakshya 49:61c9f28332ba 1351 poll++;
lakshya 49:61c9f28332ba 1352 acs_pc.printf("Data not ready waiting...POLL#%d\n \r",poll);
lakshya 20:949d13045431 1353 //POLL
lakshya 20:949d13045431 1354 wait_ms(200);
lakshya 20:949d13045431 1355
lakshya 20:949d13045431 1356 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1357
lakshya 20:949d13045431 1358 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1359 if(ack!=0)
lakshya 20:949d13045431 1360 {
lakshya 20:949d13045431 1361 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1362 if(ack!=0)
lakshya 20:949d13045431 1363 return 0;
lakshya 20:949d13045431 1364 }
lakshya 20:949d13045431 1365
lakshya 20:949d13045431 1366 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1367 if(ack!=0)
lakshya 20:949d13045431 1368 {
lakshya 20:949d13045431 1369 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1370 if(ack!=0)
lakshya 20:949d13045431 1371 return 0;
lakshya 20:949d13045431 1372 }
lakshya 20:949d13045431 1373
lakshya 20:949d13045431 1374 event = (int)status;
lakshya 49:61c9f28332ba 1375 if((event & 32) != 32 )
lakshya 20:949d13045431 1376 {
lakshya 20:949d13045431 1377
lakshya 49:61c9f28332ba 1378 acs_pc.printf("Gyro not ready..\n \r");
lakshya 20:949d13045431 1379 gyro_error = 1;
lakshya 20:949d13045431 1380
lakshya 20:949d13045431 1381 }
lakshya 49:61c9f28332ba 1382 else
lakshya 49:61c9f28332ba 1383 {
lakshya 49:61c9f28332ba 1384 gyro_error = 0;
lakshya 49:61c9f28332ba 1385 }
lakshya 20:949d13045431 1386
lakshya 49:61c9f28332ba 1387 if((event & 8) != 8 )
lakshya 20:949d13045431 1388 {
lakshya 49:61c9f28332ba 1389 acs_pc.printf("Mag not ready..\n \r");
lakshya 20:949d13045431 1390 mag_error=1;
lakshya 20:949d13045431 1391 }
lakshya 49:61c9f28332ba 1392 else
lakshya 49:61c9f28332ba 1393 {
lakshya 49:61c9f28332ba 1394 mag_error=0;
lakshya 49:61c9f28332ba 1395 }
lakshya 20:949d13045431 1396
lakshya 20:949d13045431 1397
lakshya 20:949d13045431 1398 }
lakshya 49:61c9f28332ba 1399
lakshya 49:61c9f28332ba 1400 if(poll!=15)
lakshya 49:61c9f28332ba 1401 {
lakshya 49:61c9f28332ba 1402 poll_status = 0;
lakshya 49:61c9f28332ba 1403 }
lakshya 20:949d13045431 1404
lakshya 20:949d13045431 1405
lakshya 20:949d13045431 1406 }
lakshya 20:949d13045431 1407
lakshya 49:61c9f28332ba 1408 if(((mag_error !=1)&&(gyro_error!=1))&&(poll_status!=0))
lakshya 20:949d13045431 1409 {
lakshya 49:61c9f28332ba 1410 acs_pc.printf("Error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1411 return 0;
lakshya 20:949d13045431 1412 }
lakshya 20:949d13045431 1413
lakshya 49:61c9f28332ba 1414 if(((mag_error ==1)&&(gyro_error==1))&&(poll_status!=0))
lakshya 20:949d13045431 1415 {
lakshya 49:61c9f28332ba 1416 acs_pc.printf("Error in both gyro and mag.Exiting.\n \r");
lakshya 20:949d13045431 1417 return 0;
lakshya 20:949d13045431 1418 }
lakshya 20:949d13045431 1419
lakshya 20:949d13045431 1420 }
lakshya 20:949d13045431 1421
lakshya 20:949d13045431 1422
lakshya 20:949d13045431 1423 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1424 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1425 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1426 if(ack != 0)
lakshya 20:949d13045431 1427 {
lakshya 20:949d13045431 1428 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1429 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1430 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1431 if(ack !=1)
lakshya 20:949d13045431 1432 return 0;
lakshya 20:949d13045431 1433
lakshya 20:949d13045431 1434 }
lakshya 20:949d13045431 1435
lakshya 20:949d13045431 1436
lakshya 49:61c9f28332ba 1437 // acs_pc.printf("\nGyro Values:\n");
lakshya 20:949d13045431 1438 if (gyro_error!=1)
lakshya 20:949d13045431 1439 {
lakshya 20:949d13045431 1440 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1441 //concatenating gyro LSB and MSB to get 16 bit signed data values
lakshya 20:949d13045431 1442 actual_data.bit_data_acs_mg[i]= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
lakshya 20:949d13045431 1443 gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
lakshya 20:949d13045431 1444 gyro_data[i]=gyro_data[i]/senstivity_gyro;
lakshya 20:949d13045431 1445 actual_data.AngularSpeed_actual[i] = gyro_data[i];
lakshya 20:949d13045431 1446 }
lakshya 20:949d13045431 1447 }
lakshya 20:949d13045431 1448
lakshya 20:949d13045431 1449 if(mag_error!=1)
lakshya 20:949d13045431 1450 {
lakshya 20:949d13045431 1451 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1452 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
lakshya 20:949d13045431 1453 actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
lakshya 20:949d13045431 1454
lakshya 20:949d13045431 1455 mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
lakshya 20:949d13045431 1456 mag_data[i]=mag_data[i]/senstivity_mag;
lakshya 20:949d13045431 1457 actual_data.Bvalue_actual[i] = mag_data[i];
lakshya 20:949d13045431 1458 }
lakshya 20:949d13045431 1459 }
lakshya 20:949d13045431 1460
lakshya 20:949d13045431 1461
lakshya 20:949d13045431 1462 if(mag_error == 1)
lakshya 20:949d13045431 1463 {
lakshya 20:949d13045431 1464
lakshya 49:61c9f28332ba 1465 acs_pc.printf("Gyro only successful.\n \r");
lakshya 20:949d13045431 1466 return 1;
lakshya 20:949d13045431 1467 }
lakshya 20:949d13045431 1468 if(gyro_error == 1)
lakshya 49:61c9f28332ba 1469 {
lakshya 49:61c9f28332ba 1470 acs_pc.printf("Mag only successful.\n \r");
lakshya 20:949d13045431 1471 return 2;
lakshya 20:949d13045431 1472 }
lakshya 17:fc782f7548c6 1473
lakshya 49:61c9f28332ba 1474 //acs_pc.printf("Reading data success.\n \r");
lakshya 20:949d13045431 1475 return 3;
lakshya 20:949d13045431 1476 }
lakshya 20:949d13045431 1477
lakshya 20:949d13045431 1478
lakshya 20:949d13045431 1479 int FCTN_ATS_DATA_ACQ()
lakshya 20:949d13045431 1480 {
lakshya 20:949d13045431 1481 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1482 actual_data.AngularSpeed_actual[i] = 0;
lakshya 20:949d13045431 1483 actual_data.Bvalue_actual[i] = 0;
lakshya 20:949d13045431 1484 }
lakshya 20:949d13045431 1485
lakshya 20:949d13045431 1486 int acq;
lakshya 20:949d13045431 1487 int init;
lakshya 20:949d13045431 1488
lakshya 49:61c9f28332ba 1489 //// acs_pc.printf("DATA_ACQ called \n \r");
lakshya 49:61c9f28332ba 1490 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1491
lakshya 20:949d13045431 1492
lakshya 20:949d13045431 1493 if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
lakshya 20:949d13045431 1494 {
lakshya 49:61c9f28332ba 1495 timer_SENSOR_DATA_ACQ.start();
lakshya 20:949d13045431 1496 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1497 timer_SENSOR_DATA_ACQ.stop();
lakshya 49:61c9f28332ba 1498 //pc.pritnf("\n\r timer_SENSOR_DATA_ACQ is %f",timer_SENSOR_DATA_ACQ.read());
lakshya 49:61c9f28332ba 1499
lakshya 49:61c9f28332ba 1500
lakshya 20:949d13045431 1501 if(acq == 3)
lakshya 20:949d13045431 1502 {
lakshya 20:949d13045431 1503
lakshya 20:949d13045431 1504 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1505
lakshya 20:949d13045431 1506 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1507 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1508 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1509 return 3;
lakshya 20:949d13045431 1510 }
lakshya 20:949d13045431 1511 else if((acq == 2)||(acq==1))
lakshya 20:949d13045431 1512 {
lakshya 49:61c9f28332ba 1513 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1514 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 20:949d13045431 1515 {
lakshya 20:949d13045431 1516 //other sensor both working, off or
lakshya 20:949d13045431 1517 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1518 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1519
Bragadeesh153 52:daa685b0e390 1520 ATS1_SW_ENABLE = 01; //switch off sensor 1
lakshya 20:949d13045431 1521 wait_ms(5);
lakshya 20:949d13045431 1522 if(acq == 1)
lakshya 20:949d13045431 1523 {
lakshya 20:949d13045431 1524 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 20:949d13045431 1525 }
lakshya 20:949d13045431 1526 if(acq==2)
lakshya 20:949d13045431 1527 {
lakshya 20:949d13045431 1528 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 20:949d13045431 1529 }
lakshya 20:949d13045431 1530
lakshya 20:949d13045431 1531 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 20:949d13045431 1532 wait_ms(5);
lakshya 49:61c9f28332ba 1533
lakshya 49:61c9f28332ba 1534 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1535 timer_SENSOR_INIT.start();
lakshya 49:61c9f28332ba 1536 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1537 timer_SENSOR_INIT.stop();
lakshya 49:61c9f28332ba 1538 //sensor 2 init
lakshya 20:949d13045431 1539 if( init == 0)
lakshya 20:949d13045431 1540 {
lakshya 49:61c9f28332ba 1541 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1542 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1543 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1544 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1545 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 20:949d13045431 1546 if(acq == 1)
lakshya 20:949d13045431 1547 {
lakshya 20:949d13045431 1548 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 20:949d13045431 1549 }
lakshya 20:949d13045431 1550 if(acq==2)
lakshya 20:949d13045431 1551 {
lakshya 20:949d13045431 1552 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1553 }
lakshya 20:949d13045431 1554 return acq;
lakshya 20:949d13045431 1555 }
lakshya 20:949d13045431 1556
lakshya 20:949d13045431 1557 int acq2;
lakshya 20:949d13045431 1558 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1559 if(acq2 == 3)
lakshya 20:949d13045431 1560 {
lakshya 20:949d13045431 1561 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1562 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 20:949d13045431 1563 return 3;
lakshya 20:949d13045431 1564 }
lakshya 20:949d13045431 1565 else if(acq2 == 1)
lakshya 20:949d13045431 1566 {
lakshya 20:949d13045431 1567 if(acq==2)
lakshya 20:949d13045431 1568 {
lakshya 20:949d13045431 1569 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1570 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1571 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 20:949d13045431 1572 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1573 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1574 return 3;
lakshya 20:949d13045431 1575 }
lakshya 20:949d13045431 1576 else
lakshya 20:949d13045431 1577 {
lakshya 20:949d13045431 1578 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 20:949d13045431 1579 return 1;
lakshya 20:949d13045431 1580 }
lakshya 20:949d13045431 1581 }
lakshya 20:949d13045431 1582
lakshya 20:949d13045431 1583 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 20:949d13045431 1584 {
lakshya 20:949d13045431 1585 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1586 return 2;
lakshya 20:949d13045431 1587 }
lakshya 20:949d13045431 1588 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 20:949d13045431 1589 {
lakshya 49:61c9f28332ba 1590 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1591 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1592 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 52:daa685b0e390 1593 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1594 wait_ms(5);
lakshya 20:949d13045431 1595 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 20:949d13045431 1596 return acq;
lakshya 20:949d13045431 1597 }
lakshya 20:949d13045431 1598
lakshya 20:949d13045431 1599 }
lakshya 20:949d13045431 1600 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1601 {
lakshya 20:949d13045431 1602 if(acq == 1)
lakshya 20:949d13045431 1603 {
lakshya 20:949d13045431 1604 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 20:949d13045431 1605 return 1;
lakshya 20:949d13045431 1606 }
lakshya 20:949d13045431 1607 if(acq==2)
lakshya 20:949d13045431 1608 {
lakshya 20:949d13045431 1609 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1610 return 2;
lakshya 20:949d13045431 1611 }
lakshya 49:61c9f28332ba 1612 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1613 return acq;
lakshya 20:949d13045431 1614
lakshya 20:949d13045431 1615 }
sakthipriya 0:7b4c00e3912f 1616 }
lakshya 20:949d13045431 1617
lakshya 20:949d13045431 1618 else if(acq == 0)
lakshya 20:949d13045431 1619 {
lakshya 49:61c9f28332ba 1620 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1621 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1622 wait_ms(5); //Switch ON sensor 2
lakshya 20:949d13045431 1623 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1624 wait_ms(5);
lakshya 20:949d13045431 1625 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1626 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 20:949d13045431 1627 {
lakshya 20:949d13045431 1628 init = SENSOR_INIT();
lakshya 20:949d13045431 1629 if( init == 0)
lakshya 20:949d13045431 1630 {
lakshya 49:61c9f28332ba 1631 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 20:949d13045431 1632 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1633 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 20:949d13045431 1634 return 0;
lakshya 20:949d13045431 1635 }
lakshya 20:949d13045431 1636
lakshya 20:949d13045431 1637 int acq2;
lakshya 20:949d13045431 1638 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1639 if(acq2 == 3)
lakshya 20:949d13045431 1640 {
lakshya 20:949d13045431 1641 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1642 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 20:949d13045431 1643 return 3;
lakshya 20:949d13045431 1644 }
lakshya 20:949d13045431 1645 else if(acq2 == 1)
lakshya 20:949d13045431 1646 {
lakshya 20:949d13045431 1647 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 20:949d13045431 1648 return 1;
lakshya 20:949d13045431 1649 }
lakshya 20:949d13045431 1650 else if(acq2 == 2)
lakshya 20:949d13045431 1651 {
lakshya 20:949d13045431 1652 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1653 return 2;
lakshya 20:949d13045431 1654 }
lakshya 20:949d13045431 1655 else if(acq2 == 0)
lakshya 20:949d13045431 1656 {
lakshya 49:61c9f28332ba 1657 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 20:949d13045431 1658 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1659
lakshya 20:949d13045431 1660 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1661 return 0;
lakshya 20:949d13045431 1662 }
lakshya 20:949d13045431 1663
lakshya 20:949d13045431 1664 }
lakshya 20:949d13045431 1665
sakthipriya 0:7b4c00e3912f 1666 }
lakshya 20:949d13045431 1667
lakshya 20:949d13045431 1668
sakthipriya 0:7b4c00e3912f 1669 }
lakshya 20:949d13045431 1670
lakshya 20:949d13045431 1671 if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
lakshya 20:949d13045431 1672 {
lakshya 20:949d13045431 1673 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 1674 if(acq == 3) //Both available read and exit
lakshya 20:949d13045431 1675 {
lakshya 20:949d13045431 1676 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1677 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1678 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1679 return 3;
lakshya 20:949d13045431 1680 }
lakshya 20:949d13045431 1681 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 20:949d13045431 1682 {
lakshya 49:61c9f28332ba 1683 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1684 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 20:949d13045431 1685 {
lakshya 20:949d13045431 1686 //other sensor both working, off or
lakshya 20:949d13045431 1687 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1688 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1689 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 20:949d13045431 1690 wait_ms(5);
lakshya 20:949d13045431 1691 if(acq == 1)
lakshya 20:949d13045431 1692 {
lakshya 20:949d13045431 1693 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 20:949d13045431 1694 }
lakshya 20:949d13045431 1695 if(acq==2)
lakshya 20:949d13045431 1696 {
lakshya 20:949d13045431 1697 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 20:949d13045431 1698 }
lakshya 20:949d13045431 1699
Bragadeesh153 52:daa685b0e390 1700 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 20:949d13045431 1701 wait_ms(5);
lakshya 20:949d13045431 1702 init = SENSOR_INIT(); //sensor 2 init
lakshya 20:949d13045431 1703
lakshya 20:949d13045431 1704 if( init == 0)
lakshya 20:949d13045431 1705 {
lakshya 49:61c9f28332ba 1706 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1707 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1708 wait_ms(5);
lakshya 20:949d13045431 1709 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1710 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 20:949d13045431 1711 if(acq == 1)
lakshya 20:949d13045431 1712 {
lakshya 20:949d13045431 1713 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 20:949d13045431 1714 }
lakshya 20:949d13045431 1715 if(acq==2)
lakshya 20:949d13045431 1716 {
lakshya 20:949d13045431 1717 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1718 }
lakshya 20:949d13045431 1719 return acq;
lakshya 20:949d13045431 1720 }
lakshya 20:949d13045431 1721
lakshya 20:949d13045431 1722 int acq2;
lakshya 20:949d13045431 1723 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1724
lakshya 20:949d13045431 1725 if(acq2 == 3)
lakshya 20:949d13045431 1726 {
lakshya 20:949d13045431 1727 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1728 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 20:949d13045431 1729 return 3;
lakshya 20:949d13045431 1730 }
lakshya 20:949d13045431 1731
lakshya 20:949d13045431 1732 else if(acq2 == 1)
lakshya 20:949d13045431 1733 {
lakshya 20:949d13045431 1734 if(acq==2)
lakshya 20:949d13045431 1735 {
Bragadeesh153 52:daa685b0e390 1736 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1737 wait_ms(5);
lakshya 20:949d13045431 1738 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 20:949d13045431 1739 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 20:949d13045431 1740 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1741 return 3;
lakshya 20:949d13045431 1742 }
lakshya 20:949d13045431 1743 else
lakshya 20:949d13045431 1744 {
lakshya 20:949d13045431 1745 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 20:949d13045431 1746 return 1;
lakshya 20:949d13045431 1747 }
lakshya 20:949d13045431 1748 }
lakshya 20:949d13045431 1749
lakshya 20:949d13045431 1750 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 20:949d13045431 1751 {
lakshya 20:949d13045431 1752 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1753 return 2;
lakshya 20:949d13045431 1754 }
lakshya 20:949d13045431 1755 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 20:949d13045431 1756 {
lakshya 49:61c9f28332ba 1757 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1758 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1759 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 20:949d13045431 1760 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1761 wait_ms(5);
lakshya 20:949d13045431 1762 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 20:949d13045431 1763 return acq;
lakshya 20:949d13045431 1764 }
lakshya 20:949d13045431 1765
lakshya 20:949d13045431 1766 }
lakshya 20:949d13045431 1767 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1768 {
lakshya 20:949d13045431 1769 if(acq == 1)
lakshya 20:949d13045431 1770 {
lakshya 20:949d13045431 1771 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 20:949d13045431 1772 return 1;
lakshya 20:949d13045431 1773 }
lakshya 20:949d13045431 1774 if(acq==2)
lakshya 20:949d13045431 1775 {
lakshya 20:949d13045431 1776 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1777 return 2;
lakshya 20:949d13045431 1778 }
lakshya 49:61c9f28332ba 1779 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1780 return acq;
lakshya 20:949d13045431 1781
lakshya 20:949d13045431 1782 }
lakshya 20:949d13045431 1783 }
lakshya 20:949d13045431 1784 else if(acq == 0)
lakshya 20:949d13045431 1785 {
lakshya 49:61c9f28332ba 1786 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 20:949d13045431 1787 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1788 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 1789 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1790 wait_ms(5);
lakshya 20:949d13045431 1791 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1792 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 20:949d13045431 1793 {
lakshya 20:949d13045431 1794 init = SENSOR_INIT();
lakshya 20:949d13045431 1795 if( init == 0)
lakshya 20:949d13045431 1796 {
lakshya 49:61c9f28332ba 1797 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 20:949d13045431 1798 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1799 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 20:949d13045431 1800 return 0;
lakshya 20:949d13045431 1801 }
lakshya 20:949d13045431 1802
lakshya 20:949d13045431 1803 int acq2;
lakshya 20:949d13045431 1804 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1805 if(acq2 == 3)
lakshya 20:949d13045431 1806 {
lakshya 20:949d13045431 1807 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1808 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 20:949d13045431 1809 return 3;
lakshya 20:949d13045431 1810 }
lakshya 20:949d13045431 1811 else if(acq2 == 1)
lakshya 20:949d13045431 1812 {
lakshya 20:949d13045431 1813 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 20:949d13045431 1814 return 1;
lakshya 20:949d13045431 1815 }
lakshya 20:949d13045431 1816 else if(acq2 == 2)
lakshya 20:949d13045431 1817 {
lakshya 20:949d13045431 1818 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1819 return 2;
lakshya 20:949d13045431 1820 }
lakshya 20:949d13045431 1821 else if(acq2 == 0)
lakshya 20:949d13045431 1822 {
lakshya 49:61c9f28332ba 1823 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 1824 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1825 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1826 return 0;
lakshya 20:949d13045431 1827 }
lakshya 20:949d13045431 1828 }
lakshya 20:949d13045431 1829 }
lakshya 20:949d13045431 1830 }
lakshya 49:61c9f28332ba 1831
lakshya 49:61c9f28332ba 1832
lakshya 49:61c9f28332ba 1833
lakshya 49:61c9f28332ba 1834 if(( (ACS_ATS_STATUS & 0xC0) == 0x00))
lakshya 49:61c9f28332ba 1835 {
lakshya 49:61c9f28332ba 1836
lakshya 49:61c9f28332ba 1837 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1838 wait_ms(10);
lakshya 49:61c9f28332ba 1839 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1840 if(acq == 3)
lakshya 49:61c9f28332ba 1841 {
lakshya 49:61c9f28332ba 1842
lakshya 49:61c9f28332ba 1843 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1844
lakshya 49:61c9f28332ba 1845 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1846 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1847 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 1848 return 3;
lakshya 49:61c9f28332ba 1849 }
lakshya 49:61c9f28332ba 1850 else if((acq == 2)||(acq==1))
lakshya 49:61c9f28332ba 1851 {
lakshya 49:61c9f28332ba 1852 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 1853 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 49:61c9f28332ba 1854 {
lakshya 49:61c9f28332ba 1855 //other sensor both working, off or
lakshya 49:61c9f28332ba 1856 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 1857 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 1858
Bragadeesh153 52:daa685b0e390 1859 ATS1_SW_ENABLE = 0; //switch off sensor 1
lakshya 49:61c9f28332ba 1860 wait_ms(5);
lakshya 49:61c9f28332ba 1861 if(acq == 1)
lakshya 49:61c9f28332ba 1862 {
lakshya 49:61c9f28332ba 1863 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 49:61c9f28332ba 1864 }
lakshya 49:61c9f28332ba 1865 if(acq==2)
lakshya 49:61c9f28332ba 1866 {
lakshya 49:61c9f28332ba 1867 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 49:61c9f28332ba 1868 }
lakshya 49:61c9f28332ba 1869
lakshya 49:61c9f28332ba 1870 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 49:61c9f28332ba 1871 wait_ms(5);
lakshya 49:61c9f28332ba 1872
lakshya 49:61c9f28332ba 1873 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 1874 if( init == 0)
lakshya 49:61c9f28332ba 1875 {
lakshya 49:61c9f28332ba 1876 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1877 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1878 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1879 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1880 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 49:61c9f28332ba 1881 if(acq == 1)
lakshya 49:61c9f28332ba 1882 {
lakshya 49:61c9f28332ba 1883 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 49:61c9f28332ba 1884 }
lakshya 49:61c9f28332ba 1885 if(acq==2)
lakshya 49:61c9f28332ba 1886 {
lakshya 49:61c9f28332ba 1887 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1888 }
lakshya 49:61c9f28332ba 1889 return acq;
lakshya 49:61c9f28332ba 1890 }
lakshya 49:61c9f28332ba 1891
lakshya 49:61c9f28332ba 1892 int acq2;
lakshya 49:61c9f28332ba 1893 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1894 if(acq2 == 3)
lakshya 49:61c9f28332ba 1895 {
lakshya 49:61c9f28332ba 1896 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1897 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 49:61c9f28332ba 1898 return 3;
lakshya 49:61c9f28332ba 1899 }
lakshya 49:61c9f28332ba 1900 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1901 {
lakshya 49:61c9f28332ba 1902 if(acq==2)
lakshya 49:61c9f28332ba 1903 {
lakshya 49:61c9f28332ba 1904 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1905 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1906 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 49:61c9f28332ba 1907 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 49:61c9f28332ba 1908 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1909 return 3;
lakshya 49:61c9f28332ba 1910 }
lakshya 49:61c9f28332ba 1911 else
lakshya 49:61c9f28332ba 1912 {
lakshya 49:61c9f28332ba 1913 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 49:61c9f28332ba 1914 return 1;
lakshya 49:61c9f28332ba 1915 }
lakshya 49:61c9f28332ba 1916 }
lakshya 49:61c9f28332ba 1917
lakshya 49:61c9f28332ba 1918 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 49:61c9f28332ba 1919 {
lakshya 49:61c9f28332ba 1920 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1921 return 2;
lakshya 49:61c9f28332ba 1922 }
lakshya 49:61c9f28332ba 1923 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 49:61c9f28332ba 1924 {
lakshya 49:61c9f28332ba 1925 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1926 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1927 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 52:daa685b0e390 1928 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1929 wait_ms(5);
lakshya 49:61c9f28332ba 1930 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 49:61c9f28332ba 1931 return acq;
lakshya 49:61c9f28332ba 1932 }
lakshya 49:61c9f28332ba 1933
lakshya 49:61c9f28332ba 1934 }
lakshya 49:61c9f28332ba 1935 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 1936 {
lakshya 49:61c9f28332ba 1937 if(acq == 1)
lakshya 49:61c9f28332ba 1938 {
lakshya 49:61c9f28332ba 1939 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 49:61c9f28332ba 1940 return 1;
lakshya 49:61c9f28332ba 1941 }
lakshya 49:61c9f28332ba 1942 if(acq==2)
lakshya 49:61c9f28332ba 1943 {
lakshya 49:61c9f28332ba 1944 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1945 return 2;
lakshya 49:61c9f28332ba 1946 }
lakshya 49:61c9f28332ba 1947 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 1948 return acq;
lakshya 49:61c9f28332ba 1949
lakshya 49:61c9f28332ba 1950 }
lakshya 49:61c9f28332ba 1951 }
lakshya 49:61c9f28332ba 1952
lakshya 49:61c9f28332ba 1953 else if(acq == 0)
lakshya 49:61c9f28332ba 1954 {
lakshya 49:61c9f28332ba 1955 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1956 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1957 wait_ms(5); //Switch ON sensor 2
lakshya 49:61c9f28332ba 1958 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1959 wait_ms(5);
lakshya 49:61c9f28332ba 1960 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 1961 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 49:61c9f28332ba 1962 {
lakshya 49:61c9f28332ba 1963 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1964 if( init == 0)
lakshya 49:61c9f28332ba 1965 {
lakshya 49:61c9f28332ba 1966 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 1967 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1968 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 49:61c9f28332ba 1969 return 0;
lakshya 49:61c9f28332ba 1970 }
lakshya 49:61c9f28332ba 1971
lakshya 49:61c9f28332ba 1972 int acq2;
lakshya 49:61c9f28332ba 1973 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1974 if(acq2 == 3)
lakshya 49:61c9f28332ba 1975 {
lakshya 49:61c9f28332ba 1976 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1977 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 49:61c9f28332ba 1978 return 3;
lakshya 49:61c9f28332ba 1979 }
lakshya 49:61c9f28332ba 1980 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1981 {
lakshya 49:61c9f28332ba 1982 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 49:61c9f28332ba 1983 return 1;
lakshya 49:61c9f28332ba 1984 }
lakshya 49:61c9f28332ba 1985 else if(acq2 == 2)
lakshya 49:61c9f28332ba 1986 {
lakshya 49:61c9f28332ba 1987 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1988 return 2;
lakshya 49:61c9f28332ba 1989 }
lakshya 49:61c9f28332ba 1990 else if(acq2 == 0)
lakshya 49:61c9f28332ba 1991 {
lakshya 49:61c9f28332ba 1992 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 49:61c9f28332ba 1993 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1994
lakshya 49:61c9f28332ba 1995 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 1996 return 0;
lakshya 49:61c9f28332ba 1997 }
lakshya 49:61c9f28332ba 1998
lakshya 49:61c9f28332ba 1999 }
lakshya 49:61c9f28332ba 2000
lakshya 49:61c9f28332ba 2001 }
lakshya 49:61c9f28332ba 2002
lakshya 49:61c9f28332ba 2003
lakshya 49:61c9f28332ba 2004 }
lakshya 49:61c9f28332ba 2005
lakshya 49:61c9f28332ba 2006 if(( (ACS_ATS_STATUS & 0x0C) == 0x00))
lakshya 49:61c9f28332ba 2007 {
lakshya 49:61c9f28332ba 2008 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2009 wait_ms(10);
lakshya 49:61c9f28332ba 2010 acq = SENSOR_DATA_ACQ(); //make ATS2 on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
lakshya 49:61c9f28332ba 2011 if(acq == 3) //Both available read and exit
lakshya 49:61c9f28332ba 2012 {
lakshya 49:61c9f28332ba 2013 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 2014 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2015 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 2016 return 3;
lakshya 49:61c9f28332ba 2017 }
lakshya 49:61c9f28332ba 2018 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 49:61c9f28332ba 2019 {
lakshya 49:61c9f28332ba 2020 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 2021 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 49:61c9f28332ba 2022 {
lakshya 49:61c9f28332ba 2023 //other sensor both working, off or
lakshya 49:61c9f28332ba 2024 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 2025 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 2026 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 49:61c9f28332ba 2027 wait_ms(5);
lakshya 49:61c9f28332ba 2028 if(acq == 1)
lakshya 49:61c9f28332ba 2029 {
lakshya 49:61c9f28332ba 2030 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 49:61c9f28332ba 2031 }
lakshya 49:61c9f28332ba 2032 if(acq==2)
lakshya 49:61c9f28332ba 2033 {
lakshya 49:61c9f28332ba 2034 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 49:61c9f28332ba 2035 }
lakshya 49:61c9f28332ba 2036
Bragadeesh153 52:daa685b0e390 2037 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 49:61c9f28332ba 2038 wait_ms(5);
lakshya 49:61c9f28332ba 2039 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 2040
lakshya 49:61c9f28332ba 2041 if( init == 0)
lakshya 49:61c9f28332ba 2042 {
lakshya 49:61c9f28332ba 2043 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2044 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2045 wait_ms(5);
lakshya 49:61c9f28332ba 2046 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2047 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 49:61c9f28332ba 2048 if(acq == 1)
lakshya 49:61c9f28332ba 2049 {
lakshya 49:61c9f28332ba 2050 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 49:61c9f28332ba 2051 }
lakshya 49:61c9f28332ba 2052 if(acq==2)
lakshya 49:61c9f28332ba 2053 {
lakshya 49:61c9f28332ba 2054 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2055 }
lakshya 49:61c9f28332ba 2056 return acq;
lakshya 49:61c9f28332ba 2057 }
lakshya 49:61c9f28332ba 2058
lakshya 49:61c9f28332ba 2059 int acq2;
lakshya 49:61c9f28332ba 2060 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2061
lakshya 49:61c9f28332ba 2062 if(acq2 == 3)
lakshya 49:61c9f28332ba 2063 {
lakshya 49:61c9f28332ba 2064 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2065 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 49:61c9f28332ba 2066 return 3;
lakshya 49:61c9f28332ba 2067 }
lakshya 49:61c9f28332ba 2068
lakshya 49:61c9f28332ba 2069 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2070 {
lakshya 49:61c9f28332ba 2071 if(acq==2)
lakshya 49:61c9f28332ba 2072 {
Bragadeesh153 52:daa685b0e390 2073 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2074 wait_ms(5);
lakshya 49:61c9f28332ba 2075 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 49:61c9f28332ba 2076 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 49:61c9f28332ba 2077 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2078 return 3;
lakshya 49:61c9f28332ba 2079 }
lakshya 49:61c9f28332ba 2080 else
lakshya 49:61c9f28332ba 2081 {
lakshya 49:61c9f28332ba 2082 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 49:61c9f28332ba 2083 return 1;
lakshya 49:61c9f28332ba 2084 }
lakshya 49:61c9f28332ba 2085 }
lakshya 49:61c9f28332ba 2086
lakshya 49:61c9f28332ba 2087 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 49:61c9f28332ba 2088 {
lakshya 49:61c9f28332ba 2089 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2090 return 2;
lakshya 49:61c9f28332ba 2091 }
lakshya 49:61c9f28332ba 2092 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 49:61c9f28332ba 2093 {
lakshya 49:61c9f28332ba 2094 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2095 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2096 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 49:61c9f28332ba 2097 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2098 wait_ms(5);
lakshya 49:61c9f28332ba 2099 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 49:61c9f28332ba 2100 return acq;
lakshya 49:61c9f28332ba 2101 }
lakshya 49:61c9f28332ba 2102
lakshya 49:61c9f28332ba 2103 }
lakshya 49:61c9f28332ba 2104 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 2105 {
lakshya 49:61c9f28332ba 2106 if(acq == 1)
lakshya 49:61c9f28332ba 2107 {
lakshya 49:61c9f28332ba 2108 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 49:61c9f28332ba 2109 return 1;
lakshya 49:61c9f28332ba 2110 }
lakshya 49:61c9f28332ba 2111 if(acq==2)
lakshya 49:61c9f28332ba 2112 {
lakshya 49:61c9f28332ba 2113 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2114 return 2;
lakshya 49:61c9f28332ba 2115 }
lakshya 49:61c9f28332ba 2116 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 2117 return acq;
lakshya 49:61c9f28332ba 2118
lakshya 49:61c9f28332ba 2119 }
lakshya 49:61c9f28332ba 2120 }
lakshya 49:61c9f28332ba 2121 else if(acq == 0)
lakshya 49:61c9f28332ba 2122 {
lakshya 49:61c9f28332ba 2123 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 49:61c9f28332ba 2124 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2125 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 2126 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2127 wait_ms(5);
lakshya 49:61c9f28332ba 2128 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 2129 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 49:61c9f28332ba 2130 {
lakshya 49:61c9f28332ba 2131 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 2132 if( init == 0)
lakshya 49:61c9f28332ba 2133 {
lakshya 49:61c9f28332ba 2134 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 2135 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2136 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 49:61c9f28332ba 2137 return 0;
lakshya 49:61c9f28332ba 2138 }
lakshya 49:61c9f28332ba 2139
lakshya 49:61c9f28332ba 2140 int acq2;
lakshya 49:61c9f28332ba 2141 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2142 if(acq2 == 3)
lakshya 49:61c9f28332ba 2143 {
lakshya 49:61c9f28332ba 2144 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2145 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 49:61c9f28332ba 2146 return 3;
lakshya 49:61c9f28332ba 2147 }
lakshya 49:61c9f28332ba 2148 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2149 {
lakshya 49:61c9f28332ba 2150 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 49:61c9f28332ba 2151 return 1;
lakshya 49:61c9f28332ba 2152 }
lakshya 49:61c9f28332ba 2153 else if(acq2 == 2)
lakshya 49:61c9f28332ba 2154 {
lakshya 49:61c9f28332ba 2155 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2156 return 2;
lakshya 49:61c9f28332ba 2157 }
lakshya 49:61c9f28332ba 2158 else if(acq2 == 0)
lakshya 49:61c9f28332ba 2159 {
lakshya 49:61c9f28332ba 2160 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 2161 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2162 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 2163 return 0;
lakshya 49:61c9f28332ba 2164 }
lakshya 49:61c9f28332ba 2165 }
lakshya 49:61c9f28332ba 2166 }
lakshya 49:61c9f28332ba 2167 }
lakshya 49:61c9f28332ba 2168
lakshya 49:61c9f28332ba 2169
lakshya 49:61c9f28332ba 2170
lakshya 49:61c9f28332ba 2171
lakshya 49:61c9f28332ba 2172
lakshya 49:61c9f28332ba 2173 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2174 acs_pc.printf(" Both sensors data acq failure.Exiting.\n \r");
lakshya 20:949d13045431 2175 return 0;
sakthipriya 0:7b4c00e3912f 2176 }
sakthipriya 0:7b4c00e3912f 2177
sakthipriya 0:7b4c00e3912f 2178 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 2179 {
lakshya 20:949d13045431 2180 //// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 2181
sakthipriya 0:7b4c00e3912f 2182 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 2183 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 2184 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 2185 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 2186 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 2187 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 2188
sakthipriya 0:7b4c00e3912f 2189
lakshya 20:949d13045431 2190 //// printf("\r\r");
sakthipriya 0:7b4c00e3912f 2191
sakthipriya 0:7b4c00e3912f 2192 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 2193
sakthipriya 0:7b4c00e3912f 2194
sakthipriya 0:7b4c00e3912f 2195 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 2196
sakthipriya 0:7b4c00e3912f 2197 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 2198 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 2199 {
sakthipriya 0:7b4c00e3912f 2200 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 2201 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 2202 }
sakthipriya 0:7b4c00e3912f 2203
sakthipriya 0:7b4c00e3912f 2204 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2205 //// printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 2206 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 2207 {
lakshya 10:f93407b97750 2208 l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2209 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2210 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2211 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2212 }
lakshya 10:f93407b97750 2213 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 2214 {
lakshya 10:f93407b97750 2215 l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2216 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2217 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2218 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2219 }
lakshya 10:f93407b97750 2220 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 2221 {
lakshya 10:f93407b97750 2222 l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2223 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2224 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2225 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2226 }
lakshya 10:f93407b97750 2227 else if(l_current_x==0)
lakshya 10:f93407b97750 2228 {
lakshya 20:949d13045431 2229 //// printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 2230 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 20:949d13045431 2231 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2232 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2233 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2234 }
lakshya 10:f93407b97750 2235 else //not necessary
lakshya 10:f93407b97750 2236 {
lakshya 10:f93407b97750 2237 g_err_flag_TR_x = 1;
lakshya 20:949d13045431 2238 }
lakshya 49:61c9f28332ba 2239 acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2240
lakshya 10:f93407b97750 2241
Bragadeesh153 52:daa685b0e390 2242 //----------------------------- y-direction TR --------------------------------------------//
Bragadeesh153 52:daa685b0e390 2243
Bragadeesh153 52:daa685b0e390 2244
lakshya 10:f93407b97750 2245 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 2246
lakshya 10:f93407b97750 2247 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 2248 if (l_moment_y <0)
lakshya 10:f93407b97750 2249 {
Bragadeesh153 52:daa685b0e390 2250 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 2251 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 2252 }
lakshya 10:f93407b97750 2253
lakshya 10:f93407b97750 2254 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2255 //// printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 2256 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 2257 {
lakshya 10:f93407b97750 2258 l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2259 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2260 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2261 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2262 }
lakshya 10:f93407b97750 2263 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 2264 {
lakshya 10:f93407b97750 2265 l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2266 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2267 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2268 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2269 }
lakshya 10:f93407b97750 2270 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 2271 {
lakshya 10:f93407b97750 2272 l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2273 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2274 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2275 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 52:daa685b0e390 2276 }
lakshya 10:f93407b97750 2277 else if(l_current_y==0)
lakshya 10:f93407b97750 2278 {
lakshya 20:949d13045431 2279 //// printf("\n \r l_current_y====0");
Bragadeesh153 52:daa685b0e390 2280 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 20:949d13045431 2281 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2282 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2283 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2284 }
Bragadeesh153 52:daa685b0e390 2285 else //not necessary
lakshya 10:f93407b97750 2286 {
Bragadeesh153 52:daa685b0e390 2287 g_err_flag_TR_y = 1;
Bragadeesh153 52:daa685b0e390 2288 }
Bragadeesh153 52:daa685b0e390 2289 acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2290 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 2291
Bragadeesh153 52:daa685b0e390 2292
lakshya 20:949d13045431 2293
Bragadeesh153 52:daa685b0e390 2294
lakshya 10:f93407b97750 2295 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 2296
lakshya 20:949d13045431 2297 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 2298 if (l_moment_z <0)
lakshya 10:f93407b97750 2299 {
Bragadeesh153 52:daa685b0e390 2300 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 2301 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 2302 }
lakshya 10:f93407b97750 2303
lakshya 10:f93407b97750 2304 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2305 //// printf("current in trz is %f \r \n",l_current_z);
lakshya 20:949d13045431 2306 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 2307 {
lakshya 10:f93407b97750 2308 l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2309 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2310 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2311 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2312 }
lakshya 10:f93407b97750 2313 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 2314 {
lakshya 10:f93407b97750 2315 l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2316 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2317 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2318 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2319 }
lakshya 10:f93407b97750 2320 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 2321 {
lakshya 10:f93407b97750 2322 l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation
lakshya 20:949d13045431 2323 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2324 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2325 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 52:daa685b0e390 2326 }
lakshya 10:f93407b97750 2327 else if(l_current_z==0)
lakshya 10:f93407b97750 2328 {
lakshya 20:949d13045431 2329 //// printf("\n \r l_current_z====0");
Bragadeesh153 52:daa685b0e390 2330 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 20:949d13045431 2331 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2332 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2333 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2334 }
Bragadeesh153 52:daa685b0e390 2335 else //not necessary
lakshya 10:f93407b97750 2336 {
Bragadeesh153 52:daa685b0e390 2337 g_err_flag_TR_z = 1;
Bragadeesh153 52:daa685b0e390 2338 }
Bragadeesh153 52:daa685b0e390 2339 acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2340
lakshya 20:949d13045431 2341 //changed
lakshya 20:949d13045431 2342 if(phase_TR_x)
lakshya 20:949d13045431 2343 ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
lakshya 20:949d13045431 2344 else
lakshya 20:949d13045431 2345 ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
lakshya 20:949d13045431 2346 if(phase_TR_y)
lakshya 20:949d13045431 2347 ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
lakshya 20:949d13045431 2348 else
lakshya 20:949d13045431 2349 ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
lakshya 20:949d13045431 2350 if(phase_TR_z)
lakshya 20:949d13045431 2351 ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
lakshya 20:949d13045431 2352 else
Bragadeesh153 59:0fc0bfafaa9f 2353 ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM3);
lakshya 20:949d13045431 2354
lakshya 10:f93407b97750 2355 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 2356
lakshya 20:949d13045431 2357 //// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 2358
lakshya 10:f93407b97750 2359 }
lakshya 20:949d13045431 2360