Team Fox / Mbed 2 deprecated ACS_FULL_Flowchart_BAE

ACS completed fully. All cases to be tested

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of ACS_Flowchart_BAE by Team Fox

ACS.cpp@17:1e1955f3db75, 2016-06-09 (annotated)

Committer:
Bragadeesh153
Date:
Thu Jun 09 14:12:55 2016 +0000
Revision:
17:1e1955f3db75
Parent:
16:cc77770d787f
Child:
18:21740620c65e
ACS_LATEST_CODE

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