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@18:21740620c65e, 2016-06-13 (annotated)

Committer:
Bragadeesh153
Date:
Mon Jun 13 13:44:31 2016 +0000
Revision:
18:21740620c65e
Parent:
17:1e1955f3db75
Child:
19:403cb36e22ed
ACS algo commissioning done, Hardware comissioning yet to be finalised

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