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@59:0fc0bfafaa9f, 2016-09-04 (annotated)

Committer:
Bragadeesh153
Date:
Sun Sep 04 19:06:02 2016 +0000
Revision:
59:0fc0bfafaa9f
Parent:
58:c4203e162d12
Child:
60:bd1498f03319
FINAL CODE FOR QM , BAE HK almost fully verified

Who changed what in which revision?

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