Team Fox / Mbed 2 deprecated BAE_CODE_MARCH_2017

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@100:54514f9d3de2, 2017-03-06 (annotated)

Committer:
Bragadeesh153
Date:
Mon Mar 06 19:18:56 2017 +0000
Revision:
100:54514f9d3de2
Parent:
89:d50f09fab17f 
sw_TM

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);
lakshya 66:a5d2b8dc6b9e 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
lakshya 63:55d32e7dcad7 573
Bragadeesh153 60:bd1498f03319 574 int PARAMETER_TRANSFER_GYRO(int gyro_rng)
Bragadeesh153 60:bd1498f03319 575 {
Bragadeesh153 60:bd1498f03319 576
Bragadeesh153 60:bd1498f03319 577 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 578 if(gyro_rng ==0)
Bragadeesh153 60:bd1498f03319 579 return 0;
Bragadeesh153 60:bd1498f03319 580 char range[5];
Bragadeesh153 60:bd1498f03319 581
Bragadeesh153 60:bd1498f03319 582
Bragadeesh153 60:bd1498f03319 583 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 584 range[1] = 0x4B;
Bragadeesh153 60:bd1498f03319 585 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 586 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 587
Bragadeesh153 60:bd1498f03319 588 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 589 range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 590 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 591
Bragadeesh153 60:bd1498f03319 592 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 593 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 594 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 595
Bragadeesh153 60:bd1498f03319 596 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 597
Bragadeesh153 60:bd1498f03319 598 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 599 {
Bragadeesh153 60:bd1498f03319 600 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 601 return 0;
Bragadeesh153 60:bd1498f03319 602 }
Bragadeesh153 60:bd1498f03319 603
Bragadeesh153 60:bd1498f03319 604 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 605 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 606 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 607
Bragadeesh153 60:bd1498f03319 608 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 609
Bragadeesh153 60:bd1498f03319 610 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 611 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 612 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 613
Bragadeesh153 60:bd1498f03319 614 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 615 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 616 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 617 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 618
Bragadeesh153 60:bd1498f03319 619
Bragadeesh153 60:bd1498f03319 620
Bragadeesh153 60:bd1498f03319 621 range[0] = 0x60;
Bragadeesh153 60:bd1498f03319 622
Bragadeesh153 60:bd1498f03319 623
Bragadeesh153 60:bd1498f03319 624 range[1] = (int)(gyro_rng%256); //1000
Bragadeesh153 60:bd1498f03319 625 range[2] = (int)(gyro_rng/256);
Bragadeesh153 60:bd1498f03319 626
Bragadeesh153 60:bd1498f03319 627 //range[1]= 0x88;
Bragadeesh153 60:bd1498f03319 628 //range[2] = 0x13; //5000
Bragadeesh153 60:bd1498f03319 629
Bragadeesh153 60:bd1498f03319 630 range[3] = 0x00;
Bragadeesh153 60:bd1498f03319 631 range[4] = 0x00;
Bragadeesh153 60:bd1498f03319 632 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 633
Bragadeesh153 60:bd1498f03319 634 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 635 range[1] = 0xCB;
Bragadeesh153 60:bd1498f03319 636 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 637
Bragadeesh153 60:bd1498f03319 638
Bragadeesh153 60:bd1498f03319 639 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 640 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 641 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 642
Bragadeesh153 60:bd1498f03319 643 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 644 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 645 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 646
Bragadeesh153 60:bd1498f03319 647 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 648
Bragadeesh153 60:bd1498f03319 649 if(range[0] != ((char)(0xCB)))
Bragadeesh153 60:bd1498f03319 650 {
Bragadeesh153 60:bd1498f03319 651 printf("Parameter transfer failed \r\n");
Bragadeesh153 60:bd1498f03319 652 return 0;
Bragadeesh153 60:bd1498f03319 653 }
Bragadeesh153 60:bd1498f03319 654
Bragadeesh153 60:bd1498f03319 655 printf("Parameter transfer success\r\n");
Bragadeesh153 60:bd1498f03319 656 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 657 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 658 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 659 //i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 660 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 661
Bragadeesh153 60:bd1498f03319 662 // range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 663 // range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 664 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 665
Bragadeesh153 60:bd1498f03319 666
Bragadeesh153 60:bd1498f03319 667 printf("Reading parameter\r\n");
Bragadeesh153 60:bd1498f03319 668 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 669
Bragadeesh153 60:bd1498f03319 670 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 671 range[1] = 0x4B;
Bragadeesh153 60:bd1498f03319 672 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 673 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 674
Bragadeesh153 60:bd1498f03319 675 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 676 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 677 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 678
Bragadeesh153 60:bd1498f03319 679 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 680 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 681 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 682
Bragadeesh153 60:bd1498f03319 683 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 684
Bragadeesh153 60:bd1498f03319 685 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 686 {
Bragadeesh153 60:bd1498f03319 687 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 688 return 0;
Bragadeesh153 60:bd1498f03319 689 }
Bragadeesh153 60:bd1498f03319 690
Bragadeesh153 60:bd1498f03319 691 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 692 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 693 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 694
Bragadeesh153 60:bd1498f03319 695 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 696
Bragadeesh153 60:bd1498f03319 697 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 698 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 699 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 700
Bragadeesh153 60:bd1498f03319 701 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 702 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 703 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 704
Bragadeesh153 60:bd1498f03319 705
Bragadeesh153 60:bd1498f03319 706 return 1;
Bragadeesh153 60:bd1498f03319 707
Bragadeesh153 60:bd1498f03319 708 }
Bragadeesh153 60:bd1498f03319 709
Bragadeesh153 60:bd1498f03319 710 int PARAMETER_TRANSFER_MAG(int mag_rng)
Bragadeesh153 60:bd1498f03319 711 {
Bragadeesh153 60:bd1498f03319 712
Bragadeesh153 60:bd1498f03319 713 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 714 if(mag_rng ==0)
Bragadeesh153 60:bd1498f03319 715 return 0;
Bragadeesh153 60:bd1498f03319 716 char range[5];
Bragadeesh153 60:bd1498f03319 717
Bragadeesh153 60:bd1498f03319 718
Bragadeesh153 60:bd1498f03319 719 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 720 range[1] = 0x4A;
Bragadeesh153 60:bd1498f03319 721 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 722 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 723
Bragadeesh153 60:bd1498f03319 724 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 725 range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 726 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 727
Bragadeesh153 60:bd1498f03319 728 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 729 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 730 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 731
Bragadeesh153 60:bd1498f03319 732 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 733
Bragadeesh153 60:bd1498f03319 734 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 735 {
Bragadeesh153 60:bd1498f03319 736 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 737 return 0;
Bragadeesh153 60:bd1498f03319 738 }
Bragadeesh153 60:bd1498f03319 739
Bragadeesh153 60:bd1498f03319 740 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 741 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 742 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 743
Bragadeesh153 60:bd1498f03319 744 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 745
Bragadeesh153 60:bd1498f03319 746 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 747 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 748 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 749
Bragadeesh153 60:bd1498f03319 750 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 751 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 752 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 753 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 754
Bragadeesh153 60:bd1498f03319 755
Bragadeesh153 60:bd1498f03319 756
Bragadeesh153 60:bd1498f03319 757 range[0] = 0x60;
Bragadeesh153 60:bd1498f03319 758
Bragadeesh153 60:bd1498f03319 759
Bragadeesh153 60:bd1498f03319 760 range[1] = (int)(mag_rng%256); //1000
Bragadeesh153 60:bd1498f03319 761 range[2] = (int)(mag_rng/256);
Bragadeesh153 60:bd1498f03319 762
Bragadeesh153 60:bd1498f03319 763 //range[1]= 0x88;
Bragadeesh153 60:bd1498f03319 764 //range[2] = 0x13; //5000
Bragadeesh153 60:bd1498f03319 765
Bragadeesh153 60:bd1498f03319 766 range[3] = 0x00;
Bragadeesh153 60:bd1498f03319 767 range[4] = 0x00;
Bragadeesh153 60:bd1498f03319 768 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 769
Bragadeesh153 60:bd1498f03319 770 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 771 range[1] = 0xCA;
Bragadeesh153 60:bd1498f03319 772 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 773
Bragadeesh153 60:bd1498f03319 774
Bragadeesh153 60:bd1498f03319 775 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 776 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 777 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 778
Bragadeesh153 60:bd1498f03319 779 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 780 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 781 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 782
Bragadeesh153 60:bd1498f03319 783 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 784
Bragadeesh153 60:bd1498f03319 785 if(range[0] != ((char)(0xCB)))
Bragadeesh153 60:bd1498f03319 786 {
Bragadeesh153 60:bd1498f03319 787 printf("Parameter transfer failed \r\n");
Bragadeesh153 60:bd1498f03319 788 return 0;
Bragadeesh153 60:bd1498f03319 789 }
Bragadeesh153 60:bd1498f03319 790
Bragadeesh153 60:bd1498f03319 791 printf("Parameter transfer success\r\n");
Bragadeesh153 60:bd1498f03319 792 //wait_ms(4000);
Bragadeesh153 60:bd1498f03319 793 //range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 794 //range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 795 //i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 796 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 797
Bragadeesh153 60:bd1498f03319 798 // range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 799 // range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 800 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 801
Bragadeesh153 60:bd1498f03319 802
Bragadeesh153 60:bd1498f03319 803 printf("Reading parameter\r\n");
Bragadeesh153 60:bd1498f03319 804 //READ_PARAMETER();
Bragadeesh153 60:bd1498f03319 805
Bragadeesh153 60:bd1498f03319 806 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 807 range[1] = 0x4A;
Bragadeesh153 60:bd1498f03319 808 i2c.write(SLAVE_ADDR,range,5);
Bragadeesh153 60:bd1498f03319 809 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 810
Bragadeesh153 60:bd1498f03319 811 //range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 812 //range[1] = 0x80;
Bragadeesh153 60:bd1498f03319 813 //i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 814
Bragadeesh153 60:bd1498f03319 815 range[0]=0x3A;
Bragadeesh153 60:bd1498f03319 816 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 817 i2c.read(SLAVE_ADDR_READ,range,1);
Bragadeesh153 60:bd1498f03319 818
Bragadeesh153 60:bd1498f03319 819 printf("ACK_PARA : %.02X",range[0]);
Bragadeesh153 60:bd1498f03319 820
Bragadeesh153 60:bd1498f03319 821 if(range[0] != ((char)(0x4B)))
Bragadeesh153 60:bd1498f03319 822 {
Bragadeesh153 60:bd1498f03319 823 printf("Parameter transfer read failed \r\n");
Bragadeesh153 60:bd1498f03319 824 return 0;
Bragadeesh153 60:bd1498f03319 825 }
Bragadeesh153 60:bd1498f03319 826
Bragadeesh153 60:bd1498f03319 827 range[0]=0x3B;
Bragadeesh153 60:bd1498f03319 828 i2c.write(SLAVE_ADDR,range,1);
Bragadeesh153 60:bd1498f03319 829 i2c.read(SLAVE_ADDR_READ,range,4);
Bragadeesh153 60:bd1498f03319 830
Bragadeesh153 60:bd1498f03319 831 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 832
Bragadeesh153 60:bd1498f03319 833 range[0] = 0x64;
Bragadeesh153 60:bd1498f03319 834 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 835 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 836
Bragadeesh153 60:bd1498f03319 837 range[0] = 0x54;
Bragadeesh153 60:bd1498f03319 838 range[1] = 0x00;
Bragadeesh153 60:bd1498f03319 839 i2c.write(SLAVE_ADDR,range,2);
Bragadeesh153 60:bd1498f03319 840
Bragadeesh153 60:bd1498f03319 841
Bragadeesh153 60:bd1498f03319 842 return 1;
Bragadeesh153 60:bd1498f03319 843
Bragadeesh153 60:bd1498f03319 844 }
Bragadeesh153 60:bd1498f03319 845
lakshya 63:55d32e7dcad7 846
lakshya 20:949d13045431 847 int SENSOR_INIT()
lakshya 20:949d13045431 848 {
lakshya 49:61c9f28332ba 849 /// acs_pc.printf("Entered sensor init\n \r");
lakshya 20:949d13045431 850 cmd[0]=RESETREQ;
lakshya 20:949d13045431 851 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 852 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 853 //wait_ms(575); //waiting for loading configuration file stored in EEPROM
lakshya 20:949d13045431 854
lakshya 49:61c9f28332ba 855 /// acs_pc.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
lakshya 20:949d13045431 856
lakshya 20:949d13045431 857 if( ack!=0)
lakshya 20:949d13045431 858 {
lakshya 20:949d13045431 859 cmd[0]=RESETREQ;
lakshya 20:949d13045431 860 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 861 ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
lakshya 20:949d13045431 862 if(ack !=0)
lakshya 20:949d13045431 863 return 0;
lakshya 20:949d13045431 864 }
lakshya 20:949d13045431 865
lakshya 49:61c9f28332ba 866
lakshya 49:61c9f28332ba 867 wait_ms(275);
lakshya 49:61c9f28332ba 868 //kick_WDOG();
lakshya 49:61c9f28332ba 869 acs_pc.printf("\n\r lvl2");
lakshya 49:61c9f28332ba 870 wait_ms(300);
lakshya 20:949d13045431 871
sakthipriya 0:7b4c00e3912f 872 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 873 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 874
lakshya 20:949d13045431 875 if( ack!=0)
lakshya 20:949d13045431 876 {
lakshya 20:949d13045431 877 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 878 if(ack!=0)
lakshya 20:949d13045431 879 return 0;
lakshya 20:949d13045431 880 }
lakshya 20:949d13045431 881
lakshya 20:949d13045431 882 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 883
lakshya 20:949d13045431 884 if( ack!=0)
lakshya 20:949d13045431 885 {
lakshya 20:949d13045431 886 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 887 if(ack!=0)
lakshya 20:949d13045431 888 return 0;
lakshya 20:949d13045431 889 }
lakshya 20:949d13045431 890
lakshya 49:61c9f28332ba 891 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
lakshya 66:a5d2b8dc6b9e 892 acs_pc.printf("\n\r stuck here maaan nupe passed it!! inside ACS");
lakshya 20:949d13045431 893 //to check whether EEPROM is uploaded properly
sakthipriya 0:7b4c00e3912f 894 switch((int)store) {
lakshya 20:949d13045431 895 case(3): {
sakthipriya 0:7b4c00e3912f 896 break;
sakthipriya 0:7b4c00e3912f 897 }
sakthipriya 0:7b4c00e3912f 898 case(11): {
sakthipriya 0:7b4c00e3912f 899 break;
sakthipriya 0:7b4c00e3912f 900 }
sakthipriya 0:7b4c00e3912f 901 default: {
sakthipriya 0:7b4c00e3912f 902 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 903 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 904 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 905 if( ack!=0)
lakshya 20:949d13045431 906 {
lakshya 20:949d13045431 907 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 908 if(ack!=0)
lakshya 20:949d13045431 909 return 0;
lakshya 20:949d13045431 910 }
lakshya 49:61c9f28332ba 911 wait_ms(275);//should be 600
lakshya 49:61c9f28332ba 912 //kick_WDOG();
lakshya 49:61c9f28332ba 913 acs_pc.printf("\n\r lvl3");
lakshya 49:61c9f28332ba 914 wait_ms(300);
lakshya 20:949d13045431 915 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 916 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 917 if( ack!=0)
lakshya 20:949d13045431 918 {
lakshya 20:949d13045431 919 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 920 if(ack!=0)
lakshya 20:949d13045431 921 return 0;
lakshya 20:949d13045431 922 }
lakshya 20:949d13045431 923 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 924 if( ack!=0)
lakshya 20:949d13045431 925 {
lakshya 20:949d13045431 926 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 927 if(ack!=0)
lakshya 20:949d13045431 928 return 0;
lakshya 20:949d13045431 929 }
lakshya 49:61c9f28332ba 930 /// acs_pc.printf("Sentral Status is %x\n \r",(int)store);
lakshya 20:949d13045431 931
sakthipriya 0:7b4c00e3912f 932 }
sakthipriya 0:7b4c00e3912f 933 }
lakshya 20:949d13045431 934
lakshya 20:949d13045431 935 int manual=0;
lakshya 20:949d13045431 936 if( ((int)store != 11 )&&((int)store != 3))
lakshya 20:949d13045431 937 {
lakshya 20:949d13045431 938
lakshya 20:949d13045431 939 cmd[0]=RESETREQ;
lakshya 20:949d13045431 940 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 941 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 942 if( ack!=0)
lakshya 20:949d13045431 943 {
lakshya 20:949d13045431 944 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 945 if(ack!=0)
lakshya 20:949d13045431 946 return 0;
lakshya 20:949d13045431 947 }
lakshya 49:61c9f28332ba 948 wait_ms(275);
lakshya 49:61c9f28332ba 949 //kick_WDOG();
lakshya 49:61c9f28332ba 950 acs_pc.printf("\n\r lvl4");
lakshya 49:61c9f28332ba 951 wait_ms(300);
lakshya 20:949d13045431 952
lakshya 49:61c9f28332ba 953 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 954 manual = CONFIG_UPLOAD();
lakshya 49:61c9f28332ba 955 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 956
lakshya 20:949d13045431 957 if(manual == 0)
lakshya 20:949d13045431 958 {
lakshya 20:949d13045431 959 //MANUAL CONFIGURATION FAILED
lakshya 20:949d13045431 960 return 0;
lakshya 20:949d13045431 961 }
lakshya 20:949d13045431 962
lakshya 20:949d13045431 963 }
lakshya 20:949d13045431 964 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
lakshya 20:949d13045431 965 cmd[1]=BIT_RUN_ENB;
lakshya 20:949d13045431 966 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 967 if( ack!=0)
lakshya 20:949d13045431 968 {
lakshya 20:949d13045431 969 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 970 if(ack!=0)
lakshya 20:949d13045431 971 return 0;
lakshya 20:949d13045431 972 }
lakshya 20:949d13045431 973
lakshya 20:949d13045431 974 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
lakshya 20:949d13045431 975 cmd[1]=BIT_MAGODR;
lakshya 20:949d13045431 976 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 977 if( ack!=0)
lakshya 20:949d13045431 978 {
lakshya 20:949d13045431 979 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 980 if(ack!=0)
lakshya 20:949d13045431 981 return 0;
lakshya 20:949d13045431 982 }
lakshya 20:949d13045431 983
lakshya 20:949d13045431 984 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
lakshya 20:949d13045431 985 cmd[1]=BIT_GYROODR;
lakshya 20:949d13045431 986 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 987 if( ack!=0)
lakshya 20:949d13045431 988 {
lakshya 20:949d13045431 989 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 990 if(ack!=0)
lakshya 20:949d13045431 991 return 0;
lakshya 20:949d13045431 992 }
lakshya 20:949d13045431 993
lakshya 20:949d13045431 994 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
lakshya 20:949d13045431 995 cmd[1]=0x00;
lakshya 20:949d13045431 996 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 997 if( ack!=0)
lakshya 20:949d13045431 998 {
lakshya 20:949d13045431 999 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1000 if(ack!=0)
lakshya 20:949d13045431 1001 return 0;
lakshya 20:949d13045431 1002 }
lakshya 20:949d13045431 1003 //wait_ms(20);
lakshya 20:949d13045431 1004 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
lakshya 20:949d13045431 1005 cmd[1]=0x00;
lakshya 20:949d13045431 1006 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1007 if( ack!=0)
lakshya 20:949d13045431 1008 {
lakshya 20:949d13045431 1009 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1010 if(ack!=0)
lakshya 20:949d13045431 1011 return 0;
lakshya 20:949d13045431 1012 }
lakshya 20:949d13045431 1013
lakshya 20:949d13045431 1014 cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
lakshya 20:949d13045431 1015 cmd[1]=BIT_EVT_ENB;
lakshya 20:949d13045431 1016 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1017 if( ack!=0)
lakshya 20:949d13045431 1018 {
lakshya 20:949d13045431 1019 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 1020 if(ack!=0)
lakshya 20:949d13045431 1021 return 0;
lakshya 20:949d13045431 1022 }
lakshya 20:949d13045431 1023
Bragadeesh153 60:bd1498f03319 1024 PARAMETER_TRANSFER_GYRO(0); // dont set range //PARAMETER of function sets range of gyroscope
Bragadeesh153 60:bd1498f03319 1025 PARAMETER_TRANSFER_MAG(0); // dont set range //PARAMETER of function sets range of magnetometer
lakshya 63:55d32e7dcad7 1026
lakshya 20:949d13045431 1027 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1028 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1029 if( ack!=0)
lakshya 20:949d13045431 1030 {
lakshya 20:949d13045431 1031 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1032 if(ack!=0)
lakshya 20:949d13045431 1033 return 0;
lakshya 20:949d13045431 1034 }
lakshya 20:949d13045431 1035
lakshya 20:949d13045431 1036 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 1037 if( ack!=0)
lakshya 20:949d13045431 1038 {
lakshya 20:949d13045431 1039 ack= i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 1040 if(ack!=0)
lakshya 20:949d13045431 1041 return 0;
lakshya 20:949d13045431 1042 }
lakshya 20:949d13045431 1043
lakshya 49:61c9f28332ba 1044 /// acs_pc.printf("Sentral Status after initialising is %x\n \r",(int)store);
lakshya 20:949d13045431 1045
lakshya 20:949d13045431 1046 if( (int)store == 3) //Check if initialised properly and not in idle state
lakshya 20:949d13045431 1047 {
lakshya 49:61c9f28332ba 1048 /// acs_pc.printf("Exited sensor init successfully\n \r");
lakshya 20:949d13045431 1049 return 1;
lakshya 20:949d13045431 1050 }
lakshya 20:949d13045431 1051
lakshya 20:949d13045431 1052
lakshya 49:61c9f28332ba 1053 //// acs_pc.printf("Sensor init failed \n \r") ;
lakshya 20:949d13045431 1054 return 0;
lakshya 20:949d13045431 1055 }
lakshya 20:949d13045431 1056
lakshya 20:949d13045431 1057 int FCTN_ACS_INIT()
lakshya 20:949d13045431 1058 {
lakshya 20:949d13045431 1059 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
lakshya 20:949d13045431 1060
lakshya 20:949d13045431 1061 int working=0;
lakshya 20:949d13045431 1062
lakshya 49:61c9f28332ba 1063 /// acs_pc.printf("Attitude sensor init called \n \r");
lakshya 49:61c9f28332ba 1064 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 17:fc782f7548c6 1065
lakshya 17:fc782f7548c6 1066
lakshya 20:949d13045431 1067 if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 1068 {
lakshya 20:949d13045431 1069
lakshya 49:61c9f28332ba 1070 /// acs_pc.printf("Sensor 1 marked working \n \r");
lakshya 49:61c9f28332ba 1071 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1072 working = SENSOR_INIT();
lakshya 49:61c9f28332ba 1073 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 1074 if(working ==1)
lakshya 20:949d13045431 1075 {
lakshya 20:949d13045431 1076 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1077 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
lakshya 49:61c9f28332ba 1078 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
lakshya 20:949d13045431 1079 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 1080 return 1;
lakshya 20:949d13045431 1081 }
lakshya 20:949d13045431 1082
lakshya 20:949d13045431 1083
lakshya 20:949d13045431 1084
lakshya 49:61c9f28332ba 1085 /// acs_pc.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
Bragadeesh153 52:daa685b0e390 1086 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1087 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1088
lakshya 20:949d13045431 1089 }
lakshya 17:fc782f7548c6 1090
lakshya 49:61c9f28332ba 1091 /// acs_pc.printf("Sensor 1 not working. Trying Sensor 2\n \r");
lakshya 17:fc782f7548c6 1092
lakshya 20:949d13045431 1093 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 1094 {
lakshya 20:949d13045431 1095
lakshya 20:949d13045431 1096
lakshya 20:949d13045431 1097 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1098 wait_ms(5);
lakshya 49:61c9f28332ba 1099 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1100 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1101 working = SENSOR_INIT();
lakshya 49:61c9f28332ba 1102 timer_SENSOR_INIT.stop();
lakshya 49:61c9f28332ba 1103
lakshya 20:949d13045431 1104 if(working ==1)
lakshya 20:949d13045431 1105 {
lakshya 49:61c9f28332ba 1106 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1107 /// acs_pc.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
lakshya 20:949d13045431 1108 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 1109 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 1110 return 2;
lakshya 20:949d13045431 1111 }
lakshya 20:949d13045431 1112
lakshya 20:949d13045431 1113 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1114 wait_ms(5);
lakshya 20:949d13045431 1115
lakshya 20:949d13045431 1116 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1117
lakshya 20:949d13045431 1118
lakshya 20:949d13045431 1119 }
lakshya 17:fc782f7548c6 1120
lakshya 49:61c9f28332ba 1121 /// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1122 /// acs_pc.printf("Sensor 2 also not working.Exit init.\n \r");
lakshya 17:fc782f7548c6 1123
lakshya 20:949d13045431 1124 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
lakshya 20:949d13045431 1125 return 0;
sakthipriya 0:7b4c00e3912f 1126 }
sakthipriya 0:7b4c00e3912f 1127
lakshya 20:949d13045431 1128
lakshya 20:949d13045431 1129 int SENSOR_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 1130 {
lakshya 20:949d13045431 1131 //int mag_only=0;
lakshya 49:61c9f28332ba 1132 /// acs_pc.printf("Entering Sensor data acq.\n \r");
lakshya 20:949d13045431 1133 char status;
lakshya 20:949d13045431 1134 int sentral;
lakshya 20:949d13045431 1135 int event;
lakshya 20:949d13045431 1136 int sensor;
lakshya 20:949d13045431 1137 int error;
lakshya 20:949d13045431 1138 int init;
lakshya 20:949d13045431 1139
lakshya 20:949d13045431 1140 uint8_t gyro_error=0;
lakshya 20:949d13045431 1141 uint8_t mag_error=0;
lakshya 20:949d13045431 1142
lakshya 20:949d13045431 1143 //int ack1;
lakshya 20:949d13045431 1144 //int ack2;
lakshya 20:949d13045431 1145
sakthipriya 0:7b4c00e3912f 1146 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1147 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1148 if(ack!=0)
lakshya 20:949d13045431 1149 {
lakshya 20:949d13045431 1150 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1151 if(ack!=0)
lakshya 20:949d13045431 1152 return 0;
lakshya 20:949d13045431 1153 }
lakshya 20:949d13045431 1154
lakshya 20:949d13045431 1155 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1156 if(ack!=0)
lakshya 20:949d13045431 1157 {
lakshya 20:949d13045431 1158 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1159 if(ack!=0)
lakshya 20:949d13045431 1160 return 0;
lakshya 20:949d13045431 1161 }
lakshya 17:fc782f7548c6 1162
lakshya 20:949d13045431 1163 event = (int)status;
lakshya 20:949d13045431 1164
lakshya 63:55d32e7dcad7 1165 //if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1166 if(1)
lakshya 20:949d13045431 1167 {
lakshya 20:949d13045431 1168 ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 1169 }
lakshya 20:949d13045431 1170 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1171 {
lakshya 20:949d13045431 1172 ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 1173 }
lakshya 20:949d13045431 1174
lakshya 20:949d13045431 1175 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1176 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1177 if(ack!=0)
lakshya 20:949d13045431 1178 {
lakshya 20:949d13045431 1179 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1180 if(ack!=0)
lakshya 20:949d13045431 1181 return 0;
lakshya 20:949d13045431 1182 }
lakshya 20:949d13045431 1183
lakshya 20:949d13045431 1184 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1185 if(ack!=0)
lakshya 20:949d13045431 1186 {
lakshya 20:949d13045431 1187 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1188 if(ack!=0)
lakshya 20:949d13045431 1189 return 0;
lakshya 20:949d13045431 1190 }
lakshya 20:949d13045431 1191
lakshya 17:fc782f7548c6 1192
lakshya 20:949d13045431 1193 sentral = (int) status;
lakshya 20:949d13045431 1194
lakshya 63:55d32e7dcad7 1195 //if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1196 if(1)
lakshya 20:949d13045431 1197 {
lakshya 20:949d13045431 1198 ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 1199 }
lakshya 20:949d13045431 1200 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1201 {
lakshya 20:949d13045431 1202 ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 1203 }
lakshya 20:949d13045431 1204
lakshya 49:61c9f28332ba 1205 /// acs_pc.printf("Event Status is %x\n \r",event);
lakshya 49:61c9f28332ba 1206 /// acs_pc.printf("Sentral Status is %x\n \r",sentral);
lakshya 20:949d13045431 1207
lakshya 17:fc782f7548c6 1208
lakshya 17:fc782f7548c6 1209
lakshya 49:61c9f28332ba 1210 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 1211 {
lakshya 20:949d13045431 1212
lakshya 49:61c9f28332ba 1213 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1214 timer_SENSOR_INIT.start();
lakshya 20:949d13045431 1215 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1216 timer_SENSOR_INIT.stop();
lakshya 20:949d13045431 1217
lakshya 20:949d13045431 1218 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1219 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1220 if(ack!=0)
lakshya 20:949d13045431 1221 {
lakshya 20:949d13045431 1222 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1223 if(ack!=0)
lakshya 20:949d13045431 1224 return 0;
lakshya 20:949d13045431 1225 }
lakshya 20:949d13045431 1226
lakshya 20:949d13045431 1227 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1228 if(ack!=0)
lakshya 20:949d13045431 1229 {
lakshya 20:949d13045431 1230 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1231 if(ack!=0)
lakshya 20:949d13045431 1232 return 0;
lakshya 20:949d13045431 1233 }
lakshya 20:949d13045431 1234
lakshya 20:949d13045431 1235 event = (int)status;
lakshya 20:949d13045431 1236
lakshya 20:949d13045431 1237 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 1238 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1239 if(ack!=0)
lakshya 20:949d13045431 1240 {
lakshya 20:949d13045431 1241 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1242 if(ack!=0)
lakshya 20:949d13045431 1243 return 0;
lakshya 20:949d13045431 1244 }
lakshya 20:949d13045431 1245
lakshya 20:949d13045431 1246 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1247 if(ack!=0)
lakshya 20:949d13045431 1248 {
lakshya 20:949d13045431 1249 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1250 if(ack!=0)
lakshya 20:949d13045431 1251 return 0;
lakshya 20:949d13045431 1252 }
lakshya 20:949d13045431 1253
lakshya 20:949d13045431 1254 sentral = (int)status;
lakshya 49:61c9f28332ba 1255 int poll_status;
lakshya 49:61c9f28332ba 1256 /// acs_pc.printf("Event Status after resetting and init is %x\n \r",event);
lakshya 20:949d13045431 1257
lakshya 49:61c9f28332ba 1258 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 1259 {
lakshya 20:949d13045431 1260
lakshya 20:949d13045431 1261 cmd[0]=ERROR;
lakshya 20:949d13045431 1262 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1263 if(ack!=0)
lakshya 20:949d13045431 1264 {
lakshya 20:949d13045431 1265 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1266 if(ack!=0)
lakshya 20:949d13045431 1267 return 0;
lakshya 20:949d13045431 1268 }
lakshya 20:949d13045431 1269
lakshya 20:949d13045431 1270 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1271 if(ack!=0)
lakshya 20:949d13045431 1272 {
lakshya 20:949d13045431 1273
lakshya 20:949d13045431 1274 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 1275 {
lakshya 20:949d13045431 1276 ATS1_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 1277 }
lakshya 20:949d13045431 1278 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1279 {
lakshya 20:949d13045431 1280 ATS2_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 1281 }
lakshya 20:949d13045431 1282 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1283 if(ack!=0)
lakshya 20:949d13045431 1284 return 0;
lakshya 20:949d13045431 1285 }
lakshya 20:949d13045431 1286
lakshya 20:949d13045431 1287 error = (int)status;
lakshya 63:55d32e7dcad7 1288
Bragadeesh153 59:0fc0bfafaa9f 1289 //if(ACS_ATS_STATUS&0xC0 == 0x40)
Bragadeesh153 59:0fc0bfafaa9f 1290 if(1)
lakshya 20:949d13045431 1291 {
lakshya 20:949d13045431 1292 ATS1_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 1293 }
lakshya 20:949d13045431 1294 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 1295 {
lakshya 20:949d13045431 1296 ATS2_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 1297 }
lakshya 20:949d13045431 1298
lakshya 20:949d13045431 1299 cmd[0]=SENSORSTATUS;
lakshya 20:949d13045431 1300 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1301 if(ack!=0)
lakshya 20:949d13045431 1302 {
lakshya 20:949d13045431 1303 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1304 if(ack!=0)
lakshya 20:949d13045431 1305 return 0;
lakshya 20:949d13045431 1306 }
lakshya 20:949d13045431 1307
lakshya 20:949d13045431 1308 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1309 if(ack!=0)
lakshya 20:949d13045431 1310 {
lakshya 20:949d13045431 1311 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1312 if(ack!=0)
lakshya 20:949d13045431 1313 return 0;
lakshya 20:949d13045431 1314 }
lakshya 20:949d13045431 1315
lakshya 20:949d13045431 1316 sensor = (int)status;
lakshya 20:949d13045431 1317
lakshya 20:949d13045431 1318
lakshya 20:949d13045431 1319 if((error!=0) || (sensor!=0))
lakshya 20:949d13045431 1320 {
Bragadeesh153 88:d9d647e3701f 1321 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
lakshya 20:949d13045431 1322 {
lakshya 49:61c9f28332ba 1323 acs_pc.printf("error in gyro.\n \r");
lakshya 20:949d13045431 1324 gyro_error = 1;
lakshya 20:949d13045431 1325 }
lakshya 20:949d13045431 1326
Bragadeesh153 88:d9d647e3701f 1327
Bragadeesh153 88:d9d647e3701f 1328 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
lakshya 20:949d13045431 1329 {
lakshya 20:949d13045431 1330
lakshya 49:61c9f28332ba 1331 acs_pc.printf("error in mag.Exiting.\n \r");
lakshya 20:949d13045431 1332 mag_error = 1;
lakshya 20:949d13045431 1333 }
lakshya 20:949d13045431 1334 if( (gyro_error!=1)&&(mag_error!=1))
lakshya 20:949d13045431 1335 {
lakshya 49:61c9f28332ba 1336 acs_pc.printf("error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1337 return 0;
lakshya 20:949d13045431 1338
lakshya 20:949d13045431 1339 }
lakshya 20:949d13045431 1340 }
lakshya 20:949d13045431 1341
lakshya 20:949d13045431 1342
lakshya 49:61c9f28332ba 1343 if(((event & 1) == 1 ))
lakshya 20:949d13045431 1344 {
lakshya 49:61c9f28332ba 1345 /// acs_pc.printf("error in CPU Reset.\n \r");
lakshya 20:949d13045431 1346 return 0;
lakshya 20:949d13045431 1347
lakshya 20:949d13045431 1348 }
lakshya 49:61c9f28332ba 1349 int poll =0;
lakshya 49:61c9f28332ba 1350 poll_status =1;
lakshya 49:61c9f28332ba 1351 while((poll<15)&&(((event & 8) != 8 )||((event & 32) != 32 )))
lakshya 49:61c9f28332ba 1352 //if(((event & 8) != 8 )||((event & 32) != 32 ))
lakshya 20:949d13045431 1353 {
lakshya 49:61c9f28332ba 1354 poll++;
lakshya 49:61c9f28332ba 1355 acs_pc.printf("Data not ready waiting...POLL#%d\n \r",poll);
lakshya 20:949d13045431 1356 //POLL
lakshya 20:949d13045431 1357 wait_ms(200);
lakshya 20:949d13045431 1358
lakshya 20:949d13045431 1359 cmd[0]=EVT_STATUS;
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 {
lakshya 20:949d13045431 1364 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1365 if(ack!=0)
lakshya 20:949d13045431 1366 return 0;
lakshya 20:949d13045431 1367 }
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 {
lakshya 20:949d13045431 1372 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1373 if(ack!=0)
lakshya 20:949d13045431 1374 return 0;
lakshya 20:949d13045431 1375 }
lakshya 20:949d13045431 1376
lakshya 20:949d13045431 1377 event = (int)status;
lakshya 49:61c9f28332ba 1378 if((event & 32) != 32 )
lakshya 20:949d13045431 1379 {
lakshya 20:949d13045431 1380
lakshya 49:61c9f28332ba 1381 acs_pc.printf("Gyro not ready..\n \r");
lakshya 20:949d13045431 1382 gyro_error = 1;
lakshya 20:949d13045431 1383
lakshya 20:949d13045431 1384 }
lakshya 49:61c9f28332ba 1385 else
lakshya 49:61c9f28332ba 1386 {
lakshya 49:61c9f28332ba 1387 gyro_error = 0;
lakshya 49:61c9f28332ba 1388 }
lakshya 20:949d13045431 1389
lakshya 49:61c9f28332ba 1390 if((event & 8) != 8 )
lakshya 20:949d13045431 1391 {
lakshya 49:61c9f28332ba 1392 acs_pc.printf("Mag not ready..\n \r");
lakshya 20:949d13045431 1393 mag_error=1;
lakshya 20:949d13045431 1394 }
lakshya 49:61c9f28332ba 1395 else
lakshya 49:61c9f28332ba 1396 {
lakshya 49:61c9f28332ba 1397 mag_error=0;
lakshya 49:61c9f28332ba 1398 }
lakshya 20:949d13045431 1399
lakshya 20:949d13045431 1400
lakshya 20:949d13045431 1401 }
lakshya 49:61c9f28332ba 1402
lakshya 49:61c9f28332ba 1403 if(poll!=15)
lakshya 49:61c9f28332ba 1404 {
lakshya 49:61c9f28332ba 1405 poll_status = 0;
lakshya 49:61c9f28332ba 1406 }
lakshya 20:949d13045431 1407
lakshya 20:949d13045431 1408
lakshya 20:949d13045431 1409 }
lakshya 20:949d13045431 1410
lakshya 49:61c9f28332ba 1411 if(((mag_error !=1)&&(gyro_error!=1))&&(poll_status!=0))
lakshya 20:949d13045431 1412 {
lakshya 49:61c9f28332ba 1413 acs_pc.printf("Error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1414 return 0;
lakshya 20:949d13045431 1415 }
lakshya 20:949d13045431 1416
lakshya 49:61c9f28332ba 1417 if(((mag_error ==1)&&(gyro_error==1))&&(poll_status!=0))
lakshya 20:949d13045431 1418 {
lakshya 49:61c9f28332ba 1419 acs_pc.printf("Error in both gyro and mag.Exiting.\n \r");
lakshya 20:949d13045431 1420 return 0;
lakshya 20:949d13045431 1421 }
lakshya 20:949d13045431 1422
lakshya 20:949d13045431 1423 }
lakshya 20:949d13045431 1424
lakshya 20:949d13045431 1425
lakshya 20:949d13045431 1426 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1427 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1428 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1429 if(ack != 0)
lakshya 20:949d13045431 1430 {
lakshya 20:949d13045431 1431 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1432 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1433 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1434 if(ack !=1)
lakshya 20:949d13045431 1435 return 0;
lakshya 20:949d13045431 1436
lakshya 20:949d13045431 1437 }
lakshya 20:949d13045431 1438
lakshya 20:949d13045431 1439
lakshya 49:61c9f28332ba 1440 // acs_pc.printf("\nGyro Values:\n");
lakshya 20:949d13045431 1441 if (gyro_error!=1)
lakshya 20:949d13045431 1442 {
lakshya 20:949d13045431 1443 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1444 //concatenating gyro LSB and MSB to get 16 bit signed data values
lakshya 20:949d13045431 1445 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 1446 gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
lakshya 20:949d13045431 1447 gyro_data[i]=gyro_data[i]/senstivity_gyro;
lakshya 20:949d13045431 1448 actual_data.AngularSpeed_actual[i] = gyro_data[i];
lakshya 20:949d13045431 1449 }
lakshya 20:949d13045431 1450 }
lakshya 20:949d13045431 1451
lakshya 20:949d13045431 1452 if(mag_error!=1)
lakshya 20:949d13045431 1453 {
lakshya 20:949d13045431 1454 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1455 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
lakshya 20:949d13045431 1456 actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
lakshya 20:949d13045431 1457
lakshya 20:949d13045431 1458 mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
lakshya 20:949d13045431 1459 mag_data[i]=mag_data[i]/senstivity_mag;
lakshya 20:949d13045431 1460 actual_data.Bvalue_actual[i] = mag_data[i];
lakshya 20:949d13045431 1461 }
lakshya 20:949d13045431 1462 }
lakshya 20:949d13045431 1463
lakshya 20:949d13045431 1464
lakshya 20:949d13045431 1465 if(mag_error == 1)
lakshya 20:949d13045431 1466 {
lakshya 20:949d13045431 1467
lakshya 49:61c9f28332ba 1468 acs_pc.printf("Gyro only successful.\n \r");
lakshya 20:949d13045431 1469 return 1;
lakshya 20:949d13045431 1470 }
lakshya 20:949d13045431 1471 if(gyro_error == 1)
lakshya 49:61c9f28332ba 1472 {
lakshya 49:61c9f28332ba 1473 acs_pc.printf("Mag only successful.\n \r");
lakshya 20:949d13045431 1474 return 2;
lakshya 20:949d13045431 1475 }
lakshya 17:fc782f7548c6 1476
lakshya 49:61c9f28332ba 1477 //acs_pc.printf("Reading data success.\n \r");
lakshya 20:949d13045431 1478 return 3;
lakshya 20:949d13045431 1479 }
lakshya 20:949d13045431 1480
lakshya 20:949d13045431 1481
lakshya 20:949d13045431 1482 int FCTN_ATS_DATA_ACQ()
lakshya 20:949d13045431 1483 {
lakshya 20:949d13045431 1484 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1485 actual_data.AngularSpeed_actual[i] = 0;
lakshya 20:949d13045431 1486 actual_data.Bvalue_actual[i] = 0;
lakshya 20:949d13045431 1487 }
lakshya 20:949d13045431 1488
lakshya 20:949d13045431 1489 int acq;
lakshya 20:949d13045431 1490 int init;
lakshya 20:949d13045431 1491
lakshya 49:61c9f28332ba 1492 //// acs_pc.printf("DATA_ACQ called \n \r");
lakshya 49:61c9f28332ba 1493 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1494
lakshya 20:949d13045431 1495
lakshya 20:949d13045431 1496 if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
lakshya 20:949d13045431 1497 {
lakshya 49:61c9f28332ba 1498 timer_SENSOR_DATA_ACQ.start();
lakshya 20:949d13045431 1499 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1500 timer_SENSOR_DATA_ACQ.stop();
lakshya 49:61c9f28332ba 1501 //pc.pritnf("\n\r timer_SENSOR_DATA_ACQ is %f",timer_SENSOR_DATA_ACQ.read());
lakshya 49:61c9f28332ba 1502
lakshya 49:61c9f28332ba 1503
lakshya 20:949d13045431 1504 if(acq == 3)
lakshya 20:949d13045431 1505 {
lakshya 20:949d13045431 1506
lakshya 20:949d13045431 1507 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1508
lakshya 20:949d13045431 1509 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1510 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1511 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1512 return 3;
lakshya 20:949d13045431 1513 }
lakshya 20:949d13045431 1514 else if((acq == 2)||(acq==1))
lakshya 20:949d13045431 1515 {
lakshya 49:61c9f28332ba 1516 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1517 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 20:949d13045431 1518 {
lakshya 20:949d13045431 1519 //other sensor both working, off or
lakshya 20:949d13045431 1520 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1521 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1522
Bragadeesh153 52:daa685b0e390 1523 ATS1_SW_ENABLE = 01; //switch off sensor 1
lakshya 20:949d13045431 1524 wait_ms(5);
lakshya 20:949d13045431 1525 if(acq == 1)
lakshya 20:949d13045431 1526 {
lakshya 20:949d13045431 1527 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 20:949d13045431 1528 }
lakshya 20:949d13045431 1529 if(acq==2)
lakshya 20:949d13045431 1530 {
lakshya 20:949d13045431 1531 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 20:949d13045431 1532 }
lakshya 20:949d13045431 1533
lakshya 20:949d13045431 1534 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 20:949d13045431 1535 wait_ms(5);
lakshya 49:61c9f28332ba 1536
lakshya 49:61c9f28332ba 1537 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1538 timer_SENSOR_INIT.start();
lakshya 49:61c9f28332ba 1539 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1540 timer_SENSOR_INIT.stop();
lakshya 49:61c9f28332ba 1541 //sensor 2 init
lakshya 20:949d13045431 1542 if( init == 0)
lakshya 20:949d13045431 1543 {
lakshya 49:61c9f28332ba 1544 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1545 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1546 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1547 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1548 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 20:949d13045431 1549 if(acq == 1)
lakshya 20:949d13045431 1550 {
lakshya 20:949d13045431 1551 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 20:949d13045431 1552 }
lakshya 20:949d13045431 1553 if(acq==2)
lakshya 20:949d13045431 1554 {
lakshya 20:949d13045431 1555 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1556 }
lakshya 20:949d13045431 1557 return acq;
lakshya 20:949d13045431 1558 }
lakshya 20:949d13045431 1559
lakshya 20:949d13045431 1560 int acq2;
lakshya 20:949d13045431 1561 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1562 if(acq2 == 3)
lakshya 20:949d13045431 1563 {
lakshya 20:949d13045431 1564 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1565 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 20:949d13045431 1566 return 3;
lakshya 20:949d13045431 1567 }
lakshya 20:949d13045431 1568 else if(acq2 == 1)
lakshya 20:949d13045431 1569 {
lakshya 20:949d13045431 1570 if(acq==2)
lakshya 20:949d13045431 1571 {
lakshya 20:949d13045431 1572 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1573 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1574 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 20:949d13045431 1575 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1576 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1577 return 3;
lakshya 20:949d13045431 1578 }
lakshya 20:949d13045431 1579 else
lakshya 20:949d13045431 1580 {
lakshya 20:949d13045431 1581 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 20:949d13045431 1582 return 1;
lakshya 20:949d13045431 1583 }
lakshya 20:949d13045431 1584 }
lakshya 20:949d13045431 1585
lakshya 20:949d13045431 1586 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 20:949d13045431 1587 {
lakshya 20:949d13045431 1588 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1589 return 2;
lakshya 20:949d13045431 1590 }
lakshya 20:949d13045431 1591 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 20:949d13045431 1592 {
lakshya 49:61c9f28332ba 1593 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1594 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1595 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 88:d9d647e3701f 1596 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1597 wait_ms(5);
lakshya 20:949d13045431 1598 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 20:949d13045431 1599 return acq;
lakshya 20:949d13045431 1600 }
lakshya 20:949d13045431 1601
lakshya 20:949d13045431 1602 }
lakshya 20:949d13045431 1603 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1604 {
lakshya 20:949d13045431 1605 if(acq == 1)
lakshya 20:949d13045431 1606 {
lakshya 20:949d13045431 1607 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 20:949d13045431 1608 return 1;
lakshya 20:949d13045431 1609 }
lakshya 20:949d13045431 1610 if(acq==2)
lakshya 20:949d13045431 1611 {
lakshya 20:949d13045431 1612 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1613 return 2;
lakshya 20:949d13045431 1614 }
lakshya 49:61c9f28332ba 1615 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1616 return acq;
lakshya 20:949d13045431 1617
lakshya 20:949d13045431 1618 }
sakthipriya 0:7b4c00e3912f 1619 }
lakshya 20:949d13045431 1620
lakshya 20:949d13045431 1621 else if(acq == 0)
lakshya 20:949d13045431 1622 {
lakshya 49:61c9f28332ba 1623 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1624 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1625 wait_ms(5); //Switch ON sensor 2
lakshya 20:949d13045431 1626 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1627 wait_ms(5);
lakshya 20:949d13045431 1628 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1629 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 20:949d13045431 1630 {
lakshya 20:949d13045431 1631 init = SENSOR_INIT();
lakshya 20:949d13045431 1632 if( init == 0)
lakshya 20:949d13045431 1633 {
lakshya 49:61c9f28332ba 1634 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 20:949d13045431 1635 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1636 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 20:949d13045431 1637 return 0;
lakshya 20:949d13045431 1638 }
lakshya 20:949d13045431 1639
lakshya 20:949d13045431 1640 int acq2;
lakshya 20:949d13045431 1641 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1642 if(acq2 == 3)
lakshya 20:949d13045431 1643 {
lakshya 20:949d13045431 1644 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1645 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 20:949d13045431 1646 return 3;
lakshya 20:949d13045431 1647 }
lakshya 20:949d13045431 1648 else if(acq2 == 1)
lakshya 20:949d13045431 1649 {
lakshya 20:949d13045431 1650 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 20:949d13045431 1651 return 1;
lakshya 20:949d13045431 1652 }
lakshya 20:949d13045431 1653 else if(acq2 == 2)
lakshya 20:949d13045431 1654 {
lakshya 20:949d13045431 1655 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1656 return 2;
lakshya 20:949d13045431 1657 }
lakshya 20:949d13045431 1658 else if(acq2 == 0)
lakshya 20:949d13045431 1659 {
lakshya 49:61c9f28332ba 1660 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 20:949d13045431 1661 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1662
lakshya 20:949d13045431 1663 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1664 return 0;
lakshya 20:949d13045431 1665 }
lakshya 20:949d13045431 1666
lakshya 20:949d13045431 1667 }
lakshya 20:949d13045431 1668
sakthipriya 0:7b4c00e3912f 1669 }
lakshya 20:949d13045431 1670
lakshya 20:949d13045431 1671
sakthipriya 0:7b4c00e3912f 1672 }
lakshya 20:949d13045431 1673
lakshya 20:949d13045431 1674 if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
lakshya 20:949d13045431 1675 {
lakshya 20:949d13045431 1676 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 1677 if(acq == 3) //Both available read and exit
lakshya 20:949d13045431 1678 {
lakshya 20:949d13045431 1679 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1680 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1681 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1682 return 3;
lakshya 20:949d13045431 1683 }
lakshya 20:949d13045431 1684 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 20:949d13045431 1685 {
lakshya 49:61c9f28332ba 1686 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1687 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 20:949d13045431 1688 {
lakshya 20:949d13045431 1689 //other sensor both working, off or
lakshya 20:949d13045431 1690 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1691 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1692 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 20:949d13045431 1693 wait_ms(5);
lakshya 20:949d13045431 1694 if(acq == 1)
lakshya 20:949d13045431 1695 {
lakshya 20:949d13045431 1696 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 20:949d13045431 1697 }
lakshya 20:949d13045431 1698 if(acq==2)
lakshya 20:949d13045431 1699 {
lakshya 20:949d13045431 1700 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 20:949d13045431 1701 }
lakshya 20:949d13045431 1702
Bragadeesh153 52:daa685b0e390 1703 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 20:949d13045431 1704 wait_ms(5);
lakshya 20:949d13045431 1705 init = SENSOR_INIT(); //sensor 2 init
lakshya 20:949d13045431 1706
lakshya 20:949d13045431 1707 if( init == 0)
lakshya 20:949d13045431 1708 {
lakshya 49:61c9f28332ba 1709 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1710 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1711 wait_ms(5);
lakshya 20:949d13045431 1712 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1713 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 20:949d13045431 1714 if(acq == 1)
lakshya 20:949d13045431 1715 {
lakshya 20:949d13045431 1716 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 20:949d13045431 1717 }
lakshya 20:949d13045431 1718 if(acq==2)
lakshya 20:949d13045431 1719 {
lakshya 20:949d13045431 1720 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1721 }
lakshya 20:949d13045431 1722 return acq;
lakshya 20:949d13045431 1723 }
lakshya 20:949d13045431 1724
lakshya 20:949d13045431 1725 int acq2;
lakshya 20:949d13045431 1726 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1727
lakshya 20:949d13045431 1728 if(acq2 == 3)
lakshya 20:949d13045431 1729 {
lakshya 20:949d13045431 1730 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1731 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 20:949d13045431 1732 return 3;
lakshya 20:949d13045431 1733 }
lakshya 20:949d13045431 1734
lakshya 20:949d13045431 1735 else if(acq2 == 1)
lakshya 20:949d13045431 1736 {
lakshya 20:949d13045431 1737 if(acq==2)
lakshya 20:949d13045431 1738 {
Bragadeesh153 52:daa685b0e390 1739 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1740 wait_ms(5);
lakshya 20:949d13045431 1741 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 20:949d13045431 1742 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 20:949d13045431 1743 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1744 return 3;
lakshya 20:949d13045431 1745 }
lakshya 20:949d13045431 1746 else
lakshya 20:949d13045431 1747 {
lakshya 20:949d13045431 1748 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 20:949d13045431 1749 return 1;
lakshya 20:949d13045431 1750 }
lakshya 20:949d13045431 1751 }
lakshya 20:949d13045431 1752
lakshya 20:949d13045431 1753 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 20:949d13045431 1754 {
lakshya 20:949d13045431 1755 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1756 return 2;
lakshya 20:949d13045431 1757 }
lakshya 20:949d13045431 1758 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 20:949d13045431 1759 {
lakshya 49:61c9f28332ba 1760 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1761 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1762 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 20:949d13045431 1763 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1764 wait_ms(5);
lakshya 20:949d13045431 1765 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 20:949d13045431 1766 return acq;
lakshya 20:949d13045431 1767 }
lakshya 20:949d13045431 1768
lakshya 20:949d13045431 1769 }
lakshya 20:949d13045431 1770 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1771 {
lakshya 20:949d13045431 1772 if(acq == 1)
lakshya 20:949d13045431 1773 {
lakshya 20:949d13045431 1774 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 20:949d13045431 1775 return 1;
lakshya 20:949d13045431 1776 }
lakshya 20:949d13045431 1777 if(acq==2)
lakshya 20:949d13045431 1778 {
lakshya 20:949d13045431 1779 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1780 return 2;
lakshya 20:949d13045431 1781 }
lakshya 49:61c9f28332ba 1782 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1783 return acq;
lakshya 20:949d13045431 1784
lakshya 20:949d13045431 1785 }
lakshya 20:949d13045431 1786 }
lakshya 20:949d13045431 1787 else if(acq == 0)
lakshya 20:949d13045431 1788 {
lakshya 49:61c9f28332ba 1789 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 20:949d13045431 1790 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1791 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 1792 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1793 wait_ms(5);
lakshya 20:949d13045431 1794 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1795 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 20:949d13045431 1796 {
lakshya 20:949d13045431 1797 init = SENSOR_INIT();
lakshya 20:949d13045431 1798 if( init == 0)
lakshya 20:949d13045431 1799 {
lakshya 49:61c9f28332ba 1800 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 20:949d13045431 1801 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1802 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 20:949d13045431 1803 return 0;
lakshya 20:949d13045431 1804 }
lakshya 20:949d13045431 1805
lakshya 20:949d13045431 1806 int acq2;
lakshya 20:949d13045431 1807 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1808 if(acq2 == 3)
lakshya 20:949d13045431 1809 {
lakshya 20:949d13045431 1810 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1811 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 20:949d13045431 1812 return 3;
lakshya 20:949d13045431 1813 }
lakshya 20:949d13045431 1814 else if(acq2 == 1)
lakshya 20:949d13045431 1815 {
lakshya 20:949d13045431 1816 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 20:949d13045431 1817 return 1;
lakshya 20:949d13045431 1818 }
lakshya 20:949d13045431 1819 else if(acq2 == 2)
lakshya 20:949d13045431 1820 {
lakshya 20:949d13045431 1821 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1822 return 2;
lakshya 20:949d13045431 1823 }
lakshya 20:949d13045431 1824 else if(acq2 == 0)
lakshya 20:949d13045431 1825 {
lakshya 49:61c9f28332ba 1826 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 1827 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1828 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1829 return 0;
lakshya 20:949d13045431 1830 }
lakshya 20:949d13045431 1831 }
lakshya 20:949d13045431 1832 }
lakshya 20:949d13045431 1833 }
lakshya 49:61c9f28332ba 1834
lakshya 49:61c9f28332ba 1835
lakshya 49:61c9f28332ba 1836
lakshya 49:61c9f28332ba 1837 if(( (ACS_ATS_STATUS & 0xC0) == 0x00))
lakshya 49:61c9f28332ba 1838 {
Bragadeesh153 100:54514f9d3de2 1839 ATS2_SW_ENABLE = 0;
Bragadeesh153 100:54514f9d3de2 1840 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1841 wait_ms(10);
lakshya 49:61c9f28332ba 1842 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1843 if(acq == 3)
lakshya 49:61c9f28332ba 1844 {
lakshya 49:61c9f28332ba 1845
lakshya 49:61c9f28332ba 1846 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1847
lakshya 49:61c9f28332ba 1848 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1849 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1850 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 1851 return 3;
lakshya 49:61c9f28332ba 1852 }
lakshya 49:61c9f28332ba 1853 else if((acq == 2)||(acq==1))
lakshya 49:61c9f28332ba 1854 {
lakshya 49:61c9f28332ba 1855 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 1856 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 49:61c9f28332ba 1857 {
lakshya 49:61c9f28332ba 1858 //other sensor both working, off or
lakshya 49:61c9f28332ba 1859 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 1860 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 1861
Bragadeesh153 52:daa685b0e390 1862 ATS1_SW_ENABLE = 0; //switch off sensor 1
lakshya 49:61c9f28332ba 1863 wait_ms(5);
lakshya 49:61c9f28332ba 1864 if(acq == 1)
lakshya 49:61c9f28332ba 1865 {
lakshya 49:61c9f28332ba 1866 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 49:61c9f28332ba 1867 }
lakshya 49:61c9f28332ba 1868 if(acq==2)
lakshya 49:61c9f28332ba 1869 {
lakshya 49:61c9f28332ba 1870 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 49:61c9f28332ba 1871 }
lakshya 49:61c9f28332ba 1872
lakshya 49:61c9f28332ba 1873 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 49:61c9f28332ba 1874 wait_ms(5);
lakshya 49:61c9f28332ba 1875
lakshya 49:61c9f28332ba 1876 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 1877 if( init == 0)
lakshya 49:61c9f28332ba 1878 {
lakshya 49:61c9f28332ba 1879 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1880 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1881 wait_ms(5);
Bragadeesh153 88:d9d647e3701f 1882 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1883 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 49:61c9f28332ba 1884 if(acq == 1)
lakshya 49:61c9f28332ba 1885 {
lakshya 49:61c9f28332ba 1886 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 49:61c9f28332ba 1887 }
lakshya 49:61c9f28332ba 1888 if(acq==2)
lakshya 49:61c9f28332ba 1889 {
lakshya 49:61c9f28332ba 1890 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1891 }
lakshya 49:61c9f28332ba 1892 return acq;
lakshya 49:61c9f28332ba 1893 }
lakshya 49:61c9f28332ba 1894
lakshya 49:61c9f28332ba 1895 int acq2;
lakshya 49:61c9f28332ba 1896 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1897 if(acq2 == 3)
lakshya 49:61c9f28332ba 1898 {
lakshya 49:61c9f28332ba 1899 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1900 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 49:61c9f28332ba 1901 return 3;
lakshya 49:61c9f28332ba 1902 }
lakshya 49:61c9f28332ba 1903 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1904 {
lakshya 49:61c9f28332ba 1905 if(acq==2)
lakshya 49:61c9f28332ba 1906 {
lakshya 49:61c9f28332ba 1907 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1908 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1909 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 49:61c9f28332ba 1910 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 49:61c9f28332ba 1911 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1912 return 3;
lakshya 49:61c9f28332ba 1913 }
lakshya 49:61c9f28332ba 1914 else
lakshya 49:61c9f28332ba 1915 {
lakshya 49:61c9f28332ba 1916 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 49:61c9f28332ba 1917 return 1;
lakshya 49:61c9f28332ba 1918 }
lakshya 49:61c9f28332ba 1919 }
lakshya 49:61c9f28332ba 1920
lakshya 49:61c9f28332ba 1921 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 49:61c9f28332ba 1922 {
lakshya 49:61c9f28332ba 1923 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1924 return 2;
lakshya 49:61c9f28332ba 1925 }
lakshya 49:61c9f28332ba 1926 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 49:61c9f28332ba 1927 {
lakshya 49:61c9f28332ba 1928 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1929 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1930 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 52:daa685b0e390 1931 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1932 wait_ms(5);
lakshya 49:61c9f28332ba 1933 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 49:61c9f28332ba 1934 return acq;
lakshya 49:61c9f28332ba 1935 }
lakshya 49:61c9f28332ba 1936
lakshya 49:61c9f28332ba 1937 }
lakshya 49:61c9f28332ba 1938 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 1939 {
lakshya 49:61c9f28332ba 1940 if(acq == 1)
lakshya 49:61c9f28332ba 1941 {
lakshya 49:61c9f28332ba 1942 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 49:61c9f28332ba 1943 return 1;
lakshya 49:61c9f28332ba 1944 }
lakshya 49:61c9f28332ba 1945 if(acq==2)
lakshya 49:61c9f28332ba 1946 {
lakshya 49:61c9f28332ba 1947 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1948 return 2;
lakshya 49:61c9f28332ba 1949 }
lakshya 49:61c9f28332ba 1950 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 1951 return acq;
lakshya 49:61c9f28332ba 1952
lakshya 49:61c9f28332ba 1953 }
lakshya 49:61c9f28332ba 1954 }
lakshya 49:61c9f28332ba 1955
lakshya 49:61c9f28332ba 1956 else if(acq == 0)
lakshya 49:61c9f28332ba 1957 {
lakshya 49:61c9f28332ba 1958 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1959 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1960 wait_ms(5); //Switch ON sensor 2
lakshya 49:61c9f28332ba 1961 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1962 wait_ms(5);
lakshya 49:61c9f28332ba 1963 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 1964 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 49:61c9f28332ba 1965 {
lakshya 49:61c9f28332ba 1966 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1967 if( init == 0)
lakshya 49:61c9f28332ba 1968 {
lakshya 49:61c9f28332ba 1969 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 1970 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1971 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 49:61c9f28332ba 1972 return 0;
lakshya 49:61c9f28332ba 1973 }
lakshya 49:61c9f28332ba 1974
lakshya 49:61c9f28332ba 1975 int acq2;
lakshya 49:61c9f28332ba 1976 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1977 if(acq2 == 3)
lakshya 49:61c9f28332ba 1978 {
lakshya 49:61c9f28332ba 1979 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1980 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 49:61c9f28332ba 1981 return 3;
lakshya 49:61c9f28332ba 1982 }
lakshya 49:61c9f28332ba 1983 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1984 {
lakshya 49:61c9f28332ba 1985 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 49:61c9f28332ba 1986 return 1;
lakshya 49:61c9f28332ba 1987 }
lakshya 49:61c9f28332ba 1988 else if(acq2 == 2)
lakshya 49:61c9f28332ba 1989 {
lakshya 49:61c9f28332ba 1990 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1991 return 2;
lakshya 49:61c9f28332ba 1992 }
lakshya 49:61c9f28332ba 1993 else if(acq2 == 0)
lakshya 49:61c9f28332ba 1994 {
lakshya 49:61c9f28332ba 1995 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 49:61c9f28332ba 1996 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1997
lakshya 49:61c9f28332ba 1998 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 1999 return 0;
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
lakshya 49:61c9f28332ba 2007 }
lakshya 49:61c9f28332ba 2008
lakshya 49:61c9f28332ba 2009 if(( (ACS_ATS_STATUS & 0x0C) == 0x00))
lakshya 49:61c9f28332ba 2010 {
Bragadeesh153 100:54514f9d3de2 2011 ATS1_SW_ENABLE = 0;
Bragadeesh153 100:54514f9d3de2 2012 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2013 wait_ms(10);
lakshya 49:61c9f28332ba 2014 acq = SENSOR_DATA_ACQ(); //make ATS2 on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
lakshya 49:61c9f28332ba 2015 if(acq == 3) //Both available read and exit
lakshya 49:61c9f28332ba 2016 {
lakshya 49:61c9f28332ba 2017 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 2018 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2019 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 2020 return 3;
lakshya 49:61c9f28332ba 2021 }
lakshya 49:61c9f28332ba 2022 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 49:61c9f28332ba 2023 {
lakshya 49:61c9f28332ba 2024 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 2025 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 49:61c9f28332ba 2026 {
lakshya 49:61c9f28332ba 2027 //other sensor both working, off or
lakshya 49:61c9f28332ba 2028 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 2029 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 2030 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 49:61c9f28332ba 2031 wait_ms(5);
lakshya 49:61c9f28332ba 2032 if(acq == 1)
lakshya 49:61c9f28332ba 2033 {
lakshya 49:61c9f28332ba 2034 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 49:61c9f28332ba 2035 }
lakshya 49:61c9f28332ba 2036 if(acq==2)
lakshya 49:61c9f28332ba 2037 {
lakshya 49:61c9f28332ba 2038 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 49:61c9f28332ba 2039 }
lakshya 49:61c9f28332ba 2040
Bragadeesh153 52:daa685b0e390 2041 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 49:61c9f28332ba 2042 wait_ms(5);
lakshya 49:61c9f28332ba 2043 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 2044
lakshya 49:61c9f28332ba 2045 if( init == 0)
lakshya 49:61c9f28332ba 2046 {
lakshya 49:61c9f28332ba 2047 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2048 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2049 wait_ms(5);
lakshya 49:61c9f28332ba 2050 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2051 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 49:61c9f28332ba 2052 if(acq == 1)
lakshya 49:61c9f28332ba 2053 {
lakshya 49:61c9f28332ba 2054 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 49:61c9f28332ba 2055 }
lakshya 49:61c9f28332ba 2056 if(acq==2)
lakshya 49:61c9f28332ba 2057 {
lakshya 49:61c9f28332ba 2058 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2059 }
lakshya 49:61c9f28332ba 2060 return acq;
lakshya 49:61c9f28332ba 2061 }
lakshya 49:61c9f28332ba 2062
lakshya 49:61c9f28332ba 2063 int acq2;
lakshya 49:61c9f28332ba 2064 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2065
lakshya 49:61c9f28332ba 2066 if(acq2 == 3)
lakshya 49:61c9f28332ba 2067 {
lakshya 49:61c9f28332ba 2068 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2069 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 49:61c9f28332ba 2070 return 3;
lakshya 49:61c9f28332ba 2071 }
lakshya 49:61c9f28332ba 2072
lakshya 49:61c9f28332ba 2073 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2074 {
lakshya 49:61c9f28332ba 2075 if(acq==2)
lakshya 49:61c9f28332ba 2076 {
Bragadeesh153 52:daa685b0e390 2077 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2078 wait_ms(5);
lakshya 49:61c9f28332ba 2079 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 49:61c9f28332ba 2080 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 49:61c9f28332ba 2081 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2082 return 3;
lakshya 49:61c9f28332ba 2083 }
lakshya 49:61c9f28332ba 2084 else
lakshya 49:61c9f28332ba 2085 {
lakshya 49:61c9f28332ba 2086 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 49:61c9f28332ba 2087 return 1;
lakshya 49:61c9f28332ba 2088 }
lakshya 49:61c9f28332ba 2089 }
lakshya 49:61c9f28332ba 2090
lakshya 49:61c9f28332ba 2091 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 49:61c9f28332ba 2092 {
lakshya 49:61c9f28332ba 2093 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2094 return 2;
lakshya 49:61c9f28332ba 2095 }
lakshya 49:61c9f28332ba 2096 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 49:61c9f28332ba 2097 {
lakshya 49:61c9f28332ba 2098 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2099 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2100 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 49:61c9f28332ba 2101 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2102 wait_ms(5);
lakshya 49:61c9f28332ba 2103 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 49:61c9f28332ba 2104 return acq;
lakshya 49:61c9f28332ba 2105 }
lakshya 49:61c9f28332ba 2106
lakshya 49:61c9f28332ba 2107 }
lakshya 49:61c9f28332ba 2108 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 2109 {
lakshya 49:61c9f28332ba 2110 if(acq == 1)
lakshya 49:61c9f28332ba 2111 {
lakshya 49:61c9f28332ba 2112 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 49:61c9f28332ba 2113 return 1;
lakshya 49:61c9f28332ba 2114 }
lakshya 49:61c9f28332ba 2115 if(acq==2)
lakshya 49:61c9f28332ba 2116 {
lakshya 49:61c9f28332ba 2117 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2118 return 2;
lakshya 49:61c9f28332ba 2119 }
lakshya 49:61c9f28332ba 2120 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 2121 return acq;
lakshya 49:61c9f28332ba 2122
lakshya 49:61c9f28332ba 2123 }
lakshya 49:61c9f28332ba 2124 }
lakshya 49:61c9f28332ba 2125 else if(acq == 0)
lakshya 49:61c9f28332ba 2126 {
lakshya 49:61c9f28332ba 2127 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 49:61c9f28332ba 2128 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2129 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 2130 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2131 wait_ms(5);
lakshya 49:61c9f28332ba 2132 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 2133 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 49:61c9f28332ba 2134 {
lakshya 49:61c9f28332ba 2135 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 2136 if( init == 0)
lakshya 49:61c9f28332ba 2137 {
lakshya 49:61c9f28332ba 2138 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 2139 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2140 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 49:61c9f28332ba 2141 return 0;
lakshya 49:61c9f28332ba 2142 }
lakshya 49:61c9f28332ba 2143
lakshya 49:61c9f28332ba 2144 int acq2;
lakshya 49:61c9f28332ba 2145 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2146 if(acq2 == 3)
lakshya 49:61c9f28332ba 2147 {
lakshya 49:61c9f28332ba 2148 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2149 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 49:61c9f28332ba 2150 return 3;
lakshya 49:61c9f28332ba 2151 }
lakshya 49:61c9f28332ba 2152 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2153 {
lakshya 49:61c9f28332ba 2154 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 49:61c9f28332ba 2155 return 1;
lakshya 49:61c9f28332ba 2156 }
lakshya 49:61c9f28332ba 2157 else if(acq2 == 2)
lakshya 49:61c9f28332ba 2158 {
lakshya 49:61c9f28332ba 2159 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2160 return 2;
lakshya 49:61c9f28332ba 2161 }
lakshya 49:61c9f28332ba 2162 else if(acq2 == 0)
lakshya 49:61c9f28332ba 2163 {
lakshya 49:61c9f28332ba 2164 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 2165 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2166 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 2167 return 0;
lakshya 49:61c9f28332ba 2168 }
lakshya 49:61c9f28332ba 2169 }
lakshya 49:61c9f28332ba 2170 }
lakshya 49:61c9f28332ba 2171 }
lakshya 49:61c9f28332ba 2172
lakshya 49:61c9f28332ba 2173
lakshya 49:61c9f28332ba 2174
lakshya 49:61c9f28332ba 2175
lakshya 49:61c9f28332ba 2176
lakshya 49:61c9f28332ba 2177 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2178 acs_pc.printf(" Both sensors data acq failure.Exiting.\n \r");
lakshya 20:949d13045431 2179 return 0;
sakthipriya 0:7b4c00e3912f 2180 }
sakthipriya 0:7b4c00e3912f 2181
sakthipriya 0:7b4c00e3912f 2182 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 2183 {
lakshya 20:949d13045431 2184 //// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 2185
sakthipriya 0:7b4c00e3912f 2186 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 2187 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 2188 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 2189 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 2190 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 2191 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 2192
sakthipriya 0:7b4c00e3912f 2193
lakshya 20:949d13045431 2194 //// printf("\r\r");
sakthipriya 0:7b4c00e3912f 2195
sakthipriya 0:7b4c00e3912f 2196 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 2197
sakthipriya 0:7b4c00e3912f 2198
sakthipriya 0:7b4c00e3912f 2199 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 2200
sakthipriya 0:7b4c00e3912f 2201 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 2202 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 2203 {
sakthipriya 0:7b4c00e3912f 2204 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 2205 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 2206 }
sakthipriya 0:7b4c00e3912f 2207
sakthipriya 0:7b4c00e3912f 2208 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2209 //// printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 2210 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 2211 {
lakshya 10:f93407b97750 2212 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 2213 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2214 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2215 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2216 }
lakshya 10:f93407b97750 2217 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 2218 {
lakshya 10:f93407b97750 2219 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 2220 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2221 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2222 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2223 }
lakshya 10:f93407b97750 2224 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 2225 {
lakshya 10:f93407b97750 2226 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 2227 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2228 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2229 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2230 }
lakshya 10:f93407b97750 2231 else if(l_current_x==0)
lakshya 10:f93407b97750 2232 {
lakshya 20:949d13045431 2233 //// printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 2234 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 20:949d13045431 2235 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2236 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2237 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2238 }
lakshya 10:f93407b97750 2239 else //not necessary
lakshya 10:f93407b97750 2240 {
lakshya 10:f93407b97750 2241 g_err_flag_TR_x = 1;
lakshya 20:949d13045431 2242 }
lakshya 49:61c9f28332ba 2243 acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2244
lakshya 10:f93407b97750 2245
Bragadeesh153 52:daa685b0e390 2246 //----------------------------- y-direction TR --------------------------------------------//
Bragadeesh153 52:daa685b0e390 2247
Bragadeesh153 52:daa685b0e390 2248
lakshya 10:f93407b97750 2249 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 2250
lakshya 10:f93407b97750 2251 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 2252 if (l_moment_y <0)
lakshya 10:f93407b97750 2253 {
Bragadeesh153 52:daa685b0e390 2254 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 2255 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 2256 }
lakshya 10:f93407b97750 2257
lakshya 10:f93407b97750 2258 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2259 //// printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 2260 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 2261 {
lakshya 10:f93407b97750 2262 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 2263 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2264 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2265 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2266 }
lakshya 10:f93407b97750 2267 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 2268 {
lakshya 10:f93407b97750 2269 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 2270 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2271 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2272 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2273 }
lakshya 10:f93407b97750 2274 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 2275 {
lakshya 10:f93407b97750 2276 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 2277 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2278 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2279 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 52:daa685b0e390 2280 }
lakshya 10:f93407b97750 2281 else if(l_current_y==0)
lakshya 10:f93407b97750 2282 {
lakshya 20:949d13045431 2283 //// printf("\n \r l_current_y====0");
Bragadeesh153 52:daa685b0e390 2284 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 20:949d13045431 2285 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2286 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2287 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2288 }
Bragadeesh153 52:daa685b0e390 2289 else //not necessary
lakshya 10:f93407b97750 2290 {
Bragadeesh153 52:daa685b0e390 2291 g_err_flag_TR_y = 1;
Bragadeesh153 52:daa685b0e390 2292 }
Bragadeesh153 52:daa685b0e390 2293 acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2294 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 2295
Bragadeesh153 52:daa685b0e390 2296
lakshya 20:949d13045431 2297
Bragadeesh153 52:daa685b0e390 2298
lakshya 10:f93407b97750 2299 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 2300
lakshya 20:949d13045431 2301 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 2302 if (l_moment_z <0)
lakshya 10:f93407b97750 2303 {
Bragadeesh153 52:daa685b0e390 2304 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 2305 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 2306 }
lakshya 10:f93407b97750 2307
lakshya 10:f93407b97750 2308 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2309 //// printf("current in trz is %f \r \n",l_current_z);
lakshya 20:949d13045431 2310 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 2311 {
lakshya 10:f93407b97750 2312 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 2313 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2314 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2315 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2316 }
lakshya 10:f93407b97750 2317 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 2318 {
lakshya 10:f93407b97750 2319 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 2320 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2321 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2322 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2323 }
lakshya 10:f93407b97750 2324 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 2325 {
lakshya 10:f93407b97750 2326 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 2327 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2328 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2329 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 52:daa685b0e390 2330 }
lakshya 10:f93407b97750 2331 else if(l_current_z==0)
lakshya 10:f93407b97750 2332 {
lakshya 20:949d13045431 2333 //// printf("\n \r l_current_z====0");
Bragadeesh153 52:daa685b0e390 2334 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 20:949d13045431 2335 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2336 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2337 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2338 }
Bragadeesh153 52:daa685b0e390 2339 else //not necessary
lakshya 10:f93407b97750 2340 {
Bragadeesh153 52:daa685b0e390 2341 g_err_flag_TR_z = 1;
Bragadeesh153 52:daa685b0e390 2342 }
Bragadeesh153 52:daa685b0e390 2343 acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2344
lakshya 20:949d13045431 2345 //changed
lakshya 20:949d13045431 2346 if(phase_TR_x)
lakshya 20:949d13045431 2347 ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
lakshya 20:949d13045431 2348 else
lakshya 20:949d13045431 2349 ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
lakshya 20:949d13045431 2350 if(phase_TR_y)
lakshya 20:949d13045431 2351 ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
lakshya 20:949d13045431 2352 else
lakshya 20:949d13045431 2353 ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
lakshya 20:949d13045431 2354 if(phase_TR_z)
lakshya 20:949d13045431 2355 ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
lakshya 20:949d13045431 2356 else
Bragadeesh153 59:0fc0bfafaa9f 2357 ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM3);
lakshya 20:949d13045431 2358
lakshya 10:f93407b97750 2359 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 2360
lakshya 20:949d13045431 2361 //// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 2362
lakshya 10:f93407b97750 2363 }
lakshya 20:949d13045431 2364