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@58:c4203e162d12, 2016-09-04 (annotated)

Committer:
lakshya
Date:
Sun Sep 04 18:05:31 2016 +0000
Revision:
58:c4203e162d12
Parent:
53:459b71b1861c
Child:
59:0fc0bfafaa9f
counters done, short bcn populated, acs values received,tmp values corrected, bcn tmp received; 4thsep

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