Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

ACS.cpp@66:a5d2b8dc6b9e, 2016-12-04 (annotated)

Committer:
lakshya
Date:
Sun Dec 04 07:58:20 2016 +0000
Revision:
66:a5d2b8dc6b9e
Parent:
63:55d32e7dcad7
Child:
88:d9d647e3701f
initial code 4th dec

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 {
lakshya 20:949d13045431 1321 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
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
lakshya 20:949d13045431 1327 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
lakshya 20:949d13045431 1328 {
lakshya 20:949d13045431 1329
lakshya 49:61c9f28332ba 1330 acs_pc.printf("error in mag.Exiting.\n \r");
lakshya 20:949d13045431 1331 mag_error = 1;
lakshya 20:949d13045431 1332 }
lakshya 20:949d13045431 1333 if( (gyro_error!=1)&&(mag_error!=1))
lakshya 20:949d13045431 1334 {
lakshya 49:61c9f28332ba 1335 acs_pc.printf("error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1336 return 0;
lakshya 20:949d13045431 1337
lakshya 20:949d13045431 1338 }
lakshya 20:949d13045431 1339 }
lakshya 20:949d13045431 1340
lakshya 20:949d13045431 1341
lakshya 49:61c9f28332ba 1342 if(((event & 1) == 1 ))
lakshya 20:949d13045431 1343 {
lakshya 49:61c9f28332ba 1344 /// acs_pc.printf("error in CPU Reset.\n \r");
lakshya 20:949d13045431 1345 return 0;
lakshya 20:949d13045431 1346
lakshya 20:949d13045431 1347 }
lakshya 49:61c9f28332ba 1348 int poll =0;
lakshya 49:61c9f28332ba 1349 poll_status =1;
lakshya 49:61c9f28332ba 1350 while((poll<15)&&(((event & 8) != 8 )||((event & 32) != 32 )))
lakshya 49:61c9f28332ba 1351 //if(((event & 8) != 8 )||((event & 32) != 32 ))
lakshya 20:949d13045431 1352 {
lakshya 49:61c9f28332ba 1353 poll++;
lakshya 49:61c9f28332ba 1354 acs_pc.printf("Data not ready waiting...POLL#%d\n \r",poll);
lakshya 20:949d13045431 1355 //POLL
lakshya 20:949d13045431 1356 wait_ms(200);
lakshya 20:949d13045431 1357
lakshya 20:949d13045431 1358 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1359
lakshya 20:949d13045431 1360 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1361 if(ack!=0)
lakshya 20:949d13045431 1362 {
lakshya 20:949d13045431 1363 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1364 if(ack!=0)
lakshya 20:949d13045431 1365 return 0;
lakshya 20:949d13045431 1366 }
lakshya 20:949d13045431 1367
lakshya 20:949d13045431 1368 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1369 if(ack!=0)
lakshya 20:949d13045431 1370 {
lakshya 20:949d13045431 1371 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1372 if(ack!=0)
lakshya 20:949d13045431 1373 return 0;
lakshya 20:949d13045431 1374 }
lakshya 20:949d13045431 1375
lakshya 20:949d13045431 1376 event = (int)status;
lakshya 49:61c9f28332ba 1377 if((event & 32) != 32 )
lakshya 20:949d13045431 1378 {
lakshya 20:949d13045431 1379
lakshya 49:61c9f28332ba 1380 acs_pc.printf("Gyro not ready..\n \r");
lakshya 20:949d13045431 1381 gyro_error = 1;
lakshya 20:949d13045431 1382
lakshya 20:949d13045431 1383 }
lakshya 49:61c9f28332ba 1384 else
lakshya 49:61c9f28332ba 1385 {
lakshya 49:61c9f28332ba 1386 gyro_error = 0;
lakshya 49:61c9f28332ba 1387 }
lakshya 20:949d13045431 1388
lakshya 49:61c9f28332ba 1389 if((event & 8) != 8 )
lakshya 20:949d13045431 1390 {
lakshya 49:61c9f28332ba 1391 acs_pc.printf("Mag not ready..\n \r");
lakshya 20:949d13045431 1392 mag_error=1;
lakshya 20:949d13045431 1393 }
lakshya 49:61c9f28332ba 1394 else
lakshya 49:61c9f28332ba 1395 {
lakshya 49:61c9f28332ba 1396 mag_error=0;
lakshya 49:61c9f28332ba 1397 }
lakshya 20:949d13045431 1398
lakshya 20:949d13045431 1399
lakshya 20:949d13045431 1400 }
lakshya 49:61c9f28332ba 1401
lakshya 49:61c9f28332ba 1402 if(poll!=15)
lakshya 49:61c9f28332ba 1403 {
lakshya 49:61c9f28332ba 1404 poll_status = 0;
lakshya 49:61c9f28332ba 1405 }
lakshya 20:949d13045431 1406
lakshya 20:949d13045431 1407
lakshya 20:949d13045431 1408 }
lakshya 20:949d13045431 1409
lakshya 49:61c9f28332ba 1410 if(((mag_error !=1)&&(gyro_error!=1))&&(poll_status!=0))
lakshya 20:949d13045431 1411 {
lakshya 49:61c9f28332ba 1412 acs_pc.printf("Error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1413 return 0;
lakshya 20:949d13045431 1414 }
lakshya 20:949d13045431 1415
lakshya 49:61c9f28332ba 1416 if(((mag_error ==1)&&(gyro_error==1))&&(poll_status!=0))
lakshya 20:949d13045431 1417 {
lakshya 49:61c9f28332ba 1418 acs_pc.printf("Error in both gyro and mag.Exiting.\n \r");
lakshya 20:949d13045431 1419 return 0;
lakshya 20:949d13045431 1420 }
lakshya 20:949d13045431 1421
lakshya 20:949d13045431 1422 }
lakshya 20:949d13045431 1423
lakshya 20:949d13045431 1424
lakshya 20:949d13045431 1425 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1426 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1427 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1428 if(ack != 0)
lakshya 20:949d13045431 1429 {
lakshya 20:949d13045431 1430 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1431 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1432 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1433 if(ack !=1)
lakshya 20:949d13045431 1434 return 0;
lakshya 20:949d13045431 1435
lakshya 20:949d13045431 1436 }
lakshya 20:949d13045431 1437
lakshya 20:949d13045431 1438
lakshya 49:61c9f28332ba 1439 // acs_pc.printf("\nGyro Values:\n");
lakshya 20:949d13045431 1440 if (gyro_error!=1)
lakshya 20:949d13045431 1441 {
lakshya 20:949d13045431 1442 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1443 //concatenating gyro LSB and MSB to get 16 bit signed data values
lakshya 20:949d13045431 1444 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 1445 gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
lakshya 20:949d13045431 1446 gyro_data[i]=gyro_data[i]/senstivity_gyro;
lakshya 20:949d13045431 1447 actual_data.AngularSpeed_actual[i] = gyro_data[i];
lakshya 20:949d13045431 1448 }
lakshya 20:949d13045431 1449 }
lakshya 20:949d13045431 1450
lakshya 20:949d13045431 1451 if(mag_error!=1)
lakshya 20:949d13045431 1452 {
lakshya 20:949d13045431 1453 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1454 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
lakshya 20:949d13045431 1455 actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
lakshya 20:949d13045431 1456
lakshya 20:949d13045431 1457 mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
lakshya 20:949d13045431 1458 mag_data[i]=mag_data[i]/senstivity_mag;
lakshya 20:949d13045431 1459 actual_data.Bvalue_actual[i] = mag_data[i];
lakshya 20:949d13045431 1460 }
lakshya 20:949d13045431 1461 }
lakshya 20:949d13045431 1462
lakshya 20:949d13045431 1463
lakshya 20:949d13045431 1464 if(mag_error == 1)
lakshya 20:949d13045431 1465 {
lakshya 20:949d13045431 1466
lakshya 49:61c9f28332ba 1467 acs_pc.printf("Gyro only successful.\n \r");
lakshya 20:949d13045431 1468 return 1;
lakshya 20:949d13045431 1469 }
lakshya 20:949d13045431 1470 if(gyro_error == 1)
lakshya 49:61c9f28332ba 1471 {
lakshya 49:61c9f28332ba 1472 acs_pc.printf("Mag only successful.\n \r");
lakshya 20:949d13045431 1473 return 2;
lakshya 20:949d13045431 1474 }
lakshya 17:fc782f7548c6 1475
lakshya 49:61c9f28332ba 1476 //acs_pc.printf("Reading data success.\n \r");
lakshya 20:949d13045431 1477 return 3;
lakshya 20:949d13045431 1478 }
lakshya 20:949d13045431 1479
lakshya 20:949d13045431 1480
lakshya 20:949d13045431 1481 int FCTN_ATS_DATA_ACQ()
lakshya 20:949d13045431 1482 {
lakshya 20:949d13045431 1483 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1484 actual_data.AngularSpeed_actual[i] = 0;
lakshya 20:949d13045431 1485 actual_data.Bvalue_actual[i] = 0;
lakshya 20:949d13045431 1486 }
lakshya 20:949d13045431 1487
lakshya 20:949d13045431 1488 int acq;
lakshya 20:949d13045431 1489 int init;
lakshya 20:949d13045431 1490
lakshya 49:61c9f28332ba 1491 //// acs_pc.printf("DATA_ACQ called \n \r");
lakshya 49:61c9f28332ba 1492 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1493
lakshya 20:949d13045431 1494
lakshya 20:949d13045431 1495 if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
lakshya 20:949d13045431 1496 {
lakshya 49:61c9f28332ba 1497 timer_SENSOR_DATA_ACQ.start();
lakshya 20:949d13045431 1498 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1499 timer_SENSOR_DATA_ACQ.stop();
lakshya 49:61c9f28332ba 1500 //pc.pritnf("\n\r timer_SENSOR_DATA_ACQ is %f",timer_SENSOR_DATA_ACQ.read());
lakshya 49:61c9f28332ba 1501
lakshya 49:61c9f28332ba 1502
lakshya 20:949d13045431 1503 if(acq == 3)
lakshya 20:949d13045431 1504 {
lakshya 20:949d13045431 1505
lakshya 20:949d13045431 1506 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1507
lakshya 20:949d13045431 1508 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1509 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1510 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1511 return 3;
lakshya 20:949d13045431 1512 }
lakshya 20:949d13045431 1513 else if((acq == 2)||(acq==1))
lakshya 20:949d13045431 1514 {
lakshya 49:61c9f28332ba 1515 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1516 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 20:949d13045431 1517 {
lakshya 20:949d13045431 1518 //other sensor both working, off or
lakshya 20:949d13045431 1519 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1520 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1521
Bragadeesh153 52:daa685b0e390 1522 ATS1_SW_ENABLE = 01; //switch off sensor 1
lakshya 20:949d13045431 1523 wait_ms(5);
lakshya 20:949d13045431 1524 if(acq == 1)
lakshya 20:949d13045431 1525 {
lakshya 20:949d13045431 1526 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 20:949d13045431 1527 }
lakshya 20:949d13045431 1528 if(acq==2)
lakshya 20:949d13045431 1529 {
lakshya 20:949d13045431 1530 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 20:949d13045431 1531 }
lakshya 20:949d13045431 1532
lakshya 20:949d13045431 1533 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 20:949d13045431 1534 wait_ms(5);
lakshya 49:61c9f28332ba 1535
lakshya 49:61c9f28332ba 1536 timer_SENSOR_INIT.reset();
lakshya 49:61c9f28332ba 1537 timer_SENSOR_INIT.start();
lakshya 49:61c9f28332ba 1538 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1539 timer_SENSOR_INIT.stop();
lakshya 49:61c9f28332ba 1540 //sensor 2 init
lakshya 20:949d13045431 1541 if( init == 0)
lakshya 20:949d13045431 1542 {
lakshya 49:61c9f28332ba 1543 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1544 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1545 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1546 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1547 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 20:949d13045431 1548 if(acq == 1)
lakshya 20:949d13045431 1549 {
lakshya 20:949d13045431 1550 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 20:949d13045431 1551 }
lakshya 20:949d13045431 1552 if(acq==2)
lakshya 20:949d13045431 1553 {
lakshya 20:949d13045431 1554 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1555 }
lakshya 20:949d13045431 1556 return acq;
lakshya 20:949d13045431 1557 }
lakshya 20:949d13045431 1558
lakshya 20:949d13045431 1559 int acq2;
lakshya 20:949d13045431 1560 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1561 if(acq2 == 3)
lakshya 20:949d13045431 1562 {
lakshya 20:949d13045431 1563 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1564 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 20:949d13045431 1565 return 3;
lakshya 20:949d13045431 1566 }
lakshya 20:949d13045431 1567 else if(acq2 == 1)
lakshya 20:949d13045431 1568 {
lakshya 20:949d13045431 1569 if(acq==2)
lakshya 20:949d13045431 1570 {
lakshya 20:949d13045431 1571 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1572 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1573 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 20:949d13045431 1574 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1575 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1576 return 3;
lakshya 20:949d13045431 1577 }
lakshya 20:949d13045431 1578 else
lakshya 20:949d13045431 1579 {
lakshya 20:949d13045431 1580 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 20:949d13045431 1581 return 1;
lakshya 20:949d13045431 1582 }
lakshya 20:949d13045431 1583 }
lakshya 20:949d13045431 1584
lakshya 20:949d13045431 1585 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 20:949d13045431 1586 {
lakshya 20:949d13045431 1587 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1588 return 2;
lakshya 20:949d13045431 1589 }
lakshya 20:949d13045431 1590 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 20:949d13045431 1591 {
lakshya 49:61c9f28332ba 1592 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1593 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1594 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 52:daa685b0e390 1595 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1596 wait_ms(5);
lakshya 20:949d13045431 1597 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 20:949d13045431 1598 return acq;
lakshya 20:949d13045431 1599 }
lakshya 20:949d13045431 1600
lakshya 20:949d13045431 1601 }
lakshya 20:949d13045431 1602 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1603 {
lakshya 20:949d13045431 1604 if(acq == 1)
lakshya 20:949d13045431 1605 {
lakshya 20:949d13045431 1606 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 20:949d13045431 1607 return 1;
lakshya 20:949d13045431 1608 }
lakshya 20:949d13045431 1609 if(acq==2)
lakshya 20:949d13045431 1610 {
lakshya 20:949d13045431 1611 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1612 return 2;
lakshya 20:949d13045431 1613 }
lakshya 49:61c9f28332ba 1614 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1615 return acq;
lakshya 20:949d13045431 1616
lakshya 20:949d13045431 1617 }
sakthipriya 0:7b4c00e3912f 1618 }
lakshya 20:949d13045431 1619
lakshya 20:949d13045431 1620 else if(acq == 0)
lakshya 20:949d13045431 1621 {
lakshya 49:61c9f28332ba 1622 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1623 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1624 wait_ms(5); //Switch ON sensor 2
lakshya 20:949d13045431 1625 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1626 wait_ms(5);
lakshya 20:949d13045431 1627 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1628 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 20:949d13045431 1629 {
lakshya 20:949d13045431 1630 init = SENSOR_INIT();
lakshya 20:949d13045431 1631 if( init == 0)
lakshya 20:949d13045431 1632 {
lakshya 49:61c9f28332ba 1633 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 20:949d13045431 1634 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1635 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 20:949d13045431 1636 return 0;
lakshya 20:949d13045431 1637 }
lakshya 20:949d13045431 1638
lakshya 20:949d13045431 1639 int acq2;
lakshya 20:949d13045431 1640 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1641 if(acq2 == 3)
lakshya 20:949d13045431 1642 {
lakshya 20:949d13045431 1643 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1644 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 20:949d13045431 1645 return 3;
lakshya 20:949d13045431 1646 }
lakshya 20:949d13045431 1647 else if(acq2 == 1)
lakshya 20:949d13045431 1648 {
lakshya 20:949d13045431 1649 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 20:949d13045431 1650 return 1;
lakshya 20:949d13045431 1651 }
lakshya 20:949d13045431 1652 else if(acq2 == 2)
lakshya 20:949d13045431 1653 {
lakshya 20:949d13045431 1654 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1655 return 2;
lakshya 20:949d13045431 1656 }
lakshya 20:949d13045431 1657 else if(acq2 == 0)
lakshya 20:949d13045431 1658 {
lakshya 49:61c9f28332ba 1659 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 20:949d13045431 1660 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1661
lakshya 20:949d13045431 1662 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1663 return 0;
lakshya 20:949d13045431 1664 }
lakshya 20:949d13045431 1665
lakshya 20:949d13045431 1666 }
lakshya 20:949d13045431 1667
sakthipriya 0:7b4c00e3912f 1668 }
lakshya 20:949d13045431 1669
lakshya 20:949d13045431 1670
sakthipriya 0:7b4c00e3912f 1671 }
lakshya 20:949d13045431 1672
lakshya 20:949d13045431 1673 if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
lakshya 20:949d13045431 1674 {
lakshya 20:949d13045431 1675 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 1676 if(acq == 3) //Both available read and exit
lakshya 20:949d13045431 1677 {
lakshya 20:949d13045431 1678 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1679 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1680 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1681 return 3;
lakshya 20:949d13045431 1682 }
lakshya 20:949d13045431 1683 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 20:949d13045431 1684 {
lakshya 49:61c9f28332ba 1685 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1686 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 20:949d13045431 1687 {
lakshya 20:949d13045431 1688 //other sensor both working, off or
lakshya 20:949d13045431 1689 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1690 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1691 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 20:949d13045431 1692 wait_ms(5);
lakshya 20:949d13045431 1693 if(acq == 1)
lakshya 20:949d13045431 1694 {
lakshya 20:949d13045431 1695 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 20:949d13045431 1696 }
lakshya 20:949d13045431 1697 if(acq==2)
lakshya 20:949d13045431 1698 {
lakshya 20:949d13045431 1699 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 20:949d13045431 1700 }
lakshya 20:949d13045431 1701
Bragadeesh153 52:daa685b0e390 1702 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 20:949d13045431 1703 wait_ms(5);
lakshya 20:949d13045431 1704 init = SENSOR_INIT(); //sensor 2 init
lakshya 20:949d13045431 1705
lakshya 20:949d13045431 1706 if( init == 0)
lakshya 20:949d13045431 1707 {
lakshya 49:61c9f28332ba 1708 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1709 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1710 wait_ms(5);
lakshya 20:949d13045431 1711 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1712 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 20:949d13045431 1713 if(acq == 1)
lakshya 20:949d13045431 1714 {
lakshya 20:949d13045431 1715 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 20:949d13045431 1716 }
lakshya 20:949d13045431 1717 if(acq==2)
lakshya 20:949d13045431 1718 {
lakshya 20:949d13045431 1719 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1720 }
lakshya 20:949d13045431 1721 return acq;
lakshya 20:949d13045431 1722 }
lakshya 20:949d13045431 1723
lakshya 20:949d13045431 1724 int acq2;
lakshya 20:949d13045431 1725 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1726
lakshya 20:949d13045431 1727 if(acq2 == 3)
lakshya 20:949d13045431 1728 {
lakshya 20:949d13045431 1729 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1730 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 20:949d13045431 1731 return 3;
lakshya 20:949d13045431 1732 }
lakshya 20:949d13045431 1733
lakshya 20:949d13045431 1734 else if(acq2 == 1)
lakshya 20:949d13045431 1735 {
lakshya 20:949d13045431 1736 if(acq==2)
lakshya 20:949d13045431 1737 {
Bragadeesh153 52:daa685b0e390 1738 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1739 wait_ms(5);
lakshya 20:949d13045431 1740 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 20:949d13045431 1741 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 20:949d13045431 1742 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1743 return 3;
lakshya 20:949d13045431 1744 }
lakshya 20:949d13045431 1745 else
lakshya 20:949d13045431 1746 {
lakshya 20:949d13045431 1747 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 20:949d13045431 1748 return 1;
lakshya 20:949d13045431 1749 }
lakshya 20:949d13045431 1750 }
lakshya 20:949d13045431 1751
lakshya 20:949d13045431 1752 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 20:949d13045431 1753 {
lakshya 20:949d13045431 1754 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1755 return 2;
lakshya 20:949d13045431 1756 }
lakshya 20:949d13045431 1757 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 20:949d13045431 1758 {
lakshya 49:61c9f28332ba 1759 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 1760 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1761 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 20:949d13045431 1762 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1763 wait_ms(5);
lakshya 20:949d13045431 1764 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 20:949d13045431 1765 return acq;
lakshya 20:949d13045431 1766 }
lakshya 20:949d13045431 1767
lakshya 20:949d13045431 1768 }
lakshya 20:949d13045431 1769 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1770 {
lakshya 20:949d13045431 1771 if(acq == 1)
lakshya 20:949d13045431 1772 {
lakshya 20:949d13045431 1773 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 20:949d13045431 1774 return 1;
lakshya 20:949d13045431 1775 }
lakshya 20:949d13045431 1776 if(acq==2)
lakshya 20:949d13045431 1777 {
lakshya 20:949d13045431 1778 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1779 return 2;
lakshya 20:949d13045431 1780 }
lakshya 49:61c9f28332ba 1781 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1782 return acq;
lakshya 20:949d13045431 1783
lakshya 20:949d13045431 1784 }
lakshya 20:949d13045431 1785 }
lakshya 20:949d13045431 1786 else if(acq == 0)
lakshya 20:949d13045431 1787 {
lakshya 49:61c9f28332ba 1788 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 20:949d13045431 1789 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1790 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 1791 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1792 wait_ms(5);
lakshya 20:949d13045431 1793 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1794 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 20:949d13045431 1795 {
lakshya 20:949d13045431 1796 init = SENSOR_INIT();
lakshya 20:949d13045431 1797 if( init == 0)
lakshya 20:949d13045431 1798 {
lakshya 49:61c9f28332ba 1799 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 20:949d13045431 1800 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1801 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 20:949d13045431 1802 return 0;
lakshya 20:949d13045431 1803 }
lakshya 20:949d13045431 1804
lakshya 20:949d13045431 1805 int acq2;
lakshya 20:949d13045431 1806 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1807 if(acq2 == 3)
lakshya 20:949d13045431 1808 {
lakshya 20:949d13045431 1809 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1810 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 20:949d13045431 1811 return 3;
lakshya 20:949d13045431 1812 }
lakshya 20:949d13045431 1813 else if(acq2 == 1)
lakshya 20:949d13045431 1814 {
lakshya 20:949d13045431 1815 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 20:949d13045431 1816 return 1;
lakshya 20:949d13045431 1817 }
lakshya 20:949d13045431 1818 else if(acq2 == 2)
lakshya 20:949d13045431 1819 {
lakshya 20:949d13045431 1820 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1821 return 2;
lakshya 20:949d13045431 1822 }
lakshya 20:949d13045431 1823 else if(acq2 == 0)
lakshya 20:949d13045431 1824 {
lakshya 49:61c9f28332ba 1825 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 1826 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1827 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1828 return 0;
lakshya 20:949d13045431 1829 }
lakshya 20:949d13045431 1830 }
lakshya 20:949d13045431 1831 }
lakshya 20:949d13045431 1832 }
lakshya 49:61c9f28332ba 1833
lakshya 49:61c9f28332ba 1834
lakshya 49:61c9f28332ba 1835
lakshya 49:61c9f28332ba 1836 if(( (ACS_ATS_STATUS & 0xC0) == 0x00))
lakshya 49:61c9f28332ba 1837 {
lakshya 49:61c9f28332ba 1838
lakshya 49:61c9f28332ba 1839 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1840 wait_ms(10);
lakshya 49:61c9f28332ba 1841 acq = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1842 if(acq == 3)
lakshya 49:61c9f28332ba 1843 {
lakshya 49:61c9f28332ba 1844
lakshya 49:61c9f28332ba 1845 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 1846
lakshya 49:61c9f28332ba 1847 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 49:61c9f28332ba 1848 //// acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 1849 //// acs_pc.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 1850 return 3;
lakshya 49:61c9f28332ba 1851 }
lakshya 49:61c9f28332ba 1852 else if((acq == 2)||(acq==1))
lakshya 49:61c9f28332ba 1853 {
lakshya 49:61c9f28332ba 1854 acs_pc.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 1855 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 49:61c9f28332ba 1856 {
lakshya 49:61c9f28332ba 1857 //other sensor both working, off or
lakshya 49:61c9f28332ba 1858 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 1859 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 1860
Bragadeesh153 52:daa685b0e390 1861 ATS1_SW_ENABLE = 0; //switch off sensor 1
lakshya 49:61c9f28332ba 1862 wait_ms(5);
lakshya 49:61c9f28332ba 1863 if(acq == 1)
lakshya 49:61c9f28332ba 1864 {
lakshya 49:61c9f28332ba 1865 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 49:61c9f28332ba 1866 }
lakshya 49:61c9f28332ba 1867 if(acq==2)
lakshya 49:61c9f28332ba 1868 {
lakshya 49:61c9f28332ba 1869 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 49:61c9f28332ba 1870 }
lakshya 49:61c9f28332ba 1871
lakshya 49:61c9f28332ba 1872 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 49:61c9f28332ba 1873 wait_ms(5);
lakshya 49:61c9f28332ba 1874
lakshya 49:61c9f28332ba 1875 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 1876 if( init == 0)
lakshya 49:61c9f28332ba 1877 {
lakshya 49:61c9f28332ba 1878 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1879 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1880 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1881 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1882 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 49:61c9f28332ba 1883 if(acq == 1)
lakshya 49:61c9f28332ba 1884 {
lakshya 49:61c9f28332ba 1885 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 49:61c9f28332ba 1886 }
lakshya 49:61c9f28332ba 1887 if(acq==2)
lakshya 49:61c9f28332ba 1888 {
lakshya 49:61c9f28332ba 1889 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1890 }
lakshya 49:61c9f28332ba 1891 return acq;
lakshya 49:61c9f28332ba 1892 }
lakshya 49:61c9f28332ba 1893
lakshya 49:61c9f28332ba 1894 int acq2;
lakshya 49:61c9f28332ba 1895 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1896 if(acq2 == 3)
lakshya 49:61c9f28332ba 1897 {
lakshya 49:61c9f28332ba 1898 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1899 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 49:61c9f28332ba 1900 return 3;
lakshya 49:61c9f28332ba 1901 }
lakshya 49:61c9f28332ba 1902 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1903 {
lakshya 49:61c9f28332ba 1904 if(acq==2)
lakshya 49:61c9f28332ba 1905 {
lakshya 49:61c9f28332ba 1906 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1907 wait_ms(5);
Bragadeesh153 52:daa685b0e390 1908 ATS1_SW_ENABLE = 1; //Sensor 2 gyro only,sensor 1 mag only
lakshya 49:61c9f28332ba 1909 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 49:61c9f28332ba 1910 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1911 return 3;
lakshya 49:61c9f28332ba 1912 }
lakshya 49:61c9f28332ba 1913 else
lakshya 49:61c9f28332ba 1914 {
lakshya 49:61c9f28332ba 1915 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 49:61c9f28332ba 1916 return 1;
lakshya 49:61c9f28332ba 1917 }
lakshya 49:61c9f28332ba 1918 }
lakshya 49:61c9f28332ba 1919
lakshya 49:61c9f28332ba 1920 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 49:61c9f28332ba 1921 {
lakshya 49:61c9f28332ba 1922 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1923 return 2;
lakshya 49:61c9f28332ba 1924 }
lakshya 49:61c9f28332ba 1925 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 49:61c9f28332ba 1926 {
lakshya 49:61c9f28332ba 1927 acs_pc.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 49:61c9f28332ba 1928 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1929 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 52:daa685b0e390 1930 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1931 wait_ms(5);
lakshya 49:61c9f28332ba 1932 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 49:61c9f28332ba 1933 return acq;
lakshya 49:61c9f28332ba 1934 }
lakshya 49:61c9f28332ba 1935
lakshya 49:61c9f28332ba 1936 }
lakshya 49:61c9f28332ba 1937 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 1938 {
lakshya 49:61c9f28332ba 1939 if(acq == 1)
lakshya 49:61c9f28332ba 1940 {
lakshya 49:61c9f28332ba 1941 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 49:61c9f28332ba 1942 return 1;
lakshya 49:61c9f28332ba 1943 }
lakshya 49:61c9f28332ba 1944 if(acq==2)
lakshya 49:61c9f28332ba 1945 {
lakshya 49:61c9f28332ba 1946 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 1947 return 2;
lakshya 49:61c9f28332ba 1948 }
lakshya 49:61c9f28332ba 1949 acs_pc.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 1950 return acq;
lakshya 49:61c9f28332ba 1951
lakshya 49:61c9f28332ba 1952 }
lakshya 49:61c9f28332ba 1953 }
lakshya 49:61c9f28332ba 1954
lakshya 49:61c9f28332ba 1955 else if(acq == 0)
lakshya 49:61c9f28332ba 1956 {
lakshya 49:61c9f28332ba 1957 acs_pc.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 52:daa685b0e390 1958 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1959 wait_ms(5); //Switch ON sensor 2
lakshya 49:61c9f28332ba 1960 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 1961 wait_ms(5);
lakshya 49:61c9f28332ba 1962 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 1963 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 49:61c9f28332ba 1964 {
lakshya 49:61c9f28332ba 1965 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 1966 if( init == 0)
lakshya 49:61c9f28332ba 1967 {
lakshya 49:61c9f28332ba 1968 acs_pc.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 1969 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1970 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 49:61c9f28332ba 1971 return 0;
lakshya 49:61c9f28332ba 1972 }
lakshya 49:61c9f28332ba 1973
lakshya 49:61c9f28332ba 1974 int acq2;
lakshya 49:61c9f28332ba 1975 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 1976 if(acq2 == 3)
lakshya 49:61c9f28332ba 1977 {
lakshya 49:61c9f28332ba 1978 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 1979 acs_pc.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 49:61c9f28332ba 1980 return 3;
lakshya 49:61c9f28332ba 1981 }
lakshya 49:61c9f28332ba 1982 else if(acq2 == 1)
lakshya 49:61c9f28332ba 1983 {
lakshya 49:61c9f28332ba 1984 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 49:61c9f28332ba 1985 return 1;
lakshya 49:61c9f28332ba 1986 }
lakshya 49:61c9f28332ba 1987 else if(acq2 == 2)
lakshya 49:61c9f28332ba 1988 {
lakshya 49:61c9f28332ba 1989 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 1990 return 2;
lakshya 49:61c9f28332ba 1991 }
lakshya 49:61c9f28332ba 1992 else if(acq2 == 0)
lakshya 49:61c9f28332ba 1993 {
lakshya 49:61c9f28332ba 1994 acs_pc.printf(" Sensor 2 data acq failure..\n \r");
lakshya 49:61c9f28332ba 1995 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 1996
lakshya 49:61c9f28332ba 1997 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 1998 return 0;
lakshya 49:61c9f28332ba 1999 }
lakshya 49:61c9f28332ba 2000
lakshya 49:61c9f28332ba 2001 }
lakshya 49:61c9f28332ba 2002
lakshya 49:61c9f28332ba 2003 }
lakshya 49:61c9f28332ba 2004
lakshya 49:61c9f28332ba 2005
lakshya 49:61c9f28332ba 2006 }
lakshya 49:61c9f28332ba 2007
lakshya 49:61c9f28332ba 2008 if(( (ACS_ATS_STATUS & 0x0C) == 0x00))
lakshya 49:61c9f28332ba 2009 {
lakshya 49:61c9f28332ba 2010 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2011 wait_ms(10);
lakshya 49:61c9f28332ba 2012 acq = SENSOR_DATA_ACQ(); //make ATS2 on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
lakshya 49:61c9f28332ba 2013 if(acq == 3) //Both available read and exit
lakshya 49:61c9f28332ba 2014 {
lakshya 49:61c9f28332ba 2015 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 49:61c9f28332ba 2016 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2017 acs_pc.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 49:61c9f28332ba 2018 return 3;
lakshya 49:61c9f28332ba 2019 }
lakshya 49:61c9f28332ba 2020 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 49:61c9f28332ba 2021 {
lakshya 49:61c9f28332ba 2022 acs_pc.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 49:61c9f28332ba 2023 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 49:61c9f28332ba 2024 {
lakshya 49:61c9f28332ba 2025 //other sensor both working, off or
lakshya 49:61c9f28332ba 2026 //other sensor gyro working, this sensor not working , off
lakshya 49:61c9f28332ba 2027 //other sensor mag working, this sensor not working,off
lakshya 49:61c9f28332ba 2028 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 49:61c9f28332ba 2029 wait_ms(5);
lakshya 49:61c9f28332ba 2030 if(acq == 1)
lakshya 49:61c9f28332ba 2031 {
lakshya 49:61c9f28332ba 2032 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 49:61c9f28332ba 2033 }
lakshya 49:61c9f28332ba 2034 if(acq==2)
lakshya 49:61c9f28332ba 2035 {
lakshya 49:61c9f28332ba 2036 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 49:61c9f28332ba 2037 }
lakshya 49:61c9f28332ba 2038
Bragadeesh153 52:daa685b0e390 2039 ATS1_SW_ENABLE = 1; //switch on sensor 1
lakshya 49:61c9f28332ba 2040 wait_ms(5);
lakshya 49:61c9f28332ba 2041 init = SENSOR_INIT(); //sensor 2 init
lakshya 49:61c9f28332ba 2042
lakshya 49:61c9f28332ba 2043 if( init == 0)
lakshya 49:61c9f28332ba 2044 {
lakshya 49:61c9f28332ba 2045 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2046 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2047 wait_ms(5);
lakshya 49:61c9f28332ba 2048 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2049 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 49:61c9f28332ba 2050 if(acq == 1)
lakshya 49:61c9f28332ba 2051 {
lakshya 49:61c9f28332ba 2052 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 49:61c9f28332ba 2053 }
lakshya 49:61c9f28332ba 2054 if(acq==2)
lakshya 49:61c9f28332ba 2055 {
lakshya 49:61c9f28332ba 2056 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2057 }
lakshya 49:61c9f28332ba 2058 return acq;
lakshya 49:61c9f28332ba 2059 }
lakshya 49:61c9f28332ba 2060
lakshya 49:61c9f28332ba 2061 int acq2;
lakshya 49:61c9f28332ba 2062 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2063
lakshya 49:61c9f28332ba 2064 if(acq2 == 3)
lakshya 49:61c9f28332ba 2065 {
lakshya 49:61c9f28332ba 2066 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2067 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 49:61c9f28332ba 2068 return 3;
lakshya 49:61c9f28332ba 2069 }
lakshya 49:61c9f28332ba 2070
lakshya 49:61c9f28332ba 2071 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2072 {
lakshya 49:61c9f28332ba 2073 if(acq==2)
lakshya 49:61c9f28332ba 2074 {
Bragadeesh153 52:daa685b0e390 2075 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2076 wait_ms(5);
lakshya 49:61c9f28332ba 2077 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 49:61c9f28332ba 2078 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 49:61c9f28332ba 2079 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2080 return 3;
lakshya 49:61c9f28332ba 2081 }
lakshya 49:61c9f28332ba 2082 else
lakshya 49:61c9f28332ba 2083 {
lakshya 49:61c9f28332ba 2084 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 49:61c9f28332ba 2085 return 1;
lakshya 49:61c9f28332ba 2086 }
lakshya 49:61c9f28332ba 2087 }
lakshya 49:61c9f28332ba 2088
lakshya 49:61c9f28332ba 2089 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 49:61c9f28332ba 2090 {
lakshya 49:61c9f28332ba 2091 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2092 return 2;
lakshya 49:61c9f28332ba 2093 }
lakshya 49:61c9f28332ba 2094 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 49:61c9f28332ba 2095 {
lakshya 49:61c9f28332ba 2096 acs_pc.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 52:daa685b0e390 2097 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2098 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 49:61c9f28332ba 2099 ATS2_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2100 wait_ms(5);
lakshya 49:61c9f28332ba 2101 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 49:61c9f28332ba 2102 return acq;
lakshya 49:61c9f28332ba 2103 }
lakshya 49:61c9f28332ba 2104
lakshya 49:61c9f28332ba 2105 }
lakshya 49:61c9f28332ba 2106 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 49:61c9f28332ba 2107 {
lakshya 49:61c9f28332ba 2108 if(acq == 1)
lakshya 49:61c9f28332ba 2109 {
lakshya 49:61c9f28332ba 2110 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 49:61c9f28332ba 2111 return 1;
lakshya 49:61c9f28332ba 2112 }
lakshya 49:61c9f28332ba 2113 if(acq==2)
lakshya 49:61c9f28332ba 2114 {
lakshya 49:61c9f28332ba 2115 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 49:61c9f28332ba 2116 return 2;
lakshya 49:61c9f28332ba 2117 }
lakshya 49:61c9f28332ba 2118 acs_pc.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 49:61c9f28332ba 2119 return acq;
lakshya 49:61c9f28332ba 2120
lakshya 49:61c9f28332ba 2121 }
lakshya 49:61c9f28332ba 2122 }
lakshya 49:61c9f28332ba 2123 else if(acq == 0)
lakshya 49:61c9f28332ba 2124 {
lakshya 49:61c9f28332ba 2125 acs_pc.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 49:61c9f28332ba 2126 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2127 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 52:daa685b0e390 2128 ATS1_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2129 wait_ms(5);
lakshya 49:61c9f28332ba 2130 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 49:61c9f28332ba 2131 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 49:61c9f28332ba 2132 {
lakshya 49:61c9f28332ba 2133 init = SENSOR_INIT();
lakshya 49:61c9f28332ba 2134 if( init == 0)
lakshya 49:61c9f28332ba 2135 {
lakshya 49:61c9f28332ba 2136 acs_pc.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 49:61c9f28332ba 2137 ATS2_SW_ENABLE = 1;
lakshya 49:61c9f28332ba 2138 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 49:61c9f28332ba 2139 return 0;
lakshya 49:61c9f28332ba 2140 }
lakshya 49:61c9f28332ba 2141
lakshya 49:61c9f28332ba 2142 int acq2;
lakshya 49:61c9f28332ba 2143 acq2 = SENSOR_DATA_ACQ();
lakshya 49:61c9f28332ba 2144 if(acq2 == 3)
lakshya 49:61c9f28332ba 2145 {
lakshya 49:61c9f28332ba 2146 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 49:61c9f28332ba 2147 acs_pc.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 49:61c9f28332ba 2148 return 3;
lakshya 49:61c9f28332ba 2149 }
lakshya 49:61c9f28332ba 2150 else if(acq2 == 1)
lakshya 49:61c9f28332ba 2151 {
lakshya 49:61c9f28332ba 2152 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 49:61c9f28332ba 2153 return 1;
lakshya 49:61c9f28332ba 2154 }
lakshya 49:61c9f28332ba 2155 else if(acq2 == 2)
lakshya 49:61c9f28332ba 2156 {
lakshya 49:61c9f28332ba 2157 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 49:61c9f28332ba 2158 return 2;
lakshya 49:61c9f28332ba 2159 }
lakshya 49:61c9f28332ba 2160 else if(acq2 == 0)
lakshya 49:61c9f28332ba 2161 {
lakshya 49:61c9f28332ba 2162 acs_pc.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 52:daa685b0e390 2163 ATS1_SW_ENABLE = 0;
lakshya 49:61c9f28332ba 2164 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 49:61c9f28332ba 2165 return 0;
lakshya 49:61c9f28332ba 2166 }
lakshya 49:61c9f28332ba 2167 }
lakshya 49:61c9f28332ba 2168 }
lakshya 49:61c9f28332ba 2169 }
lakshya 49:61c9f28332ba 2170
lakshya 49:61c9f28332ba 2171
lakshya 49:61c9f28332ba 2172
lakshya 49:61c9f28332ba 2173
lakshya 49:61c9f28332ba 2174
lakshya 49:61c9f28332ba 2175 acs_pc.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 49:61c9f28332ba 2176 acs_pc.printf(" Both sensors data acq failure.Exiting.\n \r");
lakshya 20:949d13045431 2177 return 0;
sakthipriya 0:7b4c00e3912f 2178 }
sakthipriya 0:7b4c00e3912f 2179
sakthipriya 0:7b4c00e3912f 2180 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 2181 {
lakshya 20:949d13045431 2182 //// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 2183
sakthipriya 0:7b4c00e3912f 2184 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 2185 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 2186 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 2187 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 2188 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 2189 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 2190
sakthipriya 0:7b4c00e3912f 2191
lakshya 20:949d13045431 2192 //// printf("\r\r");
sakthipriya 0:7b4c00e3912f 2193
sakthipriya 0:7b4c00e3912f 2194 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 2195
sakthipriya 0:7b4c00e3912f 2196
sakthipriya 0:7b4c00e3912f 2197 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 2198
sakthipriya 0:7b4c00e3912f 2199 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 2200 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 2201 {
sakthipriya 0:7b4c00e3912f 2202 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 2203 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 2204 }
sakthipriya 0:7b4c00e3912f 2205
sakthipriya 0:7b4c00e3912f 2206 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2207 //// printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 2208 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 2209 {
lakshya 10:f93407b97750 2210 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 2211 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2212 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2213 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2214 }
lakshya 10:f93407b97750 2215 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 2216 {
lakshya 10:f93407b97750 2217 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 2218 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2219 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2220 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2221 }
lakshya 10:f93407b97750 2222 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 2223 {
lakshya 10:f93407b97750 2224 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 2225 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2226 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2227 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2228 }
lakshya 10:f93407b97750 2229 else if(l_current_x==0)
lakshya 10:f93407b97750 2230 {
lakshya 20:949d13045431 2231 //// printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 2232 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 20:949d13045431 2233 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2234 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 2235 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 2236 }
lakshya 10:f93407b97750 2237 else //not necessary
lakshya 10:f93407b97750 2238 {
lakshya 10:f93407b97750 2239 g_err_flag_TR_x = 1;
lakshya 20:949d13045431 2240 }
lakshya 49:61c9f28332ba 2241 acs_pc.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 2242
lakshya 10:f93407b97750 2243
Bragadeesh153 52:daa685b0e390 2244 //----------------------------- y-direction TR --------------------------------------------//
Bragadeesh153 52:daa685b0e390 2245
Bragadeesh153 52:daa685b0e390 2246
lakshya 10:f93407b97750 2247 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 2248
lakshya 10:f93407b97750 2249 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 2250 if (l_moment_y <0)
lakshya 10:f93407b97750 2251 {
Bragadeesh153 52:daa685b0e390 2252 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 2253 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 2254 }
lakshya 10:f93407b97750 2255
lakshya 10:f93407b97750 2256 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2257 //// printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 2258 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 2259 {
lakshya 10:f93407b97750 2260 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 2261 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2262 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2263 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2264 }
lakshya 10:f93407b97750 2265 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 2266 {
lakshya 10:f93407b97750 2267 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 2268 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2269 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2270 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2271 }
lakshya 10:f93407b97750 2272 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 2273 {
lakshya 10:f93407b97750 2274 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 2275 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2276 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2277 PWM2 = l_duty_cycle_y/100 ;
Bragadeesh153 52:daa685b0e390 2278 }
lakshya 10:f93407b97750 2279 else if(l_current_y==0)
lakshya 10:f93407b97750 2280 {
lakshya 20:949d13045431 2281 //// printf("\n \r l_current_y====0");
Bragadeesh153 52:daa685b0e390 2282 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 20:949d13045431 2283 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2284 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 2285 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 2286 }
Bragadeesh153 52:daa685b0e390 2287 else //not necessary
lakshya 10:f93407b97750 2288 {
Bragadeesh153 52:daa685b0e390 2289 g_err_flag_TR_y = 1;
Bragadeesh153 52:daa685b0e390 2290 }
Bragadeesh153 52:daa685b0e390 2291 acs_pc.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 2292 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 2293
Bragadeesh153 52:daa685b0e390 2294
lakshya 20:949d13045431 2295
Bragadeesh153 52:daa685b0e390 2296
lakshya 10:f93407b97750 2297 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 2298
lakshya 20:949d13045431 2299 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 2300 if (l_moment_z <0)
lakshya 10:f93407b97750 2301 {
Bragadeesh153 52:daa685b0e390 2302 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 2303 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 2304 }
lakshya 10:f93407b97750 2305
lakshya 10:f93407b97750 2306 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 2307 //// printf("current in trz is %f \r \n",l_current_z);
lakshya 20:949d13045431 2308 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 2309 {
lakshya 10:f93407b97750 2310 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 2311 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2312 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2313 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2314 }
lakshya 10:f93407b97750 2315 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 2316 {
lakshya 10:f93407b97750 2317 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 2318 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2319 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2320 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2321 }
lakshya 10:f93407b97750 2322 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 2323 {
lakshya 10:f93407b97750 2324 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 2325 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2326 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2327 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 52:daa685b0e390 2328 }
lakshya 10:f93407b97750 2329 else if(l_current_z==0)
lakshya 10:f93407b97750 2330 {
lakshya 20:949d13045431 2331 //// printf("\n \r l_current_z====0");
Bragadeesh153 52:daa685b0e390 2332 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 20:949d13045431 2333 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2334 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 2335 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 2336 }
Bragadeesh153 52:daa685b0e390 2337 else //not necessary
lakshya 10:f93407b97750 2338 {
Bragadeesh153 52:daa685b0e390 2339 g_err_flag_TR_z = 1;
Bragadeesh153 52:daa685b0e390 2340 }
Bragadeesh153 52:daa685b0e390 2341 acs_pc.printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 2342
lakshya 20:949d13045431 2343 //changed
lakshya 20:949d13045431 2344 if(phase_TR_x)
lakshya 20:949d13045431 2345 ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
lakshya 20:949d13045431 2346 else
lakshya 20:949d13045431 2347 ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
lakshya 20:949d13045431 2348 if(phase_TR_y)
lakshya 20:949d13045431 2349 ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
lakshya 20:949d13045431 2350 else
lakshya 20:949d13045431 2351 ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
lakshya 20:949d13045431 2352 if(phase_TR_z)
lakshya 20:949d13045431 2353 ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
lakshya 20:949d13045431 2354 else
Bragadeesh153 59:0fc0bfafaa9f 2355 ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM3);
lakshya 20:949d13045431 2356
lakshya 10:f93407b97750 2357 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 2358
lakshya 20:949d13045431 2359 //// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 2360
lakshya 10:f93407b97750 2361 }
lakshya 20:949d13045431 2362