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@49:61c9f28332ba, 2016-07-14 (annotated)

Committer:
lakshya
Date:
Thu Jul 14 23:04:26 2016 +0000
Revision:
49:61c9f28332ba
Parent:
39:670133e7ffd8
Child:
52:daa685b0e390
bcn last I2C + longbcn data testesd; 15 th july

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