Team Fox / Mbed 2 deprecated BAE_ACS_FULLYDONE_153

FINAL ACS TO BE USED FOR TESTING. COMMISSIONING, ACS MAIN, DATA ACQ ALL DONE.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_FULL_Flowchart_BAE by Team Fox

ACS.cpp@16:cc77770d787f, 2016-06-03 (annotated)

Committer:
Bragadeesh153
Date:
Fri Jun 03 13:53:55 2016 +0000
Revision:
16:cc77770d787f
Parent:
15:e09aaaccf134
Child:
17:1e1955f3db75
ACS Data_Acq updated. Tested and working.

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>
sakthipriya 0:7b4c00e3912f 5
sakthipriya 0:7b4c00e3912f 6 #include "pni.h" //pni header file
sakthipriya 0:7b4c00e3912f 7 #include "pin_config.h"
sakthipriya 0:7b4c00e3912f 8 #include "ACS.h"
sakthipriya 6:036d08b62785 9 #include "EPS.h"
sakthipriya 0:7b4c00e3912f 10
sakthipriya 0:7b4c00e3912f 11 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 12 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 13 extern uint32_t BAE_ENABLE;
Bragadeesh153 13:fb7facaf308b 14 extern uint8_t ACS_INIT_STATUS;
Bragadeesh153 13:fb7facaf308b 15 extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153 13:fb7facaf308b 16 extern uint8_t ACS_ATS_STATUS;
Bragadeesh153 13:fb7facaf308b 17 extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153 13:fb7facaf308b 18 extern uint8_t ACS_STATUS;
sakthipriya 0:7b4c00e3912f 19
Bragadeesh153 16:cc77770d787f 20 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
Bragadeesh153 16:cc77770d787f 21 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
Bragadeesh153 16:cc77770d787f 22
Bragadeesh153 13:fb7facaf308b 23 extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153 13:fb7facaf308b 24 extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153 13:fb7facaf308b 25 extern uint8_t ACS_STATE;
sakthipriya 0:7b4c00e3912f 26
sakthipriya 0:7b4c00e3912f 27 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya 0:7b4c00e3912f 28 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya 0:7b4c00e3912f 29 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 30
sakthipriya 0:7b4c00e3912f 31 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 32 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 33 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 34
sakthipriya 0:7b4c00e3912f 35 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 36 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 37 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39 extern float data[6];
sakthipriya 6:036d08b62785 40 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 41
sakthipriya 0:7b4c00e3912f 42
lakshya 10:f93407b97750 43 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
lakshya 10:f93407b97750 44 //DigitalOut gpo2(PTC16); // enable of att sens switch
sakthipriya 0:7b4c00e3912f 45
sakthipriya 0:7b4c00e3912f 46
sakthipriya 0:7b4c00e3912f 47 Serial pc_acs(USBTX,USBRX); //for usb communication
lakshya 10:f93407b97750 48 //CONTROL_ALGO
lakshya 10:f93407b97750 49
lakshya 10:f93407b97750 50 float moment[3]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 51 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
lakshya 10:f93407b97750 52 int flag_firsttime=1, controlmode, alarmmode=0;
lakshya 10:f93407b97750 53
lakshya 10:f93407b97750 54
lakshya 10:f93407b97750 55
lakshya 10:f93407b97750 56 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode);
lakshya 10:f93407b97750 57 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
lakshya 10:f93407b97750 58 float max_array(float arr[3]);
sakthipriya 0:7b4c00e3912f 59 void inverse(float mat[3][3],float inv[3][3]);
lakshya 10:f93407b97750 60
lakshya 10:f93407b97750 61 //CONTROLALGO PARAMETERS
lakshya 10:f93407b97750 62
lakshya 10:f93407b97750 63
sakthipriya 0:7b4c00e3912f 64 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 65 {
Bragadeesh153 16:cc77770d787f 66
lakshya 10:f93407b97750 67 float b1[3];
lakshya 10:f93407b97750 68 float omega1[3];
lakshya 10:f93407b97750 69 b1[0] = b[0]/1000000.0;
lakshya 10:f93407b97750 70 b1[1] = b[1]/1000000.0;
Bragadeesh153 16:cc77770d787f 71 b1[2] = b[2]/1000000.0;
lakshya 10:f93407b97750 72
lakshya 10:f93407b97750 73 omega1[0] = omega[0]*3.14159/180;
lakshya 10:f93407b97750 74 omega1[1] = omega[1]*3.14159/180;
lakshya 10:f93407b97750 75 omega1[2] = omega[2]*3.14159/180;
lakshya 10:f93407b97750 76 controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
sakthipriya 0:7b4c00e3912f 77
lakshya 10:f93407b97750 78 }
lakshya 10:f93407b97750 79 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
lakshya 10:f93407b97750 80 {
lakshya 10:f93407b97750 81 float db1[3]; // Unit: Tesla/Second
lakshya 10:f93407b97750 82 float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
lakshya 10:f93407b97750 83 float MmntMax=1.1; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 84 float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
lakshya 10:f93407b97750 85 float normalising_fact;
lakshya 10:f93407b97750 86 float b1_copy[3], omega1_copy[3], db1_copy[3];
lakshya 10:f93407b97750 87 int i;
lakshya 10:f93407b97750 88 if(flag_firsttime==1)
lakshya 10:f93407b97750 89 {
lakshya 10:f93407b97750 90 for(i=0;i<3;i++)
lakshya 10:f93407b97750 91 {
lakshya 10:f93407b97750 92 db1[i]=0; // Unit: Tesla/Second
lakshya 10:f93407b97750 93 }
lakshya 10:f93407b97750 94 flag_firsttime=0;
lakshya 10:f93407b97750 95 }
sakthipriya 0:7b4c00e3912f 96 else
sakthipriya 0:7b4c00e3912f 97 {
sakthipriya 0:7b4c00e3912f 98 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 99 {
lakshya 10:f93407b97750 100 db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya 0:7b4c00e3912f 101 }
lakshya 10:f93407b97750 102 }
lakshya 10:f93407b97750 103
lakshya 10:f93407b97750 104 if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
sakthipriya 0:7b4c00e3912f 105 {
lakshya 10:f93407b97750 106 alarmmode=0;
sakthipriya 0:7b4c00e3912f 107 }
lakshya 10:f93407b97750 108 else if(max_array(omega1)>OmegaMax && alarmmode==0)
sakthipriya 0:7b4c00e3912f 109 {
lakshya 10:f93407b97750 110 alarmmode=1;
sakthipriya 0:7b4c00e3912f 111 }
lakshya 10:f93407b97750 112
lakshya 10:f93407b97750 113 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 114 {
lakshya 10:f93407b97750 115 b1_copy[i]=b1[i];
lakshya 10:f93407b97750 116 db1_copy[i]=db1[i];
lakshya 10:f93407b97750 117 omega1_copy[i]=omega1[i];
sakthipriya 0:7b4c00e3912f 118 }
lakshya 10:f93407b97750 119
lakshya 10:f93407b97750 120 if(alarmmode==0)
lakshya 10:f93407b97750 121 {
lakshya 10:f93407b97750 122 controlmode=0;
lakshya 10:f93407b97750 123 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 124 for (i=0;i<3;i++)
lakshya 10:f93407b97750 125 {
lakshya 10:f93407b97750 126 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 127 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 128 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 129 }
lakshya 10:f93407b97750 130 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 131 {
lakshya 10:f93407b97750 132 controlmode=1;
lakshya 10:f93407b97750 133 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 134 for (i=0;i<3;i++)
lakshya 10:f93407b97750 135 {
lakshya 10:f93407b97750 136 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 137 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 138 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 139 }
lakshya 10:f93407b97750 140 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 141 {
lakshya 10:f93407b97750 142 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 143 for(i=0;i<3;i++)
lakshya 10:f93407b97750 144 {
lakshya 10:f93407b97750 145 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 146 }
lakshya 10:f93407b97750 147 }
lakshya 10:f93407b97750 148 }
lakshya 10:f93407b97750 149 }
lakshya 10:f93407b97750 150 else
lakshya 10:f93407b97750 151 {
lakshya 10:f93407b97750 152 controlmode=1;
lakshya 10:f93407b97750 153 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
lakshya 10:f93407b97750 154 for (i=0;i<3;i++)
lakshya 10:f93407b97750 155 {
lakshya 10:f93407b97750 156 b1[i]=b1_copy[i];
lakshya 10:f93407b97750 157 db1[i]=db1_copy[i];
lakshya 10:f93407b97750 158 omega1[i]=omega1_copy[i];
lakshya 10:f93407b97750 159 }
lakshya 10:f93407b97750 160 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 161 {
lakshya 10:f93407b97750 162 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 163 for(i=0;i<3;i++)
lakshya 10:f93407b97750 164 {
lakshya 10:f93407b97750 165 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 166 }
lakshya 10:f93407b97750 167 }
lakshya 10:f93407b97750 168
lakshya 10:f93407b97750 169 }
lakshya 10:f93407b97750 170 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 171 {
lakshya 10:f93407b97750 172 b_old[i]=b1[i];
sakthipriya 0:7b4c00e3912f 173 }
sakthipriya 0:7b4c00e3912f 174 }
lakshya 10:f93407b97750 175
sakthipriya 0:7b4c00e3912f 176 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 177 {
sakthipriya 0:7b4c00e3912f 178 int i,j;
sakthipriya 0:7b4c00e3912f 179 float det=0;
sakthipriya 0:7b4c00e3912f 180 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 181 {
sakthipriya 0:7b4c00e3912f 182 for(j=0;j<3;j++)
lakshya 10:f93407b97750 183 {
sakthipriya 0:7b4c00e3912f 184 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 10:f93407b97750 185 }
sakthipriya 0:7b4c00e3912f 186 }
sakthipriya 0:7b4c00e3912f 187 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 188 for(i=0;i<3;i++)
lakshya 10:f93407b97750 189 {
sakthipriya 0:7b4c00e3912f 190 for(j=0;j<3;j++)
lakshya 10:f93407b97750 191 {
sakthipriya 0:7b4c00e3912f 192 inv[i][j]/=det;
lakshya 10:f93407b97750 193 }
sakthipriya 0:7b4c00e3912f 194 }
sakthipriya 0:7b4c00e3912f 195 }
sakthipriya 0:7b4c00e3912f 196
lakshya 10:f93407b97750 197 float max_array(float arr[3])
lakshya 10:f93407b97750 198 {
lakshya 10:f93407b97750 199 int i;
lakshya 10:f93407b97750 200 float temp_max=fabs(arr[0]);
lakshya 10:f93407b97750 201 for(i=1;i<3;i++)
lakshya 10:f93407b97750 202 {
lakshya 10:f93407b97750 203 if(fabs(arr[i])>temp_max)
lakshya 10:f93407b97750 204 {
lakshya 10:f93407b97750 205 temp_max=fabs(arr[i]);
lakshya 10:f93407b97750 206 }
lakshya 10:f93407b97750 207 }
lakshya 10:f93407b97750 208 return temp_max;
lakshya 10:f93407b97750 209 }
lakshya 10:f93407b97750 210
lakshya 10:f93407b97750 211
lakshya 10:f93407b97750 212 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
lakshya 10:f93407b97750 213 {
lakshya 10:f93407b97750 214 float bb[3]={0,0,0};
lakshya 10:f93407b97750 215 float d[3]={0,0,0};
lakshya 10:f93407b97750 216 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 10:f93407b97750 217 float den=0,den2;
lakshya 10:f93407b97750 218 float bcopy[3];
lakshya 10:f93407b97750 219 int i, j;//temporary variables
lakshya 10:f93407b97750 220 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya 10:f93407b97750 221 float invJm[3][3];
lakshya 10:f93407b97750 222 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000;
lakshya 10:f93407b97750 223 int infflag; // Flag variable to check if the moment value is infinity or NaN
lakshya 10:f93407b97750 224
lakshya 10:f93407b97750 225 if(controlmode1==0)
lakshya 10:f93407b97750 226 {
lakshya 10:f93407b97750 227 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
lakshya 10:f93407b97750 228 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
lakshya 10:f93407b97750 229 for(i=0;i<3;i++)
lakshya 10:f93407b97750 230 {
lakshya 10:f93407b97750 231 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
lakshya 10:f93407b97750 232 }
lakshya 10:f93407b97750 233 for(i=0;i<3;i++)
lakshya 10:f93407b97750 234 {
lakshya 10:f93407b97750 235 b[i]/=den; // Mormalized b. Hence no unit.
lakshya 10:f93407b97750 236 }
lakshya 10:f93407b97750 237 if(b[2]>0.9 || b[2]<-0.9)
lakshya 10:f93407b97750 238 {
lakshya 10:f93407b97750 239 kz=kz2;
lakshya 10:f93407b97750 240 kmu=kmu2;
lakshya 10:f93407b97750 241 gamma=gamma2;
lakshya 10:f93407b97750 242 }
lakshya 10:f93407b97750 243 for(i=0;i<2;i++)
lakshya 10:f93407b97750 244 {
lakshya 10:f93407b97750 245 Mu[i]=b[i];
lakshya 10:f93407b97750 246 v[i]=-kmu*Mu[i];
lakshya 10:f93407b97750 247 dv[i]=-kmu*db[i];
lakshya 10:f93407b97750 248 z[i]=db[i]-v[i];
lakshya 10:f93407b97750 249 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
lakshya 10:f93407b97750 250 }
lakshya 10:f93407b97750 251 inverse(Jm,invJm);
lakshya 10:f93407b97750 252 for(i=0;i<3;i++)
lakshya 10:f93407b97750 253 {
lakshya 10:f93407b97750 254 for(j=0;j<3;j++)
lakshya 10:f93407b97750 255 {
lakshya 10:f93407b97750 256 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
lakshya 10:f93407b97750 257 }
lakshya 10:f93407b97750 258 }
lakshya 10:f93407b97750 259 for(i=0;i<3;i++)
lakshya 10:f93407b97750 260 {
lakshya 10:f93407b97750 261 for(j=0;j<3;j++)
lakshya 10:f93407b97750 262 {
lakshya 10:f93407b97750 263 d[i]+=bb[j]*invJm[i][j];
lakshya 10:f93407b97750 264 }
lakshya 10:f93407b97750 265 }
lakshya 10:f93407b97750 266 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
lakshya 10:f93407b97750 267 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
lakshya 10:f93407b97750 268 bb[0]=0;
lakshya 10:f93407b97750 269 for(i=0;i<3;i++)
lakshya 10:f93407b97750 270 {
lakshya 10:f93407b97750 271 d[i]=invJm[2][i];
lakshya 10:f93407b97750 272 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
lakshya 10:f93407b97750 273 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
lakshya 10:f93407b97750 274 invJm[0][i]=b[i];
lakshya 10:f93407b97750 275 }
lakshya 10:f93407b97750 276 inverse(invJm,Jm);
lakshya 10:f93407b97750 277 for(i=0;i<3;i++)
lakshya 10:f93407b97750 278 {
lakshya 10:f93407b97750 279 for(j=0;j<3;j++)
lakshya 10:f93407b97750 280 {
lakshya 10:f93407b97750 281 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
lakshya 10:f93407b97750 282 }
lakshya 10:f93407b97750 283 }
lakshya 10:f93407b97750 284 for(i=0;i<3;i++)
lakshya 10:f93407b97750 285 {
lakshya 10:f93407b97750 286 bcopy[i]=b[i]*den;
lakshya 10:f93407b97750 287 }
lakshya 10:f93407b97750 288 for(i=0;i<3;i++)
lakshya 10:f93407b97750 289 {
lakshya 10:f93407b97750 290 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
lakshya 10:f93407b97750 291 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 292 }
lakshya 10:f93407b97750 293 infflag=0;
lakshya 10:f93407b97750 294 for (i=0; i<3 && infflag==0; i++)
lakshya 10:f93407b97750 295 {
lakshya 10:f93407b97750 296 if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
lakshya 10:f93407b97750 297 infflag=1;
lakshya 10:f93407b97750 298 }
lakshya 10:f93407b97750 299 if (infflag==1)
lakshya 10:f93407b97750 300 {
lakshya 10:f93407b97750 301 for (i=0; i<3; i++)
lakshya 10:f93407b97750 302 Mmnt[i]=2*MmntMax;
lakshya 10:f93407b97750 303 }
lakshya 10:f93407b97750 304
lakshya 10:f93407b97750 305 }
lakshya 10:f93407b97750 306 else if(controlmode1==1)
lakshya 10:f93407b97750 307 {
lakshya 10:f93407b97750 308 for(i=0;i<3;i++)
lakshya 10:f93407b97750 309 {
lakshya 10:f93407b97750 310 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 311 }
lakshya 10:f93407b97750 312 }
lakshya 10:f93407b97750 313 for(i=0;i<3;i++)
lakshya 10:f93407b97750 314 {
lakshya 10:f93407b97750 315 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 316 }
lakshya 10:f93407b97750 317 }
sakthipriya 0:7b4c00e3912f 318
sakthipriya 0:7b4c00e3912f 319 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 320
Bragadeesh153 16:cc77770d787f 321 int FCTN_ACS_INIT(); //initialization of registers happens
Bragadeesh153 16:cc77770d787f 322 int SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 323 int FCTN_ATS_DATA_ACQ(); //data is obtained
Bragadeesh153 16:cc77770d787f 324 int SENSOR_DATA_ACQ();
sakthipriya 0:7b4c00e3912f 325 void T_OUT(); //timeout function to stop infinite loop
Bragadeesh153 16:cc77770d787f 326
Bragadeesh153 16:cc77770d787f 327 void CONFIG_UPLOAD();
sakthipriya 0:7b4c00e3912f 328 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 329 int toFlag;
sakthipriya 0:7b4c00e3912f 330
sakthipriya 0:7b4c00e3912f 331 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 332 void T_OUT()
sakthipriya 0:7b4c00e3912f 333 {
sakthipriya 0:7b4c00e3912f 334 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 335 }
sakthipriya 0:7b4c00e3912f 336
sakthipriya 0:7b4c00e3912f 337
sakthipriya 0:7b4c00e3912f 338 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 339 char cmd[2];
sakthipriya 0:7b4c00e3912f 340 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 341 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 342 char store,status;
sakthipriya 0:7b4c00e3912f 343 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 344 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 345 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 346 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 347 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 348
Bragadeesh153 16:cc77770d787f 349 void CONFIG_UPLOAD()
sakthipriya 0:7b4c00e3912f 350 {
sakthipriya 0:7b4c00e3912f 351 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 352 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 353 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
Bragadeesh153 16:cc77770d787f 354 wait_ms(600);
Bragadeesh153 16:cc77770d787f 355
Bragadeesh153 16:cc77770d787f 356 //Verify magic number
Bragadeesh153 16:cc77770d787f 357
Bragadeesh153 16:cc77770d787f 358 cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
Bragadeesh153 16:cc77770d787f 359 cmd[1]=BIT_HOST_UPLD_ENB;
Bragadeesh153 16:cc77770d787f 360 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 361 wait_ms(100);
Bragadeesh153 16:cc77770d787f 362
Bragadeesh153 16:cc77770d787f 363 cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
Bragadeesh153 16:cc77770d787f 364 cmd[1]=0x0000;
Bragadeesh153 16:cc77770d787f 365 i2c.write(SLAVE_ADDR,cmd,3);
Bragadeesh153 16:cc77770d787f 366 wait_ms(100);
Bragadeesh153 16:cc77770d787f 367
Bragadeesh153 16:cc77770d787f 368
Bragadeesh153 16:cc77770d787f 369
Bragadeesh153 16:cc77770d787f 370
Bragadeesh153 16:cc77770d787f 371 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
Bragadeesh153 16:cc77770d787f 372 cmd[1]=0x00;
Bragadeesh153 16:cc77770d787f 373 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 374 wait_ms(100);
Bragadeesh153 16:cc77770d787f 375
Bragadeesh153 16:cc77770d787f 376
Bragadeesh153 16:cc77770d787f 377
Bragadeesh153 16:cc77770d787f 378 }
Bragadeesh153 16:cc77770d787f 379
Bragadeesh153 16:cc77770d787f 380 int SENSOR_INIT()
Bragadeesh153 16:cc77770d787f 381 {
Bragadeesh153 16:cc77770d787f 382
Bragadeesh153 16:cc77770d787f 383 pc_acs.printf("Entered sensor init\n \r");
Bragadeesh153 16:cc77770d787f 384 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 385 cmd[1]=BIT_RESREQ;
Bragadeesh153 16:cc77770d787f 386 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
Bragadeesh153 16:cc77770d787f 387 wait_ms(800); //waiting for loading configuration file stored in EEPROM
sakthipriya 0:7b4c00e3912f 388 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 389 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 390 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 391 wait_ms(100);
Bragadeesh153 16:cc77770d787f 392 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
Bragadeesh153 16:cc77770d787f 393
sakthipriya 0:7b4c00e3912f 394 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 395 switch((int)store) {
sakthipriya 0:7b4c00e3912f 396 case(3): {
Bragadeesh153 16:cc77770d787f 397 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 398 cmd[1]=BIT_RESREQ;
Bragadeesh153 16:cc77770d787f 399 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 400 wait_ms(600);
sakthipriya 0:7b4c00e3912f 401 break;
sakthipriya 0:7b4c00e3912f 402 }
sakthipriya 0:7b4c00e3912f 403 case(11): {
sakthipriya 0:7b4c00e3912f 404 break;
sakthipriya 0:7b4c00e3912f 405 }
sakthipriya 0:7b4c00e3912f 406 default: {
sakthipriya 0:7b4c00e3912f 407 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 408 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 409 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 410 wait_ms(600);
Bragadeesh153 16:cc77770d787f 411
sakthipriya 0:7b4c00e3912f 412 }
sakthipriya 0:7b4c00e3912f 413 }
Bragadeesh153 16:cc77770d787f 414 cmd[0]=SENTRALSTATUS;
Bragadeesh153 16:cc77770d787f 415 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 416 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 417 wait_ms(100);
Bragadeesh153 16:cc77770d787f 418 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
Bragadeesh153 16:cc77770d787f 419
Bragadeesh153 16:cc77770d787f 420 int manual=0;
Bragadeesh153 16:cc77770d787f 421 if((int)store != 11)
Bragadeesh153 16:cc77770d787f 422 {
Bragadeesh153 16:cc77770d787f 423
Bragadeesh153 16:cc77770d787f 424 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 425 cmd[1]=BIT_RESREQ;
Bragadeesh153 16:cc77770d787f 426 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 427 wait_ms(600);
Bragadeesh153 16:cc77770d787f 428
Bragadeesh153 16:cc77770d787f 429 //manually upload
Bragadeesh153 16:cc77770d787f 430
Bragadeesh153 16:cc77770d787f 431 if(manual == 0)
Bragadeesh153 16:cc77770d787f 432 {
Bragadeesh153 16:cc77770d787f 433 return 0;
Bragadeesh153 16:cc77770d787f 434 }
Bragadeesh153 16:cc77770d787f 435
Bragadeesh153 16:cc77770d787f 436 }
Bragadeesh153 16:cc77770d787f 437
Bragadeesh153 16:cc77770d787f 438 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
Bragadeesh153 16:cc77770d787f 439 cmd[1]=BIT_RUN_ENB;
Bragadeesh153 16:cc77770d787f 440 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 441 wait_ms(100);
Bragadeesh153 16:cc77770d787f 442
Bragadeesh153 16:cc77770d787f 443 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
Bragadeesh153 16:cc77770d787f 444 cmd[1]=BIT_MAGODR;
Bragadeesh153 16:cc77770d787f 445 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 446 wait_ms(100);
Bragadeesh153 16:cc77770d787f 447 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
Bragadeesh153 16:cc77770d787f 448 cmd[1]=BIT_GYROODR;
Bragadeesh153 16:cc77770d787f 449 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 450 wait_ms(100);
Bragadeesh153 16:cc77770d787f 451 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
Bragadeesh153 16:cc77770d787f 452 cmd[1]=0x00;
Bragadeesh153 16:cc77770d787f 453 wait_ms(100);
Bragadeesh153 16:cc77770d787f 454
Bragadeesh153 16:cc77770d787f 455 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 456 wait_ms(100);
Bragadeesh153 16:cc77770d787f 457 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
Bragadeesh153 16:cc77770d787f 458 cmd[1]=0x00;
Bragadeesh153 16:cc77770d787f 459 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 460 wait_ms(100);
Bragadeesh153 16:cc77770d787f 461 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
Bragadeesh153 16:cc77770d787f 462 cmd[1]=BIT_EVT_ENB;
Bragadeesh153 16:cc77770d787f 463 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 464 wait_ms(100);
Bragadeesh153 16:cc77770d787f 465 pc_acs.printf("Exited sensor init successfully\n \r");
Bragadeesh153 16:cc77770d787f 466 return 1;
Bragadeesh153 16:cc77770d787f 467
Bragadeesh153 16:cc77770d787f 468
Bragadeesh153 16:cc77770d787f 469 }
Bragadeesh153 16:cc77770d787f 470 int FCTN_ACS_INIT()
Bragadeesh153 16:cc77770d787f 471 {
Bragadeesh153 16:cc77770d787f 472 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
Bragadeesh153 16:cc77770d787f 473 if( (ATS1_SW_ENABLE!= 0 )&&(ATS2_SW_ENABLE!= 0 ) )
Bragadeesh153 16:cc77770d787f 474 {
Bragadeesh153 16:cc77770d787f 475 ATS2_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 476 ATS1_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 477 wait_ms(5);
Bragadeesh153 16:cc77770d787f 478 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 16:cc77770d787f 479 }
Bragadeesh153 16:cc77770d787f 480
Bragadeesh153 16:cc77770d787f 481 int working=0;
Bragadeesh153 16:cc77770d787f 482
Bragadeesh153 16:cc77770d787f 483 pc_acs.printf("Attitude sensor init called \n \r");
Bragadeesh153 16:cc77770d787f 484 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 485 pc_acs.printf("ATS Status & 0xC0 is %x\n\n \r",(int)(ACS_ATS_STATUS&0xC0));
Bragadeesh153 16:cc77770d787f 486 pc_acs.printf("Sensor 1 condition is %d \n\n \r",(int)(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)));
Bragadeesh153 16:cc77770d787f 487
Bragadeesh153 16:cc77770d787f 488
Bragadeesh153 16:cc77770d787f 489 if(( (ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80))
Bragadeesh153 16:cc77770d787f 490 {
Bragadeesh153 16:cc77770d787f 491
Bragadeesh153 16:cc77770d787f 492 pc_acs.printf("Sensor 1 marked working \n \r");
Bragadeesh153 16:cc77770d787f 493 working = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 494 if(working ==1)
Bragadeesh153 16:cc77770d787f 495 {
Bragadeesh153 16:cc77770d787f 496 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 16:cc77770d787f 497 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 498 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
Bragadeesh153 16:cc77770d787f 499 ACS_INIT_STATUS = 0;
Bragadeesh153 16:cc77770d787f 500 return 1;
Bragadeesh153 16:cc77770d787f 501 }
Bragadeesh153 16:cc77770d787f 502
Bragadeesh153 16:cc77770d787f 503
Bragadeesh153 16:cc77770d787f 504
Bragadeesh153 16:cc77770d787f 505 pc_acs.printf("Sensor 1 not working.Powering off.\n \r");
Bragadeesh153 16:cc77770d787f 506 ATS1_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 507 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
Bragadeesh153 16:cc77770d787f 508
Bragadeesh153 16:cc77770d787f 509 }
Bragadeesh153 16:cc77770d787f 510
Bragadeesh153 16:cc77770d787f 511 pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
Bragadeesh153 16:cc77770d787f 512
Bragadeesh153 16:cc77770d787f 513 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08))
Bragadeesh153 16:cc77770d787f 514 {
Bragadeesh153 16:cc77770d787f 515
Bragadeesh153 16:cc77770d787f 516
Bragadeesh153 16:cc77770d787f 517 ATS2_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 518 wait_ms(5);
Bragadeesh153 16:cc77770d787f 519 working = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 520 if(working ==1)
Bragadeesh153 16:cc77770d787f 521 {
Bragadeesh153 16:cc77770d787f 522 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 523 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r");
Bragadeesh153 16:cc77770d787f 524 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 16:cc77770d787f 525 ACS_INIT_STATUS = 0;
Bragadeesh153 16:cc77770d787f 526 return 2;
Bragadeesh153 16:cc77770d787f 527 }
Bragadeesh153 16:cc77770d787f 528
Bragadeesh153 16:cc77770d787f 529
Bragadeesh153 16:cc77770d787f 530 }
Bragadeesh153 16:cc77770d787f 531
Bragadeesh153 16:cc77770d787f 532 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 533 pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
Bragadeesh153 16:cc77770d787f 534
Bragadeesh153 16:cc77770d787f 535
Bragadeesh153 16:cc77770d787f 536
Bragadeesh153 16:cc77770d787f 537 ATS2_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 538 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
Bragadeesh153 16:cc77770d787f 539 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
Bragadeesh153 16:cc77770d787f 540 return 0;
sakthipriya 0:7b4c00e3912f 541 }
sakthipriya 0:7b4c00e3912f 542
Bragadeesh153 16:cc77770d787f 543
Bragadeesh153 16:cc77770d787f 544 int SENSOR_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 545 {
Bragadeesh153 16:cc77770d787f 546 int mag_only=0;
Bragadeesh153 16:cc77770d787f 547 pc_acs.printf("Entering Sensor data acq.\n \r");
Bragadeesh153 16:cc77770d787f 548 char reg;
Bragadeesh153 16:cc77770d787f 549
Bragadeesh153 16:cc77770d787f 550 //int sentral;
Bragadeesh153 16:cc77770d787f 551 int event;
Bragadeesh153 16:cc77770d787f 552 int sensor;
Bragadeesh153 16:cc77770d787f 553 int error;
Bragadeesh153 16:cc77770d787f 554
Bragadeesh153 16:cc77770d787f 555 int init;
sakthipriya 0:7b4c00e3912f 556 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 557 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 558 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 559 wait_ms(100);
sakthipriya 0:7b4c00e3912f 560 pc_acs.printf("Event Status is %x\n \r",(int)status);
Bragadeesh153 16:cc77770d787f 561 event = (int)status;
Bragadeesh153 16:cc77770d787f 562
Bragadeesh153 16:cc77770d787f 563 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
Bragadeesh153 16:cc77770d787f 564 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 565 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
Bragadeesh153 16:cc77770d787f 566 cmd[0]=MAG_XOUT_H; //LSB of x
Bragadeesh153 16:cc77770d787f 567 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 568 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
Bragadeesh153 16:cc77770d787f 569 // pc_acs.printf("\nGyro Values:\n");
Bragadeesh153 16:cc77770d787f 570 for(int i=0; i<3; i++) {
Bragadeesh153 16:cc77770d787f 571 //concatenating gyro LSB and MSB to get 16 bit signed data values
Bragadeesh153 16:cc77770d787f 572 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
Bragadeesh153 16:cc77770d787f 573 gyro_data[i]=(float)bit_data;
Bragadeesh153 16:cc77770d787f 574 gyro_data[i]=gyro_data[i]/senstivity_gyro;
Bragadeesh153 16:cc77770d787f 575 gyro_data[i]+=gyro_error[i];
Bragadeesh153 16:cc77770d787f 576 // pc_acs.printf("%f\t",gyro_data[i]);
Bragadeesh153 16:cc77770d787f 577 }
Bragadeesh153 16:cc77770d787f 578 // pc_acs.printf("\nMag Values:\n");
Bragadeesh153 16:cc77770d787f 579 for(int i=0; i<3; i++) {
Bragadeesh153 16:cc77770d787f 580 //concatenating mag LSB and MSB to get 16 bit signed data values
Bragadeesh153 16:cc77770d787f 581 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
Bragadeesh153 16:cc77770d787f 582 mag_data[i]=(float)bit_data;
Bragadeesh153 16:cc77770d787f 583 mag_data[i]=mag_data[i]/senstivity_mag;
Bragadeesh153 16:cc77770d787f 584 mag_data[i]+=mag_error[i];
Bragadeesh153 16:cc77770d787f 585 //pc_acs.printf("%f\t",mag_data[i]);
Bragadeesh153 16:cc77770d787f 586 }
Bragadeesh153 16:cc77770d787f 587 for(int i=0; i<3; i++) {
Bragadeesh153 16:cc77770d787f 588 // data[i]=gyro_data[i];
Bragadeesh153 16:cc77770d787f 589 actual_data.AngularSpeed_actual[i] = gyro_data[i];
Bragadeesh153 16:cc77770d787f 590 actual_data.Bvalue_actual[i] = mag_data[i];
Bragadeesh153 16:cc77770d787f 591 //data[i+3]=mag_data[i];
Bragadeesh153 16:cc77770d787f 592 }
Bragadeesh153 16:cc77770d787f 593
Bragadeesh153 16:cc77770d787f 594
Bragadeesh153 16:cc77770d787f 595
Bragadeesh153 16:cc77770d787f 596 //(event & 40 != 40 ) || (event & 08 != 08 ) || (event & 01 == 01 )|| (event & 02 == 02 )
Bragadeesh153 16:cc77770d787f 597
Bragadeesh153 16:cc77770d787f 598 if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 )) //check for any error
Bragadeesh153 16:cc77770d787f 599 {
Bragadeesh153 16:cc77770d787f 600 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 601 cmd[1]=BIT_RESREQ;
Bragadeesh153 16:cc77770d787f 602 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 603 wait_ms(600);
Bragadeesh153 16:cc77770d787f 604
Bragadeesh153 16:cc77770d787f 605
Bragadeesh153 16:cc77770d787f 606
Bragadeesh153 16:cc77770d787f 607 cmd[0]=EVT_STATUS;
Bragadeesh153 16:cc77770d787f 608 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 609 i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 16:cc77770d787f 610 wait_ms(100);
Bragadeesh153 16:cc77770d787f 611 pc_acs.printf("Event Status after resetting is %x\n \r",(int)status);
Bragadeesh153 16:cc77770d787f 612
Bragadeesh153 16:cc77770d787f 613 if ( (event & 0x40 != 0x40 ) || (event & 0x08 != 0x08 ) || (event & 0x01 == 0x01 )|| (event & 0x02 == 0x02 ))
Bragadeesh153 16:cc77770d787f 614 {
Bragadeesh153 16:cc77770d787f 615
Bragadeesh153 16:cc77770d787f 616
Bragadeesh153 16:cc77770d787f 617
Bragadeesh153 16:cc77770d787f 618 // cmd[0]=SENTRALSTATUS;
Bragadeesh153 16:cc77770d787f 619 // i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 620 /// i2c.read(SLAVE_ADDR_READ,&reg,1);
Bragadeesh153 16:cc77770d787f 621 // wait_ms(100);
Bragadeesh153 16:cc77770d787f 622 // sentral = (int)reg;
Bragadeesh153 16:cc77770d787f 623
Bragadeesh153 16:cc77770d787f 624 cmd[0]=SENSORSTATUS;
Bragadeesh153 16:cc77770d787f 625 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 626 i2c.read(SLAVE_ADDR_READ,&reg,1);
Bragadeesh153 16:cc77770d787f 627 wait_ms(100);
Bragadeesh153 16:cc77770d787f 628
Bragadeesh153 16:cc77770d787f 629 sensor = (int)reg;
Bragadeesh153 16:cc77770d787f 630
Bragadeesh153 16:cc77770d787f 631 cmd[0]=ERROR;
Bragadeesh153 16:cc77770d787f 632 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 633 i2c.read(SLAVE_ADDR_READ,&reg,1);
Bragadeesh153 16:cc77770d787f 634 wait_ms(100);
Bragadeesh153 16:cc77770d787f 635
Bragadeesh153 16:cc77770d787f 636 error = (int)reg;
Bragadeesh153 16:cc77770d787f 637
Bragadeesh153 16:cc77770d787f 638 if( error&128 == 128)
Bragadeesh153 16:cc77770d787f 639 {
Bragadeesh153 16:cc77770d787f 640 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
Bragadeesh153 16:cc77770d787f 641 cmd[1]=BIT_MAGODR;
Bragadeesh153 16:cc77770d787f 642 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 643 wait_ms(100);
Bragadeesh153 16:cc77770d787f 644 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
Bragadeesh153 16:cc77770d787f 645 cmd[1]=BIT_GYROODR;
Bragadeesh153 16:cc77770d787f 646 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 647 wait_ms(100);
Bragadeesh153 16:cc77770d787f 648 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
Bragadeesh153 16:cc77770d787f 649 cmd[1]=0x00;
Bragadeesh153 16:cc77770d787f 650 wait_ms(100);
Bragadeesh153 16:cc77770d787f 651 cmd[0]=ERROR;
Bragadeesh153 16:cc77770d787f 652 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 653 i2c.read(SLAVE_ADDR_READ,&reg,1);
Bragadeesh153 16:cc77770d787f 654 wait_ms(100);
Bragadeesh153 16:cc77770d787f 655
Bragadeesh153 16:cc77770d787f 656 error = (int)reg;
Bragadeesh153 16:cc77770d787f 657
Bragadeesh153 16:cc77770d787f 658 if( error&128 == 128)
Bragadeesh153 16:cc77770d787f 659 {
Bragadeesh153 16:cc77770d787f 660 pc_acs.printf("Rate error.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 661 return 1;
Bragadeesh153 16:cc77770d787f 662 }
Bragadeesh153 16:cc77770d787f 663
Bragadeesh153 16:cc77770d787f 664
Bragadeesh153 16:cc77770d787f 665 }
Bragadeesh153 16:cc77770d787f 666
Bragadeesh153 16:cc77770d787f 667
Bragadeesh153 16:cc77770d787f 668 if((error&16 == 16) || (error&32 == 32) || (sensor!=0))
Bragadeesh153 16:cc77770d787f 669 {
Bragadeesh153 16:cc77770d787f 670 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
Bragadeesh153 16:cc77770d787f 671 {
Bragadeesh153 16:cc77770d787f 672
Bragadeesh153 16:cc77770d787f 673 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
Bragadeesh153 16:cc77770d787f 674 {
Bragadeesh153 16:cc77770d787f 675 pc_acs.printf("error in both sensors.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 676 return 1;
Bragadeesh153 16:cc77770d787f 677 }
Bragadeesh153 16:cc77770d787f 678 pc_acs.printf("error in gyro alone.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 679 return 2;
Bragadeesh153 16:cc77770d787f 680 }
Bragadeesh153 16:cc77770d787f 681 pc_acs.printf("error in something else.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 682 return 1;
Bragadeesh153 16:cc77770d787f 683 }
Bragadeesh153 16:cc77770d787f 684
Bragadeesh153 16:cc77770d787f 685 if(((int)status & 1 == 1 ))
Bragadeesh153 16:cc77770d787f 686 {
Bragadeesh153 16:cc77770d787f 687 pc_acs.printf("error in CPU Reset.calling init.\n \r");
Bragadeesh153 16:cc77770d787f 688 init = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 689 if(init == 0)
Bragadeesh153 16:cc77770d787f 690 return 1;
Bragadeesh153 16:cc77770d787f 691
Bragadeesh153 16:cc77770d787f 692 cmd[0]=EVT_STATUS;
Bragadeesh153 16:cc77770d787f 693 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 694 i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 16:cc77770d787f 695 wait_ms(100);
Bragadeesh153 16:cc77770d787f 696 if(((int)status & 1 == 1 ))
Bragadeesh153 16:cc77770d787f 697 {
Bragadeesh153 16:cc77770d787f 698 pc_acs.printf("Still error in CPU Reset.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 699 return 1;
Bragadeesh153 16:cc77770d787f 700 }
Bragadeesh153 16:cc77770d787f 701 pc_acs.printf("error in CPU Reset cleared.\n \r");
Bragadeesh153 16:cc77770d787f 702
Bragadeesh153 16:cc77770d787f 703 }
Bragadeesh153 16:cc77770d787f 704
Bragadeesh153 16:cc77770d787f 705 if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
Bragadeesh153 16:cc77770d787f 706 {
Bragadeesh153 16:cc77770d787f 707 pc_acs.printf("Data not ready waiting...\n \r");
Bragadeesh153 16:cc77770d787f 708 //POLL
Bragadeesh153 16:cc77770d787f 709 wait_ms(1500);
Bragadeesh153 16:cc77770d787f 710 cmd[0]=EVT_STATUS;
Bragadeesh153 16:cc77770d787f 711 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 712 i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 16:cc77770d787f 713 wait_ms(100);
Bragadeesh153 16:cc77770d787f 714 if(!(((int)status & 8 == 8 )&&((int)status & 32 == 32 )))
Bragadeesh153 16:cc77770d787f 715 {
Bragadeesh153 16:cc77770d787f 716
Bragadeesh153 16:cc77770d787f 717 if((int)status & 32 != 32 )
Bragadeesh153 16:cc77770d787f 718 {
Bragadeesh153 16:cc77770d787f 719 if((int)status & 8 != 8 )
Bragadeesh153 16:cc77770d787f 720 {
Bragadeesh153 16:cc77770d787f 721 pc_acs.printf("Both data still not ready.Exiting..\n \r");
Bragadeesh153 16:cc77770d787f 722 return 1;
Bragadeesh153 16:cc77770d787f 723 }
Bragadeesh153 16:cc77770d787f 724 pc_acs.printf("Mag data only ready.Read..\n \r");
Bragadeesh153 16:cc77770d787f 725 mag_only = 1;
Bragadeesh153 16:cc77770d787f 726 //return 2;
Bragadeesh153 16:cc77770d787f 727
Bragadeesh153 16:cc77770d787f 728 }
Bragadeesh153 16:cc77770d787f 729
Bragadeesh153 16:cc77770d787f 730
Bragadeesh153 16:cc77770d787f 731 }
Bragadeesh153 16:cc77770d787f 732
Bragadeesh153 16:cc77770d787f 733
Bragadeesh153 16:cc77770d787f 734 }
Bragadeesh153 16:cc77770d787f 735
Bragadeesh153 16:cc77770d787f 736
Bragadeesh153 16:cc77770d787f 737 }
Bragadeesh153 16:cc77770d787f 738
Bragadeesh153 16:cc77770d787f 739
Bragadeesh153 16:cc77770d787f 740
Bragadeesh153 16:cc77770d787f 741
Bragadeesh153 16:cc77770d787f 742
Bragadeesh153 16:cc77770d787f 743 }
Bragadeesh153 16:cc77770d787f 744
Bragadeesh153 16:cc77770d787f 745 cmd[0]=EVT_STATUS;
Bragadeesh153 16:cc77770d787f 746 i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 16:cc77770d787f 747 i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 16:cc77770d787f 748 wait_ms(100);
Bragadeesh153 16:cc77770d787f 749 pc_acs.printf("Event Status is %x\n \r",(int)status);
Bragadeesh153 16:cc77770d787f 750
sakthipriya 0:7b4c00e3912f 751 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
Bragadeesh153 16:cc77770d787f 752
sakthipriya 0:7b4c00e3912f 753 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
sakthipriya 0:7b4c00e3912f 754 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 755 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
sakthipriya 0:7b4c00e3912f 756 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 757 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 758 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 759 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 760 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 761 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 762 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 763 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 764 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 765 gyro_data[i]+=gyro_error[i];
lakshya 15:e09aaaccf134 766 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 767 }
sakthipriya 0:7b4c00e3912f 768 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 769 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 770 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 771 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 772 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 773 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 774 mag_data[i]+=mag_error[i];
lakshya 15:e09aaaccf134 775 //pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 776 }
sakthipriya 0:7b4c00e3912f 777 for(int i=0; i<3; i++) {
sakthipriya 6:036d08b62785 778 // data[i]=gyro_data[i];
sakthipriya 6:036d08b62785 779 actual_data.AngularSpeed_actual[i] = gyro_data[i];
sakthipriya 6:036d08b62785 780 actual_data.Bvalue_actual[i] = mag_data[i];
sakthipriya 6:036d08b62785 781 //data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 782 }
Bragadeesh153 16:cc77770d787f 783
Bragadeesh153 16:cc77770d787f 784 if(mag_only == 0)
Bragadeesh153 16:cc77770d787f 785 {
Bragadeesh153 16:cc77770d787f 786
Bragadeesh153 16:cc77770d787f 787 pc_acs.printf("Reading data successful.\n \r");
Bragadeesh153 16:cc77770d787f 788 return 0;
Bragadeesh153 16:cc77770d787f 789 }
Bragadeesh153 16:cc77770d787f 790 else if(mag_only == 1)
Bragadeesh153 16:cc77770d787f 791 {
Bragadeesh153 16:cc77770d787f 792 pc_acs.printf("Reading data partial success.\n \r");
Bragadeesh153 16:cc77770d787f 793 return 2;
sakthipriya 0:7b4c00e3912f 794 }
Bragadeesh153 16:cc77770d787f 795
Bragadeesh153 16:cc77770d787f 796 pc_acs.printf("Reading data success.\n \r");
Bragadeesh153 16:cc77770d787f 797 return 0;
Bragadeesh153 16:cc77770d787f 798
Bragadeesh153 16:cc77770d787f 799 }
Bragadeesh153 16:cc77770d787f 800
Bragadeesh153 16:cc77770d787f 801
Bragadeesh153 16:cc77770d787f 802
Bragadeesh153 16:cc77770d787f 803 int FCTN_ATS_DATA_ACQ()
Bragadeesh153 16:cc77770d787f 804 {
Bragadeesh153 16:cc77770d787f 805
Bragadeesh153 16:cc77770d787f 806 int acq;
Bragadeesh153 16:cc77770d787f 807
Bragadeesh153 16:cc77770d787f 808 pc_acs.printf("DATA_ACQ called \n \r");
Bragadeesh153 16:cc77770d787f 809 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 810
Bragadeesh153 16:cc77770d787f 811 // 0 success //1 full failure //2 partial failure
Bragadeesh153 16:cc77770d787f 812
Bragadeesh153 16:cc77770d787f 813
Bragadeesh153 16:cc77770d787f 814
Bragadeesh153 16:cc77770d787f 815 if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
Bragadeesh153 16:cc77770d787f 816 {
Bragadeesh153 16:cc77770d787f 817
Bragadeesh153 16:cc77770d787f 818 acq = SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 819 if(acq == 0)
Bragadeesh153 16:cc77770d787f 820 {
Bragadeesh153 16:cc77770d787f 821
Bragadeesh153 16:cc77770d787f 822 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 16:cc77770d787f 823
Bragadeesh153 16:cc77770d787f 824 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 16:cc77770d787f 825 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 826 pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
Bragadeesh153 16:cc77770d787f 827 return 0;
Bragadeesh153 16:cc77770d787f 828 }
Bragadeesh153 16:cc77770d787f 829 else if(acq == 2)
Bragadeesh153 16:cc77770d787f 830 {
Bragadeesh153 16:cc77770d787f 831 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
Bragadeesh153 16:cc77770d787f 832 pc_acs.printf(" Sensor 1 data partial success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 833 return 2;
Bragadeesh153 16:cc77770d787f 834
Bragadeesh153 16:cc77770d787f 835 /*if((ACS_ATS_STATUS & 0x0F == 0x00))
Bragadeesh153 16:cc77770d787f 836 {
Bragadeesh153 16:cc77770d787f 837 pc_acs.printf(" Sensor 1 data acq partial success.Trying Sensor 2\n \r");
Bragadeesh153 16:cc77770d787f 838 ATS1_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 839 ATS2_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 840 wait_ms(5);
Bragadeesh153 16:cc77770d787f 841 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
Bragadeesh153 16:cc77770d787f 842
Bragadeesh153 16:cc77770d787f 843 int acq;
Bragadeesh153 16:cc77770d787f 844 acq = SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 845
Bragadeesh153 16:cc77770d787f 846 if(acq == 0)
Bragadeesh153 16:cc77770d787f 847 {
Bragadeesh153 16:cc77770d787f 848 ACS_DATA_ACQ_STATUS = 0;
Bragadeesh153 16:cc77770d787f 849 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 850 return 0;
Bragadeesh153 16:cc77770d787f 851
Bragadeesh153 16:cc77770d787f 852 }
Bragadeesh153 16:cc77770d787f 853 else if(acq == 2)
Bragadeesh153 16:cc77770d787f 854 {
Bragadeesh153 16:cc77770d787f 855 ACS_DATA_ACQ_STATUS = 2;
Bragadeesh153 16:cc77770d787f 856 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 857 return 2;
Bragadeesh153 16:cc77770d787f 858 }
Bragadeesh153 16:cc77770d787f 859
Bragadeesh153 16:cc77770d787f 860 else if(acq == 1)
Bragadeesh153 16:cc77770d787f 861 {
Bragadeesh153 16:cc77770d787f 862
Bragadeesh153 16:cc77770d787f 863 int acq;
Bragadeesh153 16:cc77770d787f 864 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
Bragadeesh153 16:cc77770d787f 865 ATS2_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 866 ATS1_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 867 wait_ms(5);
Bragadeesh153 16:cc77770d787f 868 acq = SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 869 if(acq == 0)
Bragadeesh153 16:cc77770d787f 870 {
Bragadeesh153 16:cc77770d787f 871 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 872 ACS_DATA_ACQ_STATUS = 0;
Bragadeesh153 16:cc77770d787f 873 return 0;
Bragadeesh153 16:cc77770d787f 874 }
Bragadeesh153 16:cc77770d787f 875 else if(acq == 2)
Bragadeesh153 16:cc77770d787f 876 {
Bragadeesh153 16:cc77770d787f 877 pc_acs.printf(" Sensor 1 data acq partial success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 878 ACS_DATA_ACQ_STATUS = 2;
Bragadeesh153 16:cc77770d787f 879 return 2;
Bragadeesh153 16:cc77770d787f 880 }
Bragadeesh153 16:cc77770d787f 881 else
Bragadeesh153 16:cc77770d787f 882 {
Bragadeesh153 16:cc77770d787f 883 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 884 ATS1_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 885 ACS_DATA_ACQ_STATUS = 1;
Bragadeesh153 16:cc77770d787f 886 return 1;
Bragadeesh153 16:cc77770d787f 887 }
Bragadeesh153 16:cc77770d787f 888
Bragadeesh153 16:cc77770d787f 889 pc_acs.printf(" Sensor 1 data acq failure.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 890 ATS1_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 891 ACS_DATA_ACQ_STATUS = 1;
Bragadeesh153 16:cc77770d787f 892 return 1;
Bragadeesh153 16:cc77770d787f 893
Bragadeesh153 16:cc77770d787f 894 }
Bragadeesh153 16:cc77770d787f 895
Bragadeesh153 16:cc77770d787f 896 }
Bragadeesh153 16:cc77770d787f 897
Bragadeesh153 16:cc77770d787f 898 else
Bragadeesh153 16:cc77770d787f 899 {
Bragadeesh153 16:cc77770d787f 900 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x40;
Bragadeesh153 16:cc77770d787f 901 pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 902 return 2;
Bragadeesh153 16:cc77770d787f 903
Bragadeesh153 16:cc77770d787f 904
Bragadeesh153 16:cc77770d787f 905 }*/
Bragadeesh153 16:cc77770d787f 906
Bragadeesh153 16:cc77770d787f 907 }
Bragadeesh153 16:cc77770d787f 908
Bragadeesh153 16:cc77770d787f 909 else if(acq == 1)
Bragadeesh153 16:cc77770d787f 910 {
Bragadeesh153 16:cc77770d787f 911 pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r");
Bragadeesh153 16:cc77770d787f 912 ATS1_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 913 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
Bragadeesh153 16:cc77770d787f 914 }
Bragadeesh153 16:cc77770d787f 915
Bragadeesh153 16:cc77770d787f 916
Bragadeesh153 16:cc77770d787f 917
sakthipriya 0:7b4c00e3912f 918 }
Bragadeesh153 16:cc77770d787f 919
Bragadeesh153 16:cc77770d787f 920
Bragadeesh153 16:cc77770d787f 921
Bragadeesh153 16:cc77770d787f 922
Bragadeesh153 16:cc77770d787f 923 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xE0;
Bragadeesh153 16:cc77770d787f 924
Bragadeesh153 16:cc77770d787f 925
Bragadeesh153 16:cc77770d787f 926 if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
sakthipriya 0:7b4c00e3912f 927 {
Bragadeesh153 16:cc77770d787f 928 ATS2_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 929 wait_ms(5);
Bragadeesh153 16:cc77770d787f 930
Bragadeesh153 16:cc77770d787f 931 acq = SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 932 if(acq == 0)
Bragadeesh153 16:cc77770d787f 933 {
Bragadeesh153 16:cc77770d787f 934 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 935 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 16:cc77770d787f 936 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 16:cc77770d787f 937 return 0;
Bragadeesh153 16:cc77770d787f 938 }
Bragadeesh153 16:cc77770d787f 939 else if(acq == 2)
Bragadeesh153 16:cc77770d787f 940 {
Bragadeesh153 16:cc77770d787f 941
Bragadeesh153 16:cc77770d787f 942 pc_acs.printf(" Sensor 2 data acq partial success.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 943
Bragadeesh153 16:cc77770d787f 944 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x04;
Bragadeesh153 16:cc77770d787f 945 ACS_DATA_ACQ_STATUS = 2; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 16:cc77770d787f 946 return 2;
Bragadeesh153 16:cc77770d787f 947
Bragadeesh153 16:cc77770d787f 948 }
Bragadeesh153 16:cc77770d787f 949
Bragadeesh153 16:cc77770d787f 950 else if(acq == 1)
Bragadeesh153 16:cc77770d787f 951 {
Bragadeesh153 16:cc77770d787f 952 pc_acs.printf(" Sensor 2 data acq failure.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 953 ATS2_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 954 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
Bragadeesh153 16:cc77770d787f 955 //Sensor 2 also not working
Bragadeesh153 16:cc77770d787f 956 }
Bragadeesh153 16:cc77770d787f 957
Bragadeesh153 16:cc77770d787f 958
Bragadeesh153 16:cc77770d787f 959
Bragadeesh153 16:cc77770d787f 960
Bragadeesh153 16:cc77770d787f 961
sakthipriya 0:7b4c00e3912f 962 }
Bragadeesh153 16:cc77770d787f 963
Bragadeesh153 16:cc77770d787f 964 pc_acs.printf(" Reading value from sensor 1 before exiting\n \r");
Bragadeesh153 16:cc77770d787f 965 ATS1_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 966 wait_ms(5);
Bragadeesh153 16:cc77770d787f 967 SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 968 ATS1_SW_ENABLE = 1;
Bragadeesh153 16:cc77770d787f 969 wait_ms(5);
Bragadeesh153 16:cc77770d787f 970
Bragadeesh153 16:cc77770d787f 971 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0E;
Bragadeesh153 16:cc77770d787f 972
Bragadeesh153 16:cc77770d787f 973 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 974 pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 975
Bragadeesh153 16:cc77770d787f 976 ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
Bragadeesh153 16:cc77770d787f 977 return 1;
sakthipriya 0:7b4c00e3912f 978 }
sakthipriya 0:7b4c00e3912f 979
sakthipriya 0:7b4c00e3912f 980 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 981 {
sakthipriya 0:7b4c00e3912f 982 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 983
sakthipriya 0:7b4c00e3912f 984 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 985 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 986 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 987 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 988 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 989 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 990
sakthipriya 0:7b4c00e3912f 991
sakthipriya 0:7b4c00e3912f 992 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 993 {
sakthipriya 0:7b4c00e3912f 994 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 995 }
sakthipriya 0:7b4c00e3912f 996
sakthipriya 0:7b4c00e3912f 997 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 998
sakthipriya 0:7b4c00e3912f 999
sakthipriya 0:7b4c00e3912f 1000 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 1001
sakthipriya 0:7b4c00e3912f 1002 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 1003 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 1004 {
sakthipriya 0:7b4c00e3912f 1005 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 1006 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 1007 }
sakthipriya 0:7b4c00e3912f 1008
sakthipriya 0:7b4c00e3912f 1009 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 1010 printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 1011 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 1012 {
lakshya 10:f93407b97750 1013 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 10:f93407b97750 1014 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1015 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1016 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1017 }
lakshya 10:f93407b97750 1018 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 1019 {
lakshya 10:f93407b97750 1020 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 10:f93407b97750 1021 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1022 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1023 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1024 }
lakshya 10:f93407b97750 1025 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 1026 {
lakshya 10:f93407b97750 1027 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 10:f93407b97750 1028 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1029 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1030 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1031 }
lakshya 10:f93407b97750 1032 else if(l_current_x==0)
lakshya 10:f93407b97750 1033 {
lakshya 10:f93407b97750 1034 printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 1035 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 10:f93407b97750 1036 printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1037 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1038 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1039 }
lakshya 10:f93407b97750 1040 else //not necessary
lakshya 10:f93407b97750 1041 {
lakshya 10:f93407b97750 1042 g_err_flag_TR_x = 1;
lakshya 10:f93407b97750 1043 }
lakshya 10:f93407b97750 1044
lakshya 10:f93407b97750 1045 //------------------------------------- y-direction TR--------------------------------------//
lakshya 10:f93407b97750 1046
lakshya 10:f93407b97750 1047
lakshya 10:f93407b97750 1048 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 1049
lakshya 10:f93407b97750 1050 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 1051 if (l_moment_y <0)
lakshya 10:f93407b97750 1052 {
lakshya 10:f93407b97750 1053 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 1054 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 1055 }
lakshya 10:f93407b97750 1056
lakshya 10:f93407b97750 1057
lakshya 10:f93407b97750 1058 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 1059 printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 1060 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 1061 {
lakshya 10:f93407b97750 1062 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 10:f93407b97750 1063 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1064 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1065 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1066 }
lakshya 10:f93407b97750 1067 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 1068 {
lakshya 10:f93407b97750 1069 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 10:f93407b97750 1070 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1071 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1072 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1073 }
lakshya 10:f93407b97750 1074 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 1075 {
lakshya 10:f93407b97750 1076 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 10:f93407b97750 1077 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1078 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1079 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1080 }
lakshya 10:f93407b97750 1081 else if(l_current_y==0)
lakshya 10:f93407b97750 1082 {
lakshya 10:f93407b97750 1083 printf("\n \r l_current_y====0");
lakshya 10:f93407b97750 1084 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 10:f93407b97750 1085 printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1086 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1087 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1088 }
lakshya 10:f93407b97750 1089 else // not necessary
lakshya 10:f93407b97750 1090 {
lakshya 10:f93407b97750 1091 g_err_flag_TR_y = 1;
lakshya 10:f93407b97750 1092 }
lakshya 10:f93407b97750 1093
lakshya 10:f93407b97750 1094 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 1095
lakshya 10:f93407b97750 1096
lakshya 10:f93407b97750 1097 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 1098
lakshya 10:f93407b97750 1099 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 1100 if (l_moment_z <0)
lakshya 10:f93407b97750 1101 {
lakshya 10:f93407b97750 1102 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 1103 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 1104 }
lakshya 10:f93407b97750 1105
lakshya 10:f93407b97750 1106
lakshya 10:f93407b97750 1107 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 10:f93407b97750 1108 printf("current in trz is %f \r \n",l_current_z);
lakshya 10:f93407b97750 1109 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 1110 {
lakshya 10:f93407b97750 1111 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 10:f93407b97750 1112 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1113 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1114 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1115 }
lakshya 10:f93407b97750 1116 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 1117 {
lakshya 10:f93407b97750 1118 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 10:f93407b97750 1119 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1120 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1121 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1122 }
lakshya 10:f93407b97750 1123 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 1124 {
lakshya 10:f93407b97750 1125 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 10:f93407b97750 1126 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1127 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1128 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1129 }
lakshya 10:f93407b97750 1130 else if(l_current_z==0)
lakshya 10:f93407b97750 1131 {
lakshya 10:f93407b97750 1132 printf("\n \r l_current_z====0");
lakshya 10:f93407b97750 1133 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 10:f93407b97750 1134 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1135 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1136 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1137 }
lakshya 10:f93407b97750 1138 else // not necessary
lakshya 10:f93407b97750 1139 {
lakshya 10:f93407b97750 1140 g_err_flag_TR_z = 1;
lakshya 10:f93407b97750 1141 }
lakshya 10:f93407b97750 1142
lakshya 10:f93407b97750 1143 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 1144
lakshya 10:f93407b97750 1145 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 1146
lakshya 10:f93407b97750 1147 }
lakshya 10:f93407b97750 1148
lakshya 10:f93407b97750 1149
lakshya 10:f93407b97750 1150 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
lakshya 10:f93407b97750 1151 {
lakshya 10:f93407b97750 1152 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
lakshya 10:f93407b97750 1153
lakshya 10:f93407b97750 1154 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
lakshya 10:f93407b97750 1155 float l_current_x=0; //Current sent in x TR's
lakshya 10:f93407b97750 1156 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
lakshya 10:f93407b97750 1157 float l_current_y=0; //Current sent in y TR's
lakshya 10:f93407b97750 1158 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
lakshya 10:f93407b97750 1159 float l_current_z=0; //Current sent in z TR's
lakshya 10:f93407b97750 1160
lakshya 10:f93407b97750 1161
lakshya 10:f93407b97750 1162 for(int i = 0 ; i<3;i++)
lakshya 10:f93407b97750 1163 {
lakshya 10:f93407b97750 1164 printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
lakshya 10:f93407b97750 1165 }
lakshya 10:f93407b97750 1166
lakshya 10:f93407b97750 1167 //----------------------------- x-direction TR --------------------------------------------//
lakshya 10:f93407b97750 1168
lakshya 10:f93407b97750 1169
lakshya 10:f93407b97750 1170 float l_moment_x = Moment[0]; //Moment in x direction
lakshya 10:f93407b97750 1171
lakshya 10:f93407b97750 1172 phase_TR_x = 1; // setting the default current direction
lakshya 10:f93407b97750 1173 if (l_moment_x <0)
lakshya 10:f93407b97750 1174 {
lakshya 10:f93407b97750 1175 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
lakshya 10:f93407b97750 1176 l_moment_x = abs(l_moment_x);
lakshya 10:f93407b97750 1177 }
lakshya 10:f93407b97750 1178
lakshya 10:f93407b97750 1179 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 1180 pc_acs.printf("current in trx is %f \r \n",l_current_x);
sakthipriya 0:7b4c00e3912f 1181 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 1182 {
sakthipriya 0:7b4c00e3912f 1183 l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1184 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 1185 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1186 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 1187 }
sakthipriya 0:7b4c00e3912f 1188 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
sakthipriya 0:7b4c00e3912f 1189 {
sakthipriya 0:7b4c00e3912f 1190 l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1191 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 1192 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1193 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 1194 }
sakthipriya 0:7b4c00e3912f 1195 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
sakthipriya 0:7b4c00e3912f 1196 {
sakthipriya 0:7b4c00e3912f 1197 l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1198 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 1199 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1200 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 1201 }
sakthipriya 0:7b4c00e3912f 1202 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
sakthipriya 0:7b4c00e3912f 1203 {
sakthipriya 0:7b4c00e3912f 1204 l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1205 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 1206 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1207 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 1208 }
sakthipriya 0:7b4c00e3912f 1209 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 1210 {
sakthipriya 0:7b4c00e3912f 1211 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 1212 l_duty_cycle_x = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 1213 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 1214 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1215 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 1216 }
sakthipriya 0:7b4c00e3912f 1217 else //not necessary
sakthipriya 0:7b4c00e3912f 1218 {
sakthipriya 0:7b4c00e3912f 1219 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 1220 }
sakthipriya 0:7b4c00e3912f 1221
sakthipriya 0:7b4c00e3912f 1222 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 1223
sakthipriya 0:7b4c00e3912f 1224
sakthipriya 0:7b4c00e3912f 1225 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 1226
sakthipriya 0:7b4c00e3912f 1227 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 1228 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 1229 {
sakthipriya 0:7b4c00e3912f 1230 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
sakthipriya 0:7b4c00e3912f 1231 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 1232 }
sakthipriya 0:7b4c00e3912f 1233
sakthipriya 0:7b4c00e3912f 1234
sakthipriya 0:7b4c00e3912f 1235 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 1236 pc_acs.printf("current in try is %f \r \n",l_current_y);
sakthipriya 0:7b4c00e3912f 1237 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 1238 {
sakthipriya 0:7b4c00e3912f 1239 l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1240 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 1241 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1242 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 1243 }
sakthipriya 0:7b4c00e3912f 1244 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
sakthipriya 0:7b4c00e3912f 1245 {
sakthipriya 0:7b4c00e3912f 1246 l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1247 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 1248 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1249 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 1250 }
sakthipriya 0:7b4c00e3912f 1251 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
sakthipriya 0:7b4c00e3912f 1252 {
sakthipriya 0:7b4c00e3912f 1253 l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1254 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 1255 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1256 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 1257 }
sakthipriya 0:7b4c00e3912f 1258 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
sakthipriya 0:7b4c00e3912f 1259 {
sakthipriya 0:7b4c00e3912f 1260 l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1261 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 1262 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1263 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 1264 }
sakthipriya 0:7b4c00e3912f 1265 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 1266 {
sakthipriya 0:7b4c00e3912f 1267 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 1268 l_duty_cycle_y = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 1269 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 1270 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1271 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 1272 }
sakthipriya 0:7b4c00e3912f 1273 else // not necessary
sakthipriya 0:7b4c00e3912f 1274 {
sakthipriya 0:7b4c00e3912f 1275 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 1276 }
sakthipriya 0:7b4c00e3912f 1277
sakthipriya 0:7b4c00e3912f 1278 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 1279
sakthipriya 0:7b4c00e3912f 1280
sakthipriya 0:7b4c00e3912f 1281 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 1282
sakthipriya 0:7b4c00e3912f 1283 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 1284 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 1285 {
sakthipriya 0:7b4c00e3912f 1286 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
sakthipriya 0:7b4c00e3912f 1287 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 1288 }
sakthipriya 0:7b4c00e3912f 1289
sakthipriya 0:7b4c00e3912f 1290
sakthipriya 0:7b4c00e3912f 1291 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 1292 pc_acs.printf("current in trz is %f \r \n",l_current_z);
sakthipriya 0:7b4c00e3912f 1293 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 1294 {
sakthipriya 0:7b4c00e3912f 1295 l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1296 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 1297 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1298 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 1299 }
sakthipriya 0:7b4c00e3912f 1300 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
sakthipriya 0:7b4c00e3912f 1301 {
sakthipriya 0:7b4c00e3912f 1302 l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1303 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 1304 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1305 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 1306 }
sakthipriya 0:7b4c00e3912f 1307 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
sakthipriya 0:7b4c00e3912f 1308 {
sakthipriya 0:7b4c00e3912f 1309 l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1310 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 1311 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1312 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 1313 }
sakthipriya 0:7b4c00e3912f 1314 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
sakthipriya 0:7b4c00e3912f 1315 {
sakthipriya 0:7b4c00e3912f 1316 l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 1317 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 1318 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1319 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 1320 }
sakthipriya 0:7b4c00e3912f 1321 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 1322 {
sakthipriya 0:7b4c00e3912f 1323 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 1324 l_duty_cycle_z = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 1325 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 1326 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 1327 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 1328 }
sakthipriya 0:7b4c00e3912f 1329 else // not necessary
sakthipriya 0:7b4c00e3912f 1330 {
sakthipriya 0:7b4c00e3912f 1331 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 1332 }
sakthipriya 0:7b4c00e3912f 1333
sakthipriya 0:7b4c00e3912f 1334 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 1335
sakthipriya 0:7b4c00e3912f 1336 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 1337
lakshya 10:f93407b97750 1338 }*/
sakthipriya 0:7b4c00e3912f 1339
sakthipriya 0:7b4c00e3912f 1340
sakthipriya 0:7b4c00e3912f 1341