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@19:403cb36e22ed, 2016-06-28 (annotated)

Committer:
Bragadeesh153
Date:
Tue Jun 28 10:11:54 2016 +0000
Revision:
19:403cb36e22ed
Parent:
18:21740620c65e 
FINAL ACS TO BE USED FOR TESTING

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:7b4c00e3912f 1 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:7b4c00e3912f 2 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya 0:7b4c00e3912f 3 #include <mbed.h>
sakthipriya 0:7b4c00e3912f 4 #include <math.h>
sakthipriya 0:7b4c00e3912f 5
sakthipriya 0:7b4c00e3912f 6 #include "pni.h" //pni header file
sakthipriya 0:7b4c00e3912f 7 #include "pin_config.h"
sakthipriya 0:7b4c00e3912f 8 #include "ACS.h"
sakthipriya 6:036d08b62785 9 #include "EPS.h"
sakthipriya 0:7b4c00e3912f 10
sakthipriya 0:7b4c00e3912f 11 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 12 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 13 extern uint32_t BAE_ENABLE;
Bragadeesh153 13:fb7facaf308b 14 extern uint8_t ACS_INIT_STATUS;
Bragadeesh153 13:fb7facaf308b 15 extern uint8_t ACS_DATA_ACQ_STATUS;
Bragadeesh153 13:fb7facaf308b 16 extern uint8_t ACS_ATS_STATUS;
Bragadeesh153 13:fb7facaf308b 17 extern uint8_t ACS_MAIN_STATUS;
Bragadeesh153 13:fb7facaf308b 18 extern uint8_t ACS_STATUS;
sakthipriya 0:7b4c00e3912f 19
Bragadeesh153 16:cc77770d787f 20 extern DigitalOut ATS1_SW_ENABLE; // enable of att sens2 switch
Bragadeesh153 16:cc77770d787f 21 extern DigitalOut ATS2_SW_ENABLE; // enable of att sens switch
Bragadeesh153 16:cc77770d787f 22
Bragadeesh153 13:fb7facaf308b 23 extern uint8_t ACS_ATS_ENABLE;
Bragadeesh153 13:fb7facaf308b 24 extern uint8_t ACS_DATA_ACQ_ENABLE;
Bragadeesh153 13:fb7facaf308b 25 extern uint8_t ACS_STATE;
sakthipriya 0:7b4c00e3912f 26
sakthipriya 0:7b4c00e3912f 27 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya 0:7b4c00e3912f 28 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya 0:7b4c00e3912f 29 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 30
sakthipriya 0:7b4c00e3912f 31 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 32 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 33 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 34
sakthipriya 0:7b4c00e3912f 35 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 36 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 37 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39 extern float data[6];
sakthipriya 6:036d08b62785 40 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 41
sakthipriya 0:7b4c00e3912f 42
sakthipriya 0:7b4c00e3912f 43 Serial pc_acs(USBTX,USBRX); //for usb communication
lakshya 10:f93407b97750 44 //CONTROL_ALGO
lakshya 10:f93407b97750 45
lakshya 10:f93407b97750 46 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla
Bragadeesh153 17:1e1955f3db75 47 uint8_t flag_firsttime=1, alarmmode=0;
lakshya 10:f93407b97750 48
lakshya 10:f93407b97750 49
lakshya 10:f93407b97750 50
Bragadeesh153 17:1e1955f3db75 51
lakshya 10:f93407b97750 52 float max_array(float arr[3]);
sakthipriya 0:7b4c00e3912f 53 void inverse(float mat[3][3],float inv[3][3]);
lakshya 10:f93407b97750 54
lakshya 10:f93407b97750 55 //CONTROLALGO PARAMETERS
lakshya 10:f93407b97750 56
lakshya 10:f93407b97750 57
Bragadeesh153 17:1e1955f3db75 58
Bragadeesh153 17:1e1955f3db75 59
Bragadeesh153 19:403cb36e22ed 60 void FCTN_ACS_CNTRLALGO (float moment[3],float b[3] ,float omega[3],uint8_t nominal,uint8_t detumbling,uint8_t ACS_DETUMBLING_ALGO_TYPE)
sakthipriya 0:7b4c00e3912f 61 {
Bragadeesh153 19:403cb36e22ed 62 float db[3];
lakshya 10:f93407b97750 63 float normalising_fact;
Bragadeesh153 17:1e1955f3db75 64 float b_copy[3], omega_copy[3], db_copy[3];
lakshya 10:f93407b97750 65 int i;
lakshya 10:f93407b97750 66 if(flag_firsttime==1)
lakshya 10:f93407b97750 67 {
lakshya 10:f93407b97750 68 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 69 {
Bragadeesh153 17:1e1955f3db75 70 db[i]=0; // Unit: Tesla/Second
Bragadeesh153 19:403cb36e22ed 71 }
lakshya 10:f93407b97750 72 flag_firsttime=0;
lakshya 10:f93407b97750 73 }
Bragadeesh153 19:403cb36e22ed 74
sakthipriya 0:7b4c00e3912f 75 else
sakthipriya 0:7b4c00e3912f 76 {
sakthipriya 0:7b4c00e3912f 77 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 78 {
Bragadeesh153 17:1e1955f3db75 79 db[i]= (b[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
sakthipriya 0:7b4c00e3912f 80 }
lakshya 10:f93407b97750 81 }
lakshya 10:f93407b97750 82
Bragadeesh153 19:403cb36e22ed 83 if(nominal == 0)
Bragadeesh153 18:21740620c65e 84
Bragadeesh153 18:21740620c65e 85 {
Bragadeesh153 18:21740620c65e 86
Bragadeesh153 18:21740620c65e 87 if(max_array(omega)<(0.8*OmegaMax) && alarmmode==1)
Bragadeesh153 19:403cb36e22ed 88 {
Bragadeesh153 19:403cb36e22ed 89 alarmmode=0;
Bragadeesh153 19:403cb36e22ed 90 }
Bragadeesh153 18:21740620c65e 91 else if(max_array(omega)>OmegaMax&& alarmmode==0)
Bragadeesh153 19:403cb36e22ed 92 {
Bragadeesh153 19:403cb36e22ed 93 alarmmode=1;
Bragadeesh153 19:403cb36e22ed 94 }
Bragadeesh153 18:21740620c65e 95
Bragadeesh153 18:21740620c65e 96 }
Bragadeesh153 18:21740620c65e 97
Bragadeesh153 18:21740620c65e 98
Bragadeesh153 18:21740620c65e 99
lakshya 10:f93407b97750 100
lakshya 10:f93407b97750 101 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 102 {
Bragadeesh153 17:1e1955f3db75 103 b_copy[i]=b[i];
Bragadeesh153 17:1e1955f3db75 104 db_copy[i]=db[i];
Bragadeesh153 17:1e1955f3db75 105 omega_copy[i]=omega[i];
sakthipriya 0:7b4c00e3912f 106 }
lakshya 10:f93407b97750 107
Bragadeesh153 19:403cb36e22ed 108 if(((alarmmode==0)|| (nominal == 1))&&(detumbling==0))
lakshya 10:f93407b97750 109 {
Bragadeesh153 19:403cb36e22ed 110 controlmodes(moment,b,db,omega,0x00,ACS_DETUMBLING_ALGO_TYPE);
lakshya 10:f93407b97750 111 for (i=0;i<3;i++)
lakshya 10:f93407b97750 112 {
Bragadeesh153 17:1e1955f3db75 113 b[i]=b_copy[i];
Bragadeesh153 17:1e1955f3db75 114 db[i]=db_copy[i];
Bragadeesh153 17:1e1955f3db75 115 omega[i]=omega_copy[i];
lakshya 10:f93407b97750 116 }
Bragadeesh153 19:403cb36e22ed 117 if(max_array(moment)>MmntMax)
Bragadeesh153 19:403cb36e22ed 118 {
Bragadeesh153 19:403cb36e22ed 119 controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
Bragadeesh153 19:403cb36e22ed 120 for (i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 121 {
Bragadeesh153 19:403cb36e22ed 122 b[i]=b_copy[i];
Bragadeesh153 19:403cb36e22ed 123 db[i]=db_copy[i];
Bragadeesh153 19:403cb36e22ed 124 omega[i]=omega_copy[i];
Bragadeesh153 19:403cb36e22ed 125 }
Bragadeesh153 19:403cb36e22ed 126 if(max_array(moment)>MmntMax)
Bragadeesh153 19:403cb36e22ed 127 {
Bragadeesh153 19:403cb36e22ed 128 normalising_fact=max_array(moment)/MmntMax;
Bragadeesh153 19:403cb36e22ed 129 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 130 {
Bragadeesh153 19:403cb36e22ed 131 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
Bragadeesh153 19:403cb36e22ed 132 }
Bragadeesh153 19:403cb36e22ed 133 }
lakshya 10:f93407b97750 134 }
Bragadeesh153 19:403cb36e22ed 135
lakshya 10:f93407b97750 136 }
Bragadeesh153 19:403cb36e22ed 137
Bragadeesh153 19:403cb36e22ed 138 else
lakshya 10:f93407b97750 139 {
Bragadeesh153 19:403cb36e22ed 140 controlmodes(moment,b,db,omega,0x01,ACS_DETUMBLING_ALGO_TYPE);
Bragadeesh153 19:403cb36e22ed 141 for (i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 142 {
Bragadeesh153 19:403cb36e22ed 143 b[i]=b_copy[i];
Bragadeesh153 19:403cb36e22ed 144 db[i]=db_copy[i];
Bragadeesh153 19:403cb36e22ed 145 omega[i]=omega_copy[i];
Bragadeesh153 19:403cb36e22ed 146 }
lakshya 10:f93407b97750 147 if(max_array(moment)>MmntMax)
lakshya 10:f93407b97750 148 {
lakshya 10:f93407b97750 149 normalising_fact=max_array(moment)/MmntMax;
lakshya 10:f93407b97750 150 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 151 {
Bragadeesh153 19:403cb36e22ed 152 moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
Bragadeesh153 19:403cb36e22ed 153 }
lakshya 10:f93407b97750 154 }
lakshya 10:f93407b97750 155
lakshya 10:f93407b97750 156 }
lakshya 10:f93407b97750 157 for (i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 158 {
Bragadeesh153 17:1e1955f3db75 159 b_old[i]=b[i];
sakthipriya 0:7b4c00e3912f 160 }
sakthipriya 0:7b4c00e3912f 161 }
lakshya 10:f93407b97750 162
Bragadeesh153 17:1e1955f3db75 163 void inverse(float mat[3][3],float inv[3][3],int &singularity_flag)
sakthipriya 0:7b4c00e3912f 164 {
sakthipriya 0:7b4c00e3912f 165 int i,j;
sakthipriya 0:7b4c00e3912f 166 float det=0;
sakthipriya 0:7b4c00e3912f 167 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 168 {
sakthipriya 0:7b4c00e3912f 169 for(j=0;j<3;j++)
lakshya 10:f93407b97750 170 {
sakthipriya 0:7b4c00e3912f 171 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 172 }
sakthipriya 0:7b4c00e3912f 173 }
sakthipriya 0:7b4c00e3912f 174 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
Bragadeesh153 17:1e1955f3db75 175 if (det==0)
Bragadeesh153 17:1e1955f3db75 176 {
Bragadeesh153 17:1e1955f3db75 177 singularity_flag=1;
Bragadeesh153 17:1e1955f3db75 178 }
Bragadeesh153 17:1e1955f3db75 179 else
lakshya 10:f93407b97750 180 {
Bragadeesh153 17:1e1955f3db75 181 singularity_flag=0;
Bragadeesh153 17:1e1955f3db75 182 for(i=0;i<3;i++)
lakshya 10:f93407b97750 183 {
Bragadeesh153 17:1e1955f3db75 184 for(j=0;j<3;j++)
Bragadeesh153 17:1e1955f3db75 185 {
Bragadeesh153 17:1e1955f3db75 186 inv[i][j]/=det;
Bragadeesh153 17:1e1955f3db75 187 }
lakshya 10:f93407b97750 188 }
sakthipriya 0:7b4c00e3912f 189 }
sakthipriya 0:7b4c00e3912f 190 }
sakthipriya 0:7b4c00e3912f 191
lakshya 10:f93407b97750 192 float max_array(float arr[3])
lakshya 10:f93407b97750 193 {
lakshya 10:f93407b97750 194 int i;
lakshya 10:f93407b97750 195 float temp_max=fabs(arr[0]);
lakshya 10:f93407b97750 196 for(i=1;i<3;i++)
lakshya 10:f93407b97750 197 {
lakshya 10:f93407b97750 198 if(fabs(arr[i])>temp_max)
lakshya 10:f93407b97750 199 {
lakshya 10:f93407b97750 200 temp_max=fabs(arr[i]);
lakshya 10:f93407b97750 201 }
lakshya 10:f93407b97750 202 }
lakshya 10:f93407b97750 203 return temp_max;
lakshya 10:f93407b97750 204 }
lakshya 10:f93407b97750 205
lakshya 10:f93407b97750 206
Bragadeesh153 19:403cb36e22ed 207 void controlmodes(float moment[3],float b[3], float db[3], float omega[3], uint8_t controlmode1,uint8_t ACS_DETUMBLING_ALGO_TYPE)
lakshya 10:f93407b97750 208 {
lakshya 10:f93407b97750 209 float bb[3]={0,0,0};
lakshya 10:f93407b97750 210 float d[3]={0,0,0};
lakshya 10:f93407b97750 211 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 212 float den=0,den2;
lakshya 10:f93407b97750 213 float bcopy[3];
lakshya 10:f93407b97750 214 int i, j;//temporary variables
lakshya 10:f93407b97750 215 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
lakshya 10:f93407b97750 216 float invJm[3][3];
Bragadeesh153 17:1e1955f3db75 217 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
Bragadeesh153 17:1e1955f3db75 218 int singularity_flag=0;
lakshya 10:f93407b97750 219
lakshya 10:f93407b97750 220 if(controlmode1==0)
lakshya 10:f93407b97750 221 {
lakshya 10:f93407b97750 222 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
lakshya 10:f93407b97750 223 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
Bragadeesh153 17:1e1955f3db75 224 if (den==0)
lakshya 10:f93407b97750 225 {
Bragadeesh153 17:1e1955f3db75 226 singularity_flag=1;
lakshya 10:f93407b97750 227 }
Bragadeesh153 17:1e1955f3db75 228 if (singularity_flag==0)
lakshya 10:f93407b97750 229 {
Bragadeesh153 17:1e1955f3db75 230 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 231 {
Bragadeesh153 19:403cb36e22ed 232 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
Bragadeesh153 19:403cb36e22ed 233 }
Bragadeesh153 17:1e1955f3db75 234 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 235 {
Bragadeesh153 19:403cb36e22ed 236 b[i]/=den; // Mormalized b. Hence no unit.
Bragadeesh153 19:403cb36e22ed 237 }
Bragadeesh153 17:1e1955f3db75 238 if(b[2]>0.9 || b[2]<-0.9)
Bragadeesh153 19:403cb36e22ed 239 {
Bragadeesh153 19:403cb36e22ed 240 kz=kz2;
Bragadeesh153 19:403cb36e22ed 241 kmu=kmu2;
Bragadeesh153 19:403cb36e22ed 242 gamma=gamma2;
Bragadeesh153 19:403cb36e22ed 243 }
Bragadeesh153 17:1e1955f3db75 244 for(i=0;i<2;i++)
Bragadeesh153 19:403cb36e22ed 245 {
Bragadeesh153 19:403cb36e22ed 246 Mu[i]=b[i];
Bragadeesh153 19:403cb36e22ed 247 v[i]=-kmu*Mu[i];
Bragadeesh153 19:403cb36e22ed 248 dv[i]=-kmu*db[i];
Bragadeesh153 19:403cb36e22ed 249 z[i]=db[i]-v[i];
Bragadeesh153 19:403cb36e22ed 250 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
Bragadeesh153 19:403cb36e22ed 251 }
Bragadeesh153 17:1e1955f3db75 252 inverse(Jm,invJm,singularity_flag);
Bragadeesh153 17:1e1955f3db75 253 for(i=0;i<3;i++)
Bragadeesh153 17:1e1955f3db75 254 {
Bragadeesh153 19:403cb36e22ed 255 for(j=0;j<3;j++)
Bragadeesh153 19:403cb36e22ed 256 {
Bragadeesh153 19:403cb36e22ed 257 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
Bragadeesh153 19:403cb36e22ed 258 }
Bragadeesh153 17:1e1955f3db75 259 }
Bragadeesh153 17:1e1955f3db75 260 for(i=0;i<3;i++)
Bragadeesh153 17:1e1955f3db75 261 {
Bragadeesh153 19:403cb36e22ed 262 for(j=0;j<3;j++)
Bragadeesh153 19:403cb36e22ed 263 {
Bragadeesh153 19:403cb36e22ed 264 d[i]+=bb[j]*invJm[i][j];
Bragadeesh153 19:403cb36e22ed 265 }
Bragadeesh153 17:1e1955f3db75 266 }
Bragadeesh153 17:1e1955f3db75 267 bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
Bragadeesh153 17:1e1955f3db75 268 bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
Bragadeesh153 17:1e1955f3db75 269 bb[0]=0;
Bragadeesh153 17:1e1955f3db75 270 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 271 {
Bragadeesh153 19:403cb36e22ed 272 d[i]=invJm[2][i];
Bragadeesh153 19:403cb36e22ed 273 invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
Bragadeesh153 19:403cb36e22ed 274 invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
Bragadeesh153 19:403cb36e22ed 275 invJm[0][i]=b[i];
Bragadeesh153 19:403cb36e22ed 276 }
Bragadeesh153 17:1e1955f3db75 277 inverse(invJm,Jm,singularity_flag);
Bragadeesh153 17:1e1955f3db75 278 if (singularity_flag==0)
Bragadeesh153 17:1e1955f3db75 279 {
Bragadeesh153 19:403cb36e22ed 280 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 281 {
Bragadeesh153 19:403cb36e22ed 282 for(j=0;j<3;j++)
Bragadeesh153 19:403cb36e22ed 283 {
Bragadeesh153 19:403cb36e22ed 284 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
Bragadeesh153 19:403cb36e22ed 285 }
Bragadeesh153 19:403cb36e22ed 286 }
Bragadeesh153 19:403cb36e22ed 287 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 288 {
Bragadeesh153 19:403cb36e22ed 289 bcopy[i]=b[i]*den;
Bragadeesh153 19:403cb36e22ed 290 }
Bragadeesh153 19:403cb36e22ed 291 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 292 {
Bragadeesh153 19:403cb36e22ed 293 Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
Bragadeesh153 19:403cb36e22ed 294 Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
Bragadeesh153 19:403cb36e22ed 295 }
Bragadeesh153 17:1e1955f3db75 296 }
Bragadeesh153 19:403cb36e22ed 297
lakshya 10:f93407b97750 298 }
Bragadeesh153 19:403cb36e22ed 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 }
Bragadeesh153 19:403cb36e22ed 307 ACS_STATUS = 5;
lakshya 10:f93407b97750 308 }
Bragadeesh153 19:403cb36e22ed 309
lakshya 10:f93407b97750 310 else if(controlmode1==1)
lakshya 10:f93407b97750 311 {
Bragadeesh153 19:403cb36e22ed 312 if (ACS_DETUMBLING_ALGO_TYPE==0) // BOmega Algo
Bragadeesh153 17:1e1955f3db75 313 {
Bragadeesh153 19:403cb36e22ed 314 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 315 {
Bragadeesh153 19:403cb36e22ed 316 Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
Bragadeesh153 19:403cb36e22ed 317 }
Bragadeesh153 19:403cb36e22ed 318 ACS_STATUS = 6;
Bragadeesh153 17:1e1955f3db75 319 }
Bragadeesh153 19:403cb36e22ed 320 else if(ACS_DETUMBLING_ALGO_TYPE==1) // BDot Algo
Bragadeesh153 17:1e1955f3db75 321 {
Bragadeesh153 19:403cb36e22ed 322 for(i=0;i<3;i++)
Bragadeesh153 19:403cb36e22ed 323 {
Bragadeesh153 19:403cb36e22ed 324 Mmnt[i]=-kdetumble*db[i];
Bragadeesh153 19:403cb36e22ed 325 }
Bragadeesh153 19:403cb36e22ed 326 ACS_STATUS = 4;
Bragadeesh153 17:1e1955f3db75 327 }
lakshya 10:f93407b97750 328 }
Bragadeesh153 19:403cb36e22ed 329
lakshya 10:f93407b97750 330 for(i=0;i<3;i++)
lakshya 10:f93407b97750 331 {
lakshya 10:f93407b97750 332 moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
lakshya 10:f93407b97750 333 }
lakshya 10:f93407b97750 334 }
sakthipriya 0:7b4c00e3912f 335
sakthipriya 0:7b4c00e3912f 336 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 337
Bragadeesh153 16:cc77770d787f 338 int FCTN_ACS_INIT(); //initialization of registers happens
Bragadeesh153 16:cc77770d787f 339 int SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 340 int FCTN_ATS_DATA_ACQ(); //data is obtained
Bragadeesh153 16:cc77770d787f 341 int SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 342
Bragadeesh153 17:1e1955f3db75 343 int CONFIG_UPLOAD();
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
sakthipriya 0:7b4c00e3912f 347
sakthipriya 0:7b4c00e3912f 348 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 349 char cmd[2];
sakthipriya 0:7b4c00e3912f 350 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 351 char raw_mag[6];
Bragadeesh153 17:1e1955f3db75 352 char reg_data[24];
sakthipriya 0:7b4c00e3912f 353 char store,status;
sakthipriya 0:7b4c00e3912f 354 int16_t bit_data;
Bragadeesh153 17:1e1955f3db75 355 uint16_t time_data;
Bragadeesh153 17:1e1955f3db75 356 float gyro_data[3], mag_data[3];
Bragadeesh153 19:403cb36e22ed 357
Bragadeesh153 19:403cb36e22ed 358
Bragadeesh153 19:403cb36e22ed 359 int ack;
sakthipriya 0:7b4c00e3912f 360
Bragadeesh153 17:1e1955f3db75 361 int CONFIG_UPLOAD()
sakthipriya 0:7b4c00e3912f 362 {
sakthipriya 0:7b4c00e3912f 363 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 364 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 365 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
Bragadeesh153 16:cc77770d787f 366 wait_ms(600);
Bragadeesh153 16:cc77770d787f 367
Bragadeesh153 16:cc77770d787f 368 //Verify magic number
Bragadeesh153 16:cc77770d787f 369
Bragadeesh153 16:cc77770d787f 370 cmd[0]=HOST_CTRL; //0x02 is written in HOST CONTROL register to enable upload
Bragadeesh153 16:cc77770d787f 371 cmd[1]=BIT_HOST_UPLD_ENB;
Bragadeesh153 16:cc77770d787f 372 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 373 wait_ms(100);
Bragadeesh153 16:cc77770d787f 374
Bragadeesh153 16:cc77770d787f 375 cmd[0]=UPLOAD_ADDR; //0x02 is written in HOST CONTROL register to enable upload
Bragadeesh153 16:cc77770d787f 376 cmd[1]=0x0000;
Bragadeesh153 16:cc77770d787f 377 i2c.write(SLAVE_ADDR,cmd,3);
Bragadeesh153 16:cc77770d787f 378 wait_ms(100);
Bragadeesh153 16:cc77770d787f 379
Bragadeesh153 16:cc77770d787f 380
Bragadeesh153 16:cc77770d787f 381
Bragadeesh153 16:cc77770d787f 382
Bragadeesh153 16:cc77770d787f 383 cmd[0]=HOST_CTRL; //0x00 is written in HOST CONTROL register to free upload
Bragadeesh153 16:cc77770d787f 384 cmd[1]=0x00;
Bragadeesh153 16:cc77770d787f 385 i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 386 wait_ms(100);
Bragadeesh153 16:cc77770d787f 387
Bragadeesh153 17:1e1955f3db75 388 return 0;
Bragadeesh153 16:cc77770d787f 389
Bragadeesh153 16:cc77770d787f 390 }
Bragadeesh153 16:cc77770d787f 391
Bragadeesh153 19:403cb36e22ed 392
Bragadeesh153 16:cc77770d787f 393 int SENSOR_INIT()
Bragadeesh153 16:cc77770d787f 394 {
Bragadeesh153 16:cc77770d787f 395
Bragadeesh153 16:cc77770d787f 396 pc_acs.printf("Entered sensor init\n \r");
Bragadeesh153 16:cc77770d787f 397 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 398 cmd[1]=BIT_RESREQ;
Bragadeesh153 19:403cb36e22ed 399 wait(1);
Bragadeesh153 19:403cb36e22ed 400 ack = i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
Bragadeesh153 19:403cb36e22ed 401
Bragadeesh153 19:403cb36e22ed 402 pc_acs.printf("ACK for reset is %d\r\n",ack); //waiting for loading configuration file stored in EEPROM
Bragadeesh153 19:403cb36e22ed 403
Bragadeesh153 19:403cb36e22ed 404 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 405 {
Bragadeesh153 19:403cb36e22ed 406 cmd[0]=RESETREQ;
Bragadeesh153 19:403cb36e22ed 407 cmd[1]=BIT_RESREQ;
Bragadeesh153 19:403cb36e22ed 408 ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
Bragadeesh153 19:403cb36e22ed 409 if(ack !=0)
Bragadeesh153 19:403cb36e22ed 410 return 0;
Bragadeesh153 19:403cb36e22ed 411 }
Bragadeesh153 19:403cb36e22ed 412
Bragadeesh153 19:403cb36e22ed 413 wait_ms(600);
Bragadeesh153 19:403cb36e22ed 414
sakthipriya 0:7b4c00e3912f 415 cmd[0]=SENTRALSTATUS;
Bragadeesh153 19:403cb36e22ed 416 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 417
Bragadeesh153 19:403cb36e22ed 418 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 419 {
Bragadeesh153 19:403cb36e22ed 420 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 421 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 422 return 0;
Bragadeesh153 19:403cb36e22ed 423 }
Bragadeesh153 19:403cb36e22ed 424 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 425
Bragadeesh153 19:403cb36e22ed 426 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 427 {
Bragadeesh153 19:403cb36e22ed 428 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 429 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 430 return 0;
Bragadeesh153 19:403cb36e22ed 431 }
Bragadeesh153 19:403cb36e22ed 432
Bragadeesh153 19:403cb36e22ed 433
Bragadeesh153 17:1e1955f3db75 434
Bragadeesh153 16:cc77770d787f 435 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
Bragadeesh153 16:cc77770d787f 436
Bragadeesh153 17:1e1955f3db75 437 //to check whether EEPROM is uploaded properly
sakthipriya 0:7b4c00e3912f 438 switch((int)store) {
sakthipriya 0:7b4c00e3912f 439 case(3): {
sakthipriya 0:7b4c00e3912f 440 break;
sakthipriya 0:7b4c00e3912f 441 }
sakthipriya 0:7b4c00e3912f 442 case(11): {
sakthipriya 0:7b4c00e3912f 443 break;
sakthipriya 0:7b4c00e3912f 444 }
sakthipriya 0:7b4c00e3912f 445 default: {
sakthipriya 0:7b4c00e3912f 446 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 447 cmd[1]=BIT_RESREQ;
Bragadeesh153 19:403cb36e22ed 448 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 449 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 450 {
Bragadeesh153 19:403cb36e22ed 451 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 452 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 453 return 0;
Bragadeesh153 19:403cb36e22ed 454 }
Bragadeesh153 16:cc77770d787f 455 wait_ms(600);
Bragadeesh153 16:cc77770d787f 456
Bragadeesh153 17:1e1955f3db75 457 cmd[0]=SENTRALSTATUS;
Bragadeesh153 19:403cb36e22ed 458 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 459
Bragadeesh153 19:403cb36e22ed 460 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 461 {
Bragadeesh153 19:403cb36e22ed 462 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 463 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 464 return 0;
Bragadeesh153 19:403cb36e22ed 465 }
Bragadeesh153 19:403cb36e22ed 466 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 467
Bragadeesh153 19:403cb36e22ed 468 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 469 {
Bragadeesh153 19:403cb36e22ed 470 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 471 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 472 return 0;
Bragadeesh153 19:403cb36e22ed 473 }
Bragadeesh153 19:403cb36e22ed 474
Bragadeesh153 17:1e1955f3db75 475 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
Bragadeesh153 17:1e1955f3db75 476
sakthipriya 0:7b4c00e3912f 477 }
sakthipriya 0:7b4c00e3912f 478 }
Bragadeesh153 17:1e1955f3db75 479
Bragadeesh153 16:cc77770d787f 480
Bragadeesh153 16:cc77770d787f 481 int manual=0;
Bragadeesh153 19:403cb36e22ed 482
Bragadeesh153 19:403cb36e22ed 483 if( ((int)store != 11 )&&((int)store != 3))
Bragadeesh153 16:cc77770d787f 484 {
Bragadeesh153 16:cc77770d787f 485
Bragadeesh153 16:cc77770d787f 486 cmd[0]=RESETREQ;
Bragadeesh153 16:cc77770d787f 487 cmd[1]=BIT_RESREQ;
Bragadeesh153 19:403cb36e22ed 488 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 489 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 490 {
Bragadeesh153 19:403cb36e22ed 491 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 492 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 493 return 0;
Bragadeesh153 19:403cb36e22ed 494 }
Bragadeesh153 16:cc77770d787f 495 wait_ms(600);
Bragadeesh153 16:cc77770d787f 496
Bragadeesh153 17:1e1955f3db75 497 manual = CONFIG_UPLOAD();
Bragadeesh153 16:cc77770d787f 498
Bragadeesh153 16:cc77770d787f 499 if(manual == 0)
Bragadeesh153 16:cc77770d787f 500 {
Bragadeesh153 17:1e1955f3db75 501 //MANUAL CONFIGURATION FAILED
Bragadeesh153 16:cc77770d787f 502 return 0;
Bragadeesh153 16:cc77770d787f 503 }
Bragadeesh153 16:cc77770d787f 504
Bragadeesh153 16:cc77770d787f 505 }
Bragadeesh153 16:cc77770d787f 506
Bragadeesh153 16:cc77770d787f 507 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
Bragadeesh153 16:cc77770d787f 508 cmd[1]=BIT_RUN_ENB;
Bragadeesh153 19:403cb36e22ed 509 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 510
Bragadeesh153 19:403cb36e22ed 511 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 512 {
Bragadeesh153 19:403cb36e22ed 513 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 514 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 515 return 0;
Bragadeesh153 19:403cb36e22ed 516 }
Bragadeesh153 16:cc77770d787f 517
Bragadeesh153 16:cc77770d787f 518 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
Bragadeesh153 16:cc77770d787f 519 cmd[1]=BIT_MAGODR;
Bragadeesh153 19:403cb36e22ed 520 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 521 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 522 {
Bragadeesh153 19:403cb36e22ed 523 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 524 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 525 return 0;
Bragadeesh153 19:403cb36e22ed 526 }
Bragadeesh153 17:1e1955f3db75 527
Bragadeesh153 16:cc77770d787f 528 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
Bragadeesh153 16:cc77770d787f 529 cmd[1]=BIT_GYROODR;
Bragadeesh153 19:403cb36e22ed 530 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 531
Bragadeesh153 19:403cb36e22ed 532 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 533 {
Bragadeesh153 19:403cb36e22ed 534 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 535 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 536 return 0;
Bragadeesh153 19:403cb36e22ed 537 }
Bragadeesh153 17:1e1955f3db75 538
Bragadeesh153 16:cc77770d787f 539 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
Bragadeesh153 19:403cb36e22ed 540 cmd[1]=0x00;
Bragadeesh153 19:403cb36e22ed 541 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 16:cc77770d787f 542
Bragadeesh153 19:403cb36e22ed 543 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 544 {
Bragadeesh153 19:403cb36e22ed 545 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 546 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 547 return 0;
Bragadeesh153 19:403cb36e22ed 548 }
Bragadeesh153 19:403cb36e22ed 549
Bragadeesh153 17:1e1955f3db75 550 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
Bragadeesh153 16:cc77770d787f 551 cmd[1]=0x00;
Bragadeesh153 19:403cb36e22ed 552 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 553
Bragadeesh153 19:403cb36e22ed 554 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 555 {
Bragadeesh153 19:403cb36e22ed 556 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 557 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 558 return 0;
Bragadeesh153 19:403cb36e22ed 559 }
Bragadeesh153 17:1e1955f3db75 560
Bragadeesh153 17:1e1955f3db75 561 cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
Bragadeesh153 16:cc77770d787f 562 cmd[1]=BIT_EVT_ENB;
Bragadeesh153 19:403cb36e22ed 563 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 564 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 565 {
Bragadeesh153 19:403cb36e22ed 566 ack = i2c.write(SLAVE_ADDR,cmd,2);
Bragadeesh153 19:403cb36e22ed 567 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 568 return 0;
Bragadeesh153 19:403cb36e22ed 569 }
Bragadeesh153 17:1e1955f3db75 570
Bragadeesh153 17:1e1955f3db75 571 cmd[0]=SENTRALSTATUS;
Bragadeesh153 19:403cb36e22ed 572 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 573
Bragadeesh153 19:403cb36e22ed 574 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 575 {
Bragadeesh153 19:403cb36e22ed 576 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 577 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 578 return 0;
Bragadeesh153 19:403cb36e22ed 579 }
Bragadeesh153 19:403cb36e22ed 580
Bragadeesh153 19:403cb36e22ed 581 ack= i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 582 if( ack!=0)
Bragadeesh153 19:403cb36e22ed 583 {
Bragadeesh153 19:403cb36e22ed 584 ack= i2c.read(SLAVE_ADDR_READ,&store,1);
Bragadeesh153 19:403cb36e22ed 585 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 586 return 0;
Bragadeesh153 19:403cb36e22ed 587 }
Bragadeesh153 19:403cb36e22ed 588
Bragadeesh153 19:403cb36e22ed 589
Bragadeesh153 17:1e1955f3db75 590 pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
Bragadeesh153 17:1e1955f3db75 591
Bragadeesh153 17:1e1955f3db75 592 if( (int)store == 3) //Check if initialised properly and not in idle state
Bragadeesh153 17:1e1955f3db75 593 {
Bragadeesh153 17:1e1955f3db75 594 pc_acs.printf("Exited sensor init successfully\n \r");
Bragadeesh153 17:1e1955f3db75 595 return 1;
Bragadeesh153 17:1e1955f3db75 596 }
Bragadeesh153 19:403cb36e22ed 597
Bragadeesh153 17:1e1955f3db75 598 pc_acs.printf("Sensor init failed \n \r") ;
Bragadeesh153 17:1e1955f3db75 599 return 0;
Bragadeesh153 17:1e1955f3db75 600 }
Bragadeesh153 16:cc77770d787f 601
Bragadeesh153 16:cc77770d787f 602 int FCTN_ACS_INIT()
Bragadeesh153 16:cc77770d787f 603 {
Bragadeesh153 16:cc77770d787f 604 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
Bragadeesh153 17:1e1955f3db75 605
Bragadeesh153 16:cc77770d787f 606
Bragadeesh153 16:cc77770d787f 607 int working=0;
Bragadeesh153 16:cc77770d787f 608
Bragadeesh153 16:cc77770d787f 609 pc_acs.printf("Attitude sensor init called \n \r");
Bragadeesh153 16:cc77770d787f 610 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 611
Bragadeesh153 16:cc77770d787f 612
Bragadeesh153 19:403cb36e22ed 613 if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
Bragadeesh153 16:cc77770d787f 614 {
Bragadeesh153 16:cc77770d787f 615
Bragadeesh153 16:cc77770d787f 616 pc_acs.printf("Sensor 1 marked working \n \r");
Bragadeesh153 16:cc77770d787f 617 working = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 618 if(working ==1)
Bragadeesh153 16:cc77770d787f 619 {
Bragadeesh153 19:403cb36e22ed 620 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
Bragadeesh153 17:1e1955f3db75 621 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
Bragadeesh153 16:cc77770d787f 622 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
Bragadeesh153 16:cc77770d787f 623 ACS_INIT_STATUS = 0;
Bragadeesh153 16:cc77770d787f 624 return 1;
Bragadeesh153 16:cc77770d787f 625 }
Bragadeesh153 16:cc77770d787f 626
Bragadeesh153 19:403cb36e22ed 627
Bragadeesh153 17:1e1955f3db75 628 pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
Bragadeesh153 16:cc77770d787f 629 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 630 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
Bragadeesh153 16:cc77770d787f 631
Bragadeesh153 16:cc77770d787f 632 }
Bragadeesh153 16:cc77770d787f 633
Bragadeesh153 16:cc77770d787f 634 pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
Bragadeesh153 16:cc77770d787f 635
Bragadeesh153 17:1e1955f3db75 636 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
Bragadeesh153 16:cc77770d787f 637 {
Bragadeesh153 16:cc77770d787f 638
Bragadeesh153 16:cc77770d787f 639
Bragadeesh153 16:cc77770d787f 640 ATS2_SW_ENABLE = 0;
Bragadeesh153 16:cc77770d787f 641 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 642
Bragadeesh153 16:cc77770d787f 643 working = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 644 if(working ==1)
Bragadeesh153 16:cc77770d787f 645 {
Bragadeesh153 16:cc77770d787f 646 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 17:1e1955f3db75 647 pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
Bragadeesh153 19:403cb36e22ed 648 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
Bragadeesh153 16:cc77770d787f 649 ACS_INIT_STATUS = 0;
Bragadeesh153 16:cc77770d787f 650 return 2;
Bragadeesh153 16:cc77770d787f 651 }
Bragadeesh153 19:403cb36e22ed 652
Bragadeesh153 19:403cb36e22ed 653 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 654 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 655
Bragadeesh153 19:403cb36e22ed 656 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
Bragadeesh153 16:cc77770d787f 657
Bragadeesh153 16:cc77770d787f 658
Bragadeesh153 16:cc77770d787f 659 }
Bragadeesh153 16:cc77770d787f 660
Bragadeesh153 16:cc77770d787f 661 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 662 pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
Bragadeesh153 16:cc77770d787f 663
Bragadeesh153 16:cc77770d787f 664
Bragadeesh153 16:cc77770d787f 665
Bragadeesh153 19:403cb36e22ed 666
Bragadeesh153 19:403cb36e22ed 667
Bragadeesh153 17:1e1955f3db75 668 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
Bragadeesh153 16:cc77770d787f 669 return 0;
sakthipriya 0:7b4c00e3912f 670 }
sakthipriya 0:7b4c00e3912f 671
Bragadeesh153 16:cc77770d787f 672
Bragadeesh153 16:cc77770d787f 673 int SENSOR_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 674 {
Bragadeesh153 19:403cb36e22ed 675
Bragadeesh153 16:cc77770d787f 676 pc_acs.printf("Entering Sensor data acq.\n \r");
Bragadeesh153 17:1e1955f3db75 677 char status;
Bragadeesh153 17:1e1955f3db75 678 int sentral;
Bragadeesh153 16:cc77770d787f 679 int event;
Bragadeesh153 16:cc77770d787f 680 int sensor;
Bragadeesh153 16:cc77770d787f 681 int error;
Bragadeesh153 17:1e1955f3db75 682 int init;
Bragadeesh153 16:cc77770d787f 683
Bragadeesh153 19:403cb36e22ed 684 uint8_t gyro_error=0;
Bragadeesh153 19:403cb36e22ed 685 uint8_t mag_error=0;
Bragadeesh153 19:403cb36e22ed 686
Bragadeesh153 17:1e1955f3db75 687
sakthipriya 0:7b4c00e3912f 688 cmd[0]=EVT_STATUS;
Bragadeesh153 19:403cb36e22ed 689
Bragadeesh153 19:403cb36e22ed 690 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 691 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 692 {
Bragadeesh153 19:403cb36e22ed 693 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 694 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 695 return 0;
Bragadeesh153 19:403cb36e22ed 696 }
Bragadeesh153 19:403cb36e22ed 697 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 698 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 699 {
Bragadeesh153 19:403cb36e22ed 700 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 701 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 702 return 0;
Bragadeesh153 19:403cb36e22ed 703 }
Bragadeesh153 19:403cb36e22ed 704
Bragadeesh153 17:1e1955f3db75 705 event = (int)status;
Bragadeesh153 17:1e1955f3db75 706
Bragadeesh153 17:1e1955f3db75 707 cmd[0]=SENTRALSTATUS;
Bragadeesh153 19:403cb36e22ed 708 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 709 if(ack!=0)
Bragadeesh153 17:1e1955f3db75 710 {
Bragadeesh153 19:403cb36e22ed 711 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 712 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 713 return 0;
Bragadeesh153 17:1e1955f3db75 714 }
Bragadeesh153 19:403cb36e22ed 715 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 716 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 717 {
Bragadeesh153 19:403cb36e22ed 718 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 719 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 720 return 0;
Bragadeesh153 19:403cb36e22ed 721 }
Bragadeesh153 19:403cb36e22ed 722
Bragadeesh153 19:403cb36e22ed 723
Bragadeesh153 16:cc77770d787f 724
Bragadeesh153 17:1e1955f3db75 725 sentral = (int) status;
Bragadeesh153 17:1e1955f3db75 726
Bragadeesh153 17:1e1955f3db75 727 pc_acs.printf("Event Status is %x\n \r",event);
Bragadeesh153 17:1e1955f3db75 728 pc_acs.printf("Sentral Status is %x\n \r",sentral);
Bragadeesh153 17:1e1955f3db75 729
Bragadeesh153 17:1e1955f3db75 730
Bragadeesh153 17:1e1955f3db75 731
Bragadeesh153 17:1e1955f3db75 732 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 733 {
Bragadeesh153 16:cc77770d787f 734
Bragadeesh153 16:cc77770d787f 735
Bragadeesh153 17:1e1955f3db75 736 init = SENSOR_INIT();
Bragadeesh153 16:cc77770d787f 737
Bragadeesh153 17:1e1955f3db75 738
Bragadeesh153 16:cc77770d787f 739 cmd[0]=EVT_STATUS;
Bragadeesh153 19:403cb36e22ed 740
Bragadeesh153 19:403cb36e22ed 741 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 742 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 743 {
Bragadeesh153 19:403cb36e22ed 744 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 745 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 746 return 0;
Bragadeesh153 19:403cb36e22ed 747 }
Bragadeesh153 19:403cb36e22ed 748 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 749 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 750 {
Bragadeesh153 19:403cb36e22ed 751 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 752 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 753 return 0;
Bragadeesh153 19:403cb36e22ed 754 }
Bragadeesh153 19:403cb36e22ed 755
Bragadeesh153 17:1e1955f3db75 756 event = (int)status;
Bragadeesh153 16:cc77770d787f 757
Bragadeesh153 17:1e1955f3db75 758 cmd[0]=SENTRALSTATUS;
Bragadeesh153 19:403cb36e22ed 759 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 760 if(ack!=0)
Bragadeesh153 16:cc77770d787f 761 {
Bragadeesh153 19:403cb36e22ed 762 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 763 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 764 return 0;
Bragadeesh153 17:1e1955f3db75 765 }
Bragadeesh153 19:403cb36e22ed 766 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 767 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 768 {
Bragadeesh153 19:403cb36e22ed 769 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 770 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 771 return 0;
Bragadeesh153 19:403cb36e22ed 772 }
Bragadeesh153 19:403cb36e22ed 773
Bragadeesh153 19:403cb36e22ed 774
Bragadeesh153 19:403cb36e22ed 775
Bragadeesh153 19:403cb36e22ed 776 sentral = (int) status;
Bragadeesh153 17:1e1955f3db75 777
Bragadeesh153 17:1e1955f3db75 778 pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
Bragadeesh153 17:1e1955f3db75 779
Bragadeesh153 17:1e1955f3db75 780 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 19:403cb36e22ed 781 {
Bragadeesh153 17:1e1955f3db75 782
Bragadeesh153 19:403cb36e22ed 783 cmd[0]=ERROR;
Bragadeesh153 19:403cb36e22ed 784
Bragadeesh153 19:403cb36e22ed 785 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 786 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 787 {
Bragadeesh153 19:403cb36e22ed 788 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 789 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 790 return 0;
Bragadeesh153 19:403cb36e22ed 791 }
Bragadeesh153 19:403cb36e22ed 792 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 793 if(ack!=0)
Bragadeesh153 16:cc77770d787f 794 {
Bragadeesh153 19:403cb36e22ed 795 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 796 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 797 return 0;
Bragadeesh153 16:cc77770d787f 798 }
Bragadeesh153 19:403cb36e22ed 799
Bragadeesh153 19:403cb36e22ed 800 error = (int)status;
Bragadeesh153 17:1e1955f3db75 801
Bragadeesh153 19:403cb36e22ed 802 cmd[0]=SENSORSTATUS;
Bragadeesh153 19:403cb36e22ed 803 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 804 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 805 {
Bragadeesh153 19:403cb36e22ed 806 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 807 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 808 return 0;
Bragadeesh153 19:403cb36e22ed 809 }
Bragadeesh153 19:403cb36e22ed 810 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 811 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 812 {
Bragadeesh153 19:403cb36e22ed 813 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 814 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 815 return 0;
Bragadeesh153 19:403cb36e22ed 816 }
Bragadeesh153 19:403cb36e22ed 817
Bragadeesh153 19:403cb36e22ed 818
Bragadeesh153 19:403cb36e22ed 819
Bragadeesh153 19:403cb36e22ed 820 sensor = (int) status;
Bragadeesh153 19:403cb36e22ed 821
Bragadeesh153 16:cc77770d787f 822
Bragadeesh153 16:cc77770d787f 823
Bragadeesh153 19:403cb36e22ed 824 if((error!=0) || (sensor!=0))
Bragadeesh153 19:403cb36e22ed 825 {
Bragadeesh153 19:403cb36e22ed 826 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
Bragadeesh153 19:403cb36e22ed 827 {
Bragadeesh153 19:403cb36e22ed 828 pc_acs.printf("error in gyro alone..\n \r");
Bragadeesh153 19:403cb36e22ed 829 gyro_error = 1;
Bragadeesh153 19:403cb36e22ed 830 }
Bragadeesh153 19:403cb36e22ed 831
Bragadeesh153 19:403cb36e22ed 832 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
Bragadeesh153 19:403cb36e22ed 833 {
Bragadeesh153 19:403cb36e22ed 834
Bragadeesh153 19:403cb36e22ed 835 pc_acs.printf("error in mag alone.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 836 mag_error = 1;
Bragadeesh153 19:403cb36e22ed 837 }
Bragadeesh153 19:403cb36e22ed 838
Bragadeesh153 19:403cb36e22ed 839 if( (gyro_error!=1)&&(mag_error!=1))
Bragadeesh153 19:403cb36e22ed 840 {
Bragadeesh153 19:403cb36e22ed 841 pc_acs.printf("error in something else.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 842 return 0;
Bragadeesh153 19:403cb36e22ed 843
Bragadeesh153 19:403cb36e22ed 844 }
Bragadeesh153 19:403cb36e22ed 845 }
Bragadeesh153 19:403cb36e22ed 846
Bragadeesh153 19:403cb36e22ed 847 if((event & 1 == 1 ))
Bragadeesh153 19:403cb36e22ed 848 {
Bragadeesh153 19:403cb36e22ed 849 pc_acs.printf("error in CPU Reset.\n \r");
Bragadeesh153 19:403cb36e22ed 850 return 1;
Bragadeesh153 19:403cb36e22ed 851
Bragadeesh153 19:403cb36e22ed 852 }
Bragadeesh153 19:403cb36e22ed 853
Bragadeesh153 19:403cb36e22ed 854 if((event & 8 != 8 )||(event & 32 != 32 ))
Bragadeesh153 19:403cb36e22ed 855 {
Bragadeesh153 19:403cb36e22ed 856 pc_acs.printf("Data not ready waiting...\n \r");
Bragadeesh153 19:403cb36e22ed 857 //POLL
Bragadeesh153 19:403cb36e22ed 858 wait_ms(200);
Bragadeesh153 19:403cb36e22ed 859
Bragadeesh153 19:403cb36e22ed 860 cmd[0]=EVT_STATUS;
Bragadeesh153 19:403cb36e22ed 861
Bragadeesh153 19:403cb36e22ed 862 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 863 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 864 {
Bragadeesh153 19:403cb36e22ed 865 ack = i2c.write(SLAVE_ADDR,cmd,1);
Bragadeesh153 19:403cb36e22ed 866 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 867 return 0;
Bragadeesh153 19:403cb36e22ed 868 }
Bragadeesh153 19:403cb36e22ed 869 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 870 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 871 {
Bragadeesh153 19:403cb36e22ed 872 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
Bragadeesh153 19:403cb36e22ed 873 if(ack!=0)
Bragadeesh153 19:403cb36e22ed 874 return 0;
Bragadeesh153 19:403cb36e22ed 875 }
Bragadeesh153 19:403cb36e22ed 876
Bragadeesh153 19:403cb36e22ed 877 event = (int)status;
Bragadeesh153 19:403cb36e22ed 878 if(event & 32 != 32 )
Bragadeesh153 19:403cb36e22ed 879 {
Bragadeesh153 16:cc77770d787f 880
Bragadeesh153 19:403cb36e22ed 881 pc_acs.printf("Mag data only ready.Read..\n \r");
Bragadeesh153 19:403cb36e22ed 882 gyro_error = 1;
Bragadeesh153 19:403cb36e22ed 883
Bragadeesh153 19:403cb36e22ed 884 }
Bragadeesh153 19:403cb36e22ed 885
Bragadeesh153 19:403cb36e22ed 886 if(event & 8 != 8 )
Bragadeesh153 19:403cb36e22ed 887 {
Bragadeesh153 19:403cb36e22ed 888 pc_acs.printf("Both data still not ready.Exiting..\n \r");
Bragadeesh153 19:403cb36e22ed 889 mag_error=1;
Bragadeesh153 19:403cb36e22ed 890 }
Bragadeesh153 19:403cb36e22ed 891
Bragadeesh153 19:403cb36e22ed 892
Bragadeesh153 19:403cb36e22ed 893
Bragadeesh153 19:403cb36e22ed 894
Bragadeesh153 19:403cb36e22ed 895 }
Bragadeesh153 19:403cb36e22ed 896
Bragadeesh153 19:403cb36e22ed 897
Bragadeesh153 19:403cb36e22ed 898 }
Bragadeesh153 19:403cb36e22ed 899
Bragadeesh153 19:403cb36e22ed 900 if((mag_error !=1)&&(gyro_error!=1))
Bragadeesh153 19:403cb36e22ed 901 {
Bragadeesh153 19:403cb36e22ed 902 pc_acs.printf("Error in something else.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 903 return 0;
Bragadeesh153 19:403cb36e22ed 904 }
Bragadeesh153 19:403cb36e22ed 905
Bragadeesh153 19:403cb36e22ed 906 if((mag_error !=1)&&(gyro_error!=1))
Bragadeesh153 19:403cb36e22ed 907 {
Bragadeesh153 19:403cb36e22ed 908 pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 909 return 0;
Bragadeesh153 19:403cb36e22ed 910 }
Bragadeesh153 17:1e1955f3db75 911
Bragadeesh153 19:403cb36e22ed 912 }
Bragadeesh153 17:1e1955f3db75 913
Bragadeesh153 16:cc77770d787f 914
Bragadeesh153 16:cc77770d787f 915
Bragadeesh153 16:cc77770d787f 916
Bragadeesh153 16:cc77770d787f 917
Bragadeesh153 19:403cb36e22ed 918
Bragadeesh153 16:cc77770d787f 919
Bragadeesh153 17:1e1955f3db75 920
Bragadeesh153 17:1e1955f3db75 921
sakthipriya 0:7b4c00e3912f 922 cmd[0]=MAG_XOUT_H; //LSB of x
Bragadeesh153 17:1e1955f3db75 923 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
Bragadeesh153 19:403cb36e22ed 924 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
Bragadeesh153 17:1e1955f3db75 925
Bragadeesh153 19:403cb36e22ed 926 if(ack != 0)
Bragadeesh153 17:1e1955f3db75 927 {
Bragadeesh153 17:1e1955f3db75 928 wait_ms(100);
Bragadeesh153 17:1e1955f3db75 929 cmd[0]=MAG_XOUT_H; //LSB of x
Bragadeesh153 17:1e1955f3db75 930 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
Bragadeesh153 19:403cb36e22ed 931 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
Bragadeesh153 17:1e1955f3db75 932 wait_ms(100);
Bragadeesh153 19:403cb36e22ed 933 if(ack !=1)
Bragadeesh153 19:403cb36e22ed 934 return 0;
Bragadeesh153 19:403cb36e22ed 935
Bragadeesh153 17:1e1955f3db75 936 }
Bragadeesh153 17:1e1955f3db75 937
Bragadeesh153 17:1e1955f3db75 938
sakthipriya 0:7b4c00e3912f 939 // pc_acs.printf("\nGyro Values:\n");
Bragadeesh153 19:403cb36e22ed 940
Bragadeesh153 19:403cb36e22ed 941 if (gyro_error!=1)
Bragadeesh153 19:403cb36e22ed 942 {
Bragadeesh153 19:403cb36e22ed 943 for(int i=0; i<3; i++) {
Bragadeesh153 19:403cb36e22ed 944 //concatenating gyro LSB and MSB to get 16 bit signed data values
Bragadeesh153 19:403cb36e22ed 945 bit_data= ((int16_t)reg_data[16+2*i+1]<<8)|(int16_t)reg_data[16+2*i];
Bragadeesh153 19:403cb36e22ed 946 gyro_data[i]=(float)bit_data;
Bragadeesh153 19:403cb36e22ed 947 gyro_data[i]=gyro_data[i]/senstivity_gyro;
Bragadeesh153 19:403cb36e22ed 948 actual_data.AngularSpeed_actual[i] = gyro_data[i];
Bragadeesh153 19:403cb36e22ed 949 }
sakthipriya 0:7b4c00e3912f 950 }
Bragadeesh153 17:1e1955f3db75 951
Bragadeesh153 19:403cb36e22ed 952
Bragadeesh153 19:403cb36e22ed 953 if(mag_error!=1){
Bragadeesh153 19:403cb36e22ed 954 for(int i=0; i<3; i++) {
Bragadeesh153 19:403cb36e22ed 955 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
Bragadeesh153 19:403cb36e22ed 956 bit_data= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
Bragadeesh153 19:403cb36e22ed 957 mag_data[i]=(float)bit_data;
Bragadeesh153 19:403cb36e22ed 958 mag_data[i]=mag_data[i]/senstivity_mag;
Bragadeesh153 19:403cb36e22ed 959 actual_data.Bvalue_actual[i] = mag_data[i];
Bragadeesh153 19:403cb36e22ed 960 }
sakthipriya 0:7b4c00e3912f 961 }
Bragadeesh153 19:403cb36e22ed 962
Bragadeesh153 19:403cb36e22ed 963
Bragadeesh153 17:1e1955f3db75 964
Bragadeesh153 16:cc77770d787f 965
Bragadeesh153 19:403cb36e22ed 966 if(mag_error == 1)
Bragadeesh153 16:cc77770d787f 967 {
Bragadeesh153 16:cc77770d787f 968
Bragadeesh153 19:403cb36e22ed 969 pc_acs.printf("Gyro only successful.\n \r");
Bragadeesh153 19:403cb36e22ed 970 return 1;
Bragadeesh153 16:cc77770d787f 971 }
Bragadeesh153 19:403cb36e22ed 972 if(gyro_error == 1)
Bragadeesh153 16:cc77770d787f 973 {
Bragadeesh153 19:403cb36e22ed 974 pc_acs.printf("Mag only successful.\n \r");
Bragadeesh153 19:403cb36e22ed 975 return 2;
sakthipriya 0:7b4c00e3912f 976 }
Bragadeesh153 16:cc77770d787f 977
Bragadeesh153 19:403cb36e22ed 978 pc_acs.printf("Reading data success.\n \r");
Bragadeesh153 19:403cb36e22ed 979 return 3;
Bragadeesh153 16:cc77770d787f 980
Bragadeesh153 16:cc77770d787f 981 }
Bragadeesh153 16:cc77770d787f 982
Bragadeesh153 16:cc77770d787f 983
Bragadeesh153 16:cc77770d787f 984
Bragadeesh153 16:cc77770d787f 985 int FCTN_ATS_DATA_ACQ()
Bragadeesh153 16:cc77770d787f 986 {
Bragadeesh153 16:cc77770d787f 987
Bragadeesh153 19:403cb36e22ed 988 for(int i=0; i<3; i++) {
Bragadeesh153 19:403cb36e22ed 989 actual_data.AngularSpeed_actual[i] = 0;
Bragadeesh153 19:403cb36e22ed 990 actual_data.Bvalue_actual[i] = 0;
Bragadeesh153 19:403cb36e22ed 991 }
Bragadeesh153 19:403cb36e22ed 992
Bragadeesh153 16:cc77770d787f 993 int acq;
Bragadeesh153 19:403cb36e22ed 994 int init;
Bragadeesh153 16:cc77770d787f 995
Bragadeesh153 16:cc77770d787f 996 pc_acs.printf("DATA_ACQ called \n \r");
Bragadeesh153 16:cc77770d787f 997 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 998
Bragadeesh153 16:cc77770d787f 999
Bragadeesh153 16:cc77770d787f 1000
Bragadeesh153 19:403cb36e22ed 1001 if(( (ACS_ATS_STATUS & 0xC0) == 0x40)) //ATS1 status is 01XX to check if ATS1 is ON
Bragadeesh153 19:403cb36e22ed 1002 {
Bragadeesh153 19:403cb36e22ed 1003
Bragadeesh153 19:403cb36e22ed 1004 acq = SENSOR_DATA_ACQ(); //ATS1 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
Bragadeesh153 19:403cb36e22ed 1005
Bragadeesh153 19:403cb36e22ed 1006 if(acq == 3) //Both available read and exit
Bragadeesh153 19:403cb36e22ed 1007 {
Bragadeesh153 19:403cb36e22ed 1008
Bragadeesh153 19:403cb36e22ed 1009 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
Bragadeesh153 19:403cb36e22ed 1010
Bragadeesh153 19:403cb36e22ed 1011 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 19:403cb36e22ed 1012 pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
Bragadeesh153 19:403cb36e22ed 1013 return 3;
Bragadeesh153 19:403cb36e22ed 1014 }
Bragadeesh153 19:403cb36e22ed 1015 else if((acq == 2)||(acq==1)) //Only mag or only gyro
Bragadeesh153 19:403cb36e22ed 1016 {
Bragadeesh153 19:403cb36e22ed 1017
Bragadeesh153 19:403cb36e22ed 1018 pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
Bragadeesh153 19:403cb36e22ed 1019
Bragadeesh153 19:403cb36e22ed 1020
Bragadeesh153 19:403cb36e22ed 1021 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
Bragadeesh153 19:403cb36e22ed 1022
Bragadeesh153 19:403cb36e22ed 1023 {
Bragadeesh153 19:403cb36e22ed 1024
Bragadeesh153 19:403cb36e22ed 1025 //other sensor both working, off or
Bragadeesh153 19:403cb36e22ed 1026 //other sensor gyro working, this sensor not working , off
Bragadeesh153 19:403cb36e22ed 1027 //other sensor mag working, this sensor not working,off
Bragadeesh153 19:403cb36e22ed 1028
Bragadeesh153 19:403cb36e22ed 1029
Bragadeesh153 19:403cb36e22ed 1030 ATS1_SW_ENABLE = 1; //switch off sensor 1
Bragadeesh153 19:403cb36e22ed 1031 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1032 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1033 {
Bragadeesh153 19:403cb36e22ed 1034 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
Bragadeesh153 19:403cb36e22ed 1035 }
Bragadeesh153 19:403cb36e22ed 1036 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1037 {
Bragadeesh153 19:403cb36e22ed 1038 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
Bragadeesh153 19:403cb36e22ed 1039 }
Bragadeesh153 19:403cb36e22ed 1040
Bragadeesh153 19:403cb36e22ed 1041
Bragadeesh153 19:403cb36e22ed 1042 ATS2_SW_ENABLE = 0; //switch on sensor 2
Bragadeesh153 19:403cb36e22ed 1043 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1044
Bragadeesh153 19:403cb36e22ed 1045 init = SENSOR_INIT(); //sensor 2 init
Bragadeesh153 19:403cb36e22ed 1046
Bragadeesh153 19:403cb36e22ed 1047 if( init == 0)
Bragadeesh153 19:403cb36e22ed 1048 {
Bragadeesh153 19:403cb36e22ed 1049
Bragadeesh153 19:403cb36e22ed 1050 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
Bragadeesh153 19:403cb36e22ed 1051 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1052 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1053 ATS1_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1054 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
Bragadeesh153 19:403cb36e22ed 1055 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1056 {
Bragadeesh153 19:403cb36e22ed 1057 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
Bragadeesh153 19:403cb36e22ed 1058 }
Bragadeesh153 19:403cb36e22ed 1059 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1060 {
Bragadeesh153 19:403cb36e22ed 1061 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 19:403cb36e22ed 1062 }
Bragadeesh153 19:403cb36e22ed 1063 return acq;
Bragadeesh153 19:403cb36e22ed 1064 }
Bragadeesh153 19:403cb36e22ed 1065
Bragadeesh153 19:403cb36e22ed 1066 int acq2;
Bragadeesh153 19:403cb36e22ed 1067 acq2 = SENSOR_DATA_ACQ();
Bragadeesh153 19:403cb36e22ed 1068
Bragadeesh153 19:403cb36e22ed 1069 if(acq2 == 3)
Bragadeesh153 19:403cb36e22ed 1070 {
Bragadeesh153 19:403cb36e22ed 1071 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
Bragadeesh153 19:403cb36e22ed 1072 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
Bragadeesh153 19:403cb36e22ed 1073 return 3;
Bragadeesh153 19:403cb36e22ed 1074 }
Bragadeesh153 19:403cb36e22ed 1075
Bragadeesh153 19:403cb36e22ed 1076 else if(acq2 == 1)
Bragadeesh153 19:403cb36e22ed 1077 {
Bragadeesh153 19:403cb36e22ed 1078 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1079 {
Bragadeesh153 19:403cb36e22ed 1080 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1081 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1082 ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
Bragadeesh153 19:403cb36e22ed 1083 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
Bragadeesh153 19:403cb36e22ed 1084 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 19:403cb36e22ed 1085 return 3;
Bragadeesh153 19:403cb36e22ed 1086 }
Bragadeesh153 19:403cb36e22ed 1087 else
Bragadeesh153 19:403cb36e22ed 1088 {
Bragadeesh153 19:403cb36e22ed 1089 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
Bragadeesh153 19:403cb36e22ed 1090 return 1;
Bragadeesh153 19:403cb36e22ed 1091 }
Bragadeesh153 19:403cb36e22ed 1092 }
Bragadeesh153 19:403cb36e22ed 1093
Bragadeesh153 19:403cb36e22ed 1094 else if(acq2==2) //Sensor 2 mag only, exit in both cases
Bragadeesh153 19:403cb36e22ed 1095 {
Bragadeesh153 19:403cb36e22ed 1096 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 19:403cb36e22ed 1097 return 2;
Bragadeesh153 19:403cb36e22ed 1098 }
Bragadeesh153 19:403cb36e22ed 1099
Bragadeesh153 19:403cb36e22ed 1100
Bragadeesh153 19:403cb36e22ed 1101
Bragadeesh153 19:403cb36e22ed 1102 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
Bragadeesh153 19:403cb36e22ed 1103 {
Bragadeesh153 19:403cb36e22ed 1104
Bragadeesh153 19:403cb36e22ed 1105 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
Bragadeesh153 19:403cb36e22ed 1106 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1107 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
Bragadeesh153 19:403cb36e22ed 1108 ATS1_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1109 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1110 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
Bragadeesh153 19:403cb36e22ed 1111 return acq;
Bragadeesh153 19:403cb36e22ed 1112
Bragadeesh153 19:403cb36e22ed 1113 }
Bragadeesh153 19:403cb36e22ed 1114
Bragadeesh153 19:403cb36e22ed 1115 }
Bragadeesh153 19:403cb36e22ed 1116
Bragadeesh153 19:403cb36e22ed 1117 else //Sensor 2 not working or both sensors gyro/mag ONLY
Bragadeesh153 19:403cb36e22ed 1118 {
Bragadeesh153 19:403cb36e22ed 1119 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1120 {
Bragadeesh153 19:403cb36e22ed 1121 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
Bragadeesh153 19:403cb36e22ed 1122 return 1;
Bragadeesh153 19:403cb36e22ed 1123 }
Bragadeesh153 19:403cb36e22ed 1124 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1125 {
Bragadeesh153 19:403cb36e22ed 1126 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 19:403cb36e22ed 1127 return 2;
Bragadeesh153 19:403cb36e22ed 1128 }
Bragadeesh153 19:403cb36e22ed 1129 pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 1130 return acq;
Bragadeesh153 19:403cb36e22ed 1131
Bragadeesh153 19:403cb36e22ed 1132 }
Bragadeesh153 19:403cb36e22ed 1133 }
Bragadeesh153 19:403cb36e22ed 1134
Bragadeesh153 19:403cb36e22ed 1135 else if(acq == 0)
Bragadeesh153 19:403cb36e22ed 1136 {
Bragadeesh153 19:403cb36e22ed 1137 pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
Bragadeesh153 19:403cb36e22ed 1138 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1139 wait_ms(5); //Switch ON sensor 2
Bragadeesh153 19:403cb36e22ed 1140 ATS2_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1141 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1142 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
Bragadeesh153 19:403cb36e22ed 1143 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
Bragadeesh153 19:403cb36e22ed 1144 {
Bragadeesh153 19:403cb36e22ed 1145
Bragadeesh153 19:403cb36e22ed 1146
Bragadeesh153 19:403cb36e22ed 1147 init = SENSOR_INIT();
Bragadeesh153 19:403cb36e22ed 1148
Bragadeesh153 19:403cb36e22ed 1149 if( init == 0)
Bragadeesh153 19:403cb36e22ed 1150 {
Bragadeesh153 19:403cb36e22ed 1151
Bragadeesh153 19:403cb36e22ed 1152 pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
Bragadeesh153 19:403cb36e22ed 1153 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1154 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
Bragadeesh153 19:403cb36e22ed 1155 return 0;
Bragadeesh153 19:403cb36e22ed 1156 }
Bragadeesh153 19:403cb36e22ed 1157
Bragadeesh153 19:403cb36e22ed 1158 int acq2;
Bragadeesh153 19:403cb36e22ed 1159 acq2 = SENSOR_DATA_ACQ();
Bragadeesh153 19:403cb36e22ed 1160
Bragadeesh153 19:403cb36e22ed 1161 if(acq2 == 3)
Bragadeesh153 19:403cb36e22ed 1162 {
Bragadeesh153 19:403cb36e22ed 1163 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
Bragadeesh153 19:403cb36e22ed 1164 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
Bragadeesh153 19:403cb36e22ed 1165 return 3;
Bragadeesh153 19:403cb36e22ed 1166 }
Bragadeesh153 19:403cb36e22ed 1167
Bragadeesh153 19:403cb36e22ed 1168 else if(acq2 == 1)
Bragadeesh153 19:403cb36e22ed 1169 {
Bragadeesh153 19:403cb36e22ed 1170
Bragadeesh153 19:403cb36e22ed 1171 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
Bragadeesh153 19:403cb36e22ed 1172 return 1;
Bragadeesh153 19:403cb36e22ed 1173
Bragadeesh153 19:403cb36e22ed 1174 }
Bragadeesh153 19:403cb36e22ed 1175
Bragadeesh153 19:403cb36e22ed 1176 else if(acq2 == 2)
Bragadeesh153 19:403cb36e22ed 1177 {
Bragadeesh153 19:403cb36e22ed 1178 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 19:403cb36e22ed 1179 return 2;
Bragadeesh153 19:403cb36e22ed 1180 }
Bragadeesh153 19:403cb36e22ed 1181
Bragadeesh153 19:403cb36e22ed 1182
Bragadeesh153 19:403cb36e22ed 1183 else if(acq2 == 0)
Bragadeesh153 19:403cb36e22ed 1184 {
Bragadeesh153 19:403cb36e22ed 1185
Bragadeesh153 19:403cb36e22ed 1186 pc_acs.printf(" Sensor 2 data acq failure..\n \r");
Bragadeesh153 19:403cb36e22ed 1187 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1188
Bragadeesh153 19:403cb36e22ed 1189 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
Bragadeesh153 19:403cb36e22ed 1190 return 0;
Bragadeesh153 19:403cb36e22ed 1191
Bragadeesh153 19:403cb36e22ed 1192 }
Bragadeesh153 19:403cb36e22ed 1193
Bragadeesh153 19:403cb36e22ed 1194 }
Bragadeesh153 19:403cb36e22ed 1195
Bragadeesh153 19:403cb36e22ed 1196 }
Bragadeesh153 19:403cb36e22ed 1197 }
Bragadeesh153 19:403cb36e22ed 1198
Bragadeesh153 19:403cb36e22ed 1199
Bragadeesh153 19:403cb36e22ed 1200 if(( (ACS_ATS_STATUS & 0x0C) == 0x04)) //ATS2 status is 01XX to check if ATS2 is ON
Bragadeesh153 16:cc77770d787f 1201 {
Bragadeesh153 16:cc77770d787f 1202
Bragadeesh153 19:403cb36e22ed 1203 acq = SENSOR_DATA_ACQ(); //ATS2 should already be on //acquire data 3 full success, 0 full failure , 1 gyro only , 2 mag only
Bragadeesh153 19:403cb36e22ed 1204
Bragadeesh153 19:403cb36e22ed 1205 if(acq == 3) //Both available read and exit
Bragadeesh153 19:403cb36e22ed 1206 {
Bragadeesh153 16:cc77770d787f 1207
Bragadeesh153 19:403cb36e22ed 1208 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
Bragadeesh153 19:403cb36e22ed 1209
Bragadeesh153 19:403cb36e22ed 1210 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 19:403cb36e22ed 1211 pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
Bragadeesh153 19:403cb36e22ed 1212 return 3;
Bragadeesh153 19:403cb36e22ed 1213 }
Bragadeesh153 19:403cb36e22ed 1214 else if((acq == 2)||(acq==1)) //Only mag or only gyro
Bragadeesh153 19:403cb36e22ed 1215 {
Bragadeesh153 19:403cb36e22ed 1216
Bragadeesh153 19:403cb36e22ed 1217 pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
Bragadeesh153 19:403cb36e22ed 1218
Bragadeesh153 16:cc77770d787f 1219
Bragadeesh153 19:403cb36e22ed 1220 if( (ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
Bragadeesh153 19:403cb36e22ed 1221
Bragadeesh153 19:403cb36e22ed 1222 {
Bragadeesh153 16:cc77770d787f 1223
Bragadeesh153 19:403cb36e22ed 1224 //other sensor both working, off or
Bragadeesh153 19:403cb36e22ed 1225 //other sensor gyro working, this sensor not working , off
Bragadeesh153 19:403cb36e22ed 1226 //other sensor mag working, this sensor not working,off
Bragadeesh153 19:403cb36e22ed 1227
Bragadeesh153 19:403cb36e22ed 1228
Bragadeesh153 19:403cb36e22ed 1229 ATS2_SW_ENABLE = 1; //switch off sensor 2
Bragadeesh153 19:403cb36e22ed 1230 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1231 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1232 {
Bragadeesh153 19:403cb36e22ed 1233 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
Bragadeesh153 19:403cb36e22ed 1234 }
Bragadeesh153 19:403cb36e22ed 1235 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1236 {
Bragadeesh153 19:403cb36e22ed 1237 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
Bragadeesh153 19:403cb36e22ed 1238 }
Bragadeesh153 16:cc77770d787f 1239
Bragadeesh153 19:403cb36e22ed 1240
Bragadeesh153 19:403cb36e22ed 1241 ATS1_SW_ENABLE = 0; //switch on sensor 1
Bragadeesh153 19:403cb36e22ed 1242 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1243
Bragadeesh153 19:403cb36e22ed 1244 init = SENSOR_INIT(); //sensor 2 init
Bragadeesh153 19:403cb36e22ed 1245
Bragadeesh153 19:403cb36e22ed 1246 if( init == 0)
Bragadeesh153 19:403cb36e22ed 1247 {
Bragadeesh153 19:403cb36e22ed 1248
Bragadeesh153 19:403cb36e22ed 1249 pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 19:403cb36e22ed 1250 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1251 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1252 ATS2_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1253 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
Bragadeesh153 19:403cb36e22ed 1254 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1255 {
Bragadeesh153 19:403cb36e22ed 1256 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
Bragadeesh153 19:403cb36e22ed 1257 }
Bragadeesh153 19:403cb36e22ed 1258 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1259 {
Bragadeesh153 19:403cb36e22ed 1260 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 19:403cb36e22ed 1261 }
Bragadeesh153 19:403cb36e22ed 1262 return acq;
Bragadeesh153 19:403cb36e22ed 1263 }
Bragadeesh153 19:403cb36e22ed 1264
Bragadeesh153 19:403cb36e22ed 1265 int acq2;
Bragadeesh153 19:403cb36e22ed 1266 acq2 = SENSOR_DATA_ACQ();
Bragadeesh153 16:cc77770d787f 1267
Bragadeesh153 19:403cb36e22ed 1268 if(acq2 == 3)
Bragadeesh153 19:403cb36e22ed 1269 {
Bragadeesh153 19:403cb36e22ed 1270 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
Bragadeesh153 19:403cb36e22ed 1271 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
Bragadeesh153 19:403cb36e22ed 1272 return 3;
Bragadeesh153 19:403cb36e22ed 1273 }
Bragadeesh153 19:403cb36e22ed 1274
Bragadeesh153 19:403cb36e22ed 1275 else if(acq2 == 1)
Bragadeesh153 19:403cb36e22ed 1276 {
Bragadeesh153 19:403cb36e22ed 1277 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1278 {
Bragadeesh153 19:403cb36e22ed 1279 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1280 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1281 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
Bragadeesh153 19:403cb36e22ed 1282 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
Bragadeesh153 19:403cb36e22ed 1283 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 19:403cb36e22ed 1284 return 3;
Bragadeesh153 19:403cb36e22ed 1285 }
Bragadeesh153 19:403cb36e22ed 1286 else
Bragadeesh153 19:403cb36e22ed 1287 {
Bragadeesh153 19:403cb36e22ed 1288 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
Bragadeesh153 16:cc77770d787f 1289 return 1;
Bragadeesh153 19:403cb36e22ed 1290 }
Bragadeesh153 19:403cb36e22ed 1291 }
Bragadeesh153 19:403cb36e22ed 1292
Bragadeesh153 19:403cb36e22ed 1293 else if(acq2==2) //Sensor 1 mag only, exit in both cases
Bragadeesh153 19:403cb36e22ed 1294 {
Bragadeesh153 19:403cb36e22ed 1295 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 19:403cb36e22ed 1296 return 2;
Bragadeesh153 19:403cb36e22ed 1297 }
Bragadeesh153 19:403cb36e22ed 1298
Bragadeesh153 19:403cb36e22ed 1299
Bragadeesh153 19:403cb36e22ed 1300
Bragadeesh153 19:403cb36e22ed 1301 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
Bragadeesh153 19:403cb36e22ed 1302 {
Bragadeesh153 16:cc77770d787f 1303
Bragadeesh153 19:403cb36e22ed 1304 pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
Bragadeesh153 19:403cb36e22ed 1305 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1306 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
Bragadeesh153 19:403cb36e22ed 1307 ATS2_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1308 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1309 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
Bragadeesh153 19:403cb36e22ed 1310 return acq;
Bragadeesh153 19:403cb36e22ed 1311
Bragadeesh153 19:403cb36e22ed 1312 }
Bragadeesh153 19:403cb36e22ed 1313
Bragadeesh153 19:403cb36e22ed 1314 }
Bragadeesh153 16:cc77770d787f 1315
Bragadeesh153 19:403cb36e22ed 1316 else //Sensor 1 not working or both sensors gyro/mag ONLY
Bragadeesh153 19:403cb36e22ed 1317 {
Bragadeesh153 19:403cb36e22ed 1318 if(acq == 1)
Bragadeesh153 19:403cb36e22ed 1319 {
Bragadeesh153 19:403cb36e22ed 1320 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
Bragadeesh153 19:403cb36e22ed 1321 return 1;
Bragadeesh153 19:403cb36e22ed 1322 }
Bragadeesh153 19:403cb36e22ed 1323 if(acq==2)
Bragadeesh153 19:403cb36e22ed 1324 {
Bragadeesh153 19:403cb36e22ed 1325 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
Bragadeesh153 19:403cb36e22ed 1326 return 2;
Bragadeesh153 19:403cb36e22ed 1327 }
Bragadeesh153 19:403cb36e22ed 1328 pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
Bragadeesh153 19:403cb36e22ed 1329 return acq;
Bragadeesh153 19:403cb36e22ed 1330
Bragadeesh153 19:403cb36e22ed 1331 }
Bragadeesh153 16:cc77770d787f 1332 }
Bragadeesh153 16:cc77770d787f 1333
Bragadeesh153 19:403cb36e22ed 1334 else if(acq == 0)
Bragadeesh153 16:cc77770d787f 1335 {
Bragadeesh153 19:403cb36e22ed 1336 pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
Bragadeesh153 19:403cb36e22ed 1337 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1338 wait_ms(5); //Switch ON sensor 1
Bragadeesh153 19:403cb36e22ed 1339 ATS1_SW_ENABLE = 0;
Bragadeesh153 19:403cb36e22ed 1340 wait_ms(5);
Bragadeesh153 19:403cb36e22ed 1341 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
Bragadeesh153 19:403cb36e22ed 1342 if( (ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
Bragadeesh153 19:403cb36e22ed 1343 {
Bragadeesh153 19:403cb36e22ed 1344
Bragadeesh153 19:403cb36e22ed 1345
Bragadeesh153 19:403cb36e22ed 1346 init = SENSOR_INIT();
Bragadeesh153 19:403cb36e22ed 1347
Bragadeesh153 19:403cb36e22ed 1348 if( init == 0)
Bragadeesh153 19:403cb36e22ed 1349 {
Bragadeesh153 19:403cb36e22ed 1350
Bragadeesh153 19:403cb36e22ed 1351 pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
Bragadeesh153 19:403cb36e22ed 1352 ATS2_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1353 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
Bragadeesh153 19:403cb36e22ed 1354 return 0;
Bragadeesh153 19:403cb36e22ed 1355 }
Bragadeesh153 19:403cb36e22ed 1356
Bragadeesh153 19:403cb36e22ed 1357 int acq2;
Bragadeesh153 19:403cb36e22ed 1358 acq2 = SENSOR_DATA_ACQ();
Bragadeesh153 19:403cb36e22ed 1359
Bragadeesh153 19:403cb36e22ed 1360 if(acq2 == 3)
Bragadeesh153 19:403cb36e22ed 1361 {
Bragadeesh153 19:403cb36e22ed 1362 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
Bragadeesh153 19:403cb36e22ed 1363 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
Bragadeesh153 19:403cb36e22ed 1364 return 3;
Bragadeesh153 19:403cb36e22ed 1365 }
Bragadeesh153 19:403cb36e22ed 1366
Bragadeesh153 19:403cb36e22ed 1367 else if(acq2 == 1)
Bragadeesh153 19:403cb36e22ed 1368 {
Bragadeesh153 19:403cb36e22ed 1369
Bragadeesh153 19:403cb36e22ed 1370 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
Bragadeesh153 19:403cb36e22ed 1371 return 1;
Bragadeesh153 19:403cb36e22ed 1372
Bragadeesh153 19:403cb36e22ed 1373 }
Bragadeesh153 19:403cb36e22ed 1374
Bragadeesh153 19:403cb36e22ed 1375 else if(acq2 == 2)
Bragadeesh153 19:403cb36e22ed 1376 {
Bragadeesh153 19:403cb36e22ed 1377 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
Bragadeesh153 19:403cb36e22ed 1378 return 2;
Bragadeesh153 19:403cb36e22ed 1379 }
Bragadeesh153 19:403cb36e22ed 1380
Bragadeesh153 19:403cb36e22ed 1381
Bragadeesh153 19:403cb36e22ed 1382 else if(acq2 == 0)
Bragadeesh153 19:403cb36e22ed 1383 {
Bragadeesh153 19:403cb36e22ed 1384
Bragadeesh153 19:403cb36e22ed 1385 pc_acs.printf(" Sensor 1 data acq failure..\n \r");
Bragadeesh153 19:403cb36e22ed 1386 ATS1_SW_ENABLE = 1;
Bragadeesh153 19:403cb36e22ed 1387
Bragadeesh153 19:403cb36e22ed 1388 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
Bragadeesh153 19:403cb36e22ed 1389 return 0;
Bragadeesh153 19:403cb36e22ed 1390
Bragadeesh153 19:403cb36e22ed 1391 }
Bragadeesh153 19:403cb36e22ed 1392
Bragadeesh153 19:403cb36e22ed 1393 }
Bragadeesh153 19:403cb36e22ed 1394
Bragadeesh153 16:cc77770d787f 1395 }
Bragadeesh153 19:403cb36e22ed 1396 }
Bragadeesh153 16:cc77770d787f 1397
Bragadeesh153 16:cc77770d787f 1398
Bragadeesh153 19:403cb36e22ed 1399
Bragadeesh153 16:cc77770d787f 1400
Bragadeesh153 16:cc77770d787f 1401
Bragadeesh153 16:cc77770d787f 1402
Bragadeesh153 16:cc77770d787f 1403
Bragadeesh153 19:403cb36e22ed 1404
Bragadeesh153 16:cc77770d787f 1405
Bragadeesh153 17:1e1955f3db75 1406
Bragadeesh153 19:403cb36e22ed 1407
Bragadeesh153 19:403cb36e22ed 1408
Bragadeesh153 16:cc77770d787f 1409
Bragadeesh153 19:403cb36e22ed 1410
Bragadeesh153 19:403cb36e22ed 1411
Bragadeesh153 16:cc77770d787f 1412 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
Bragadeesh153 16:cc77770d787f 1413 pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
Bragadeesh153 16:cc77770d787f 1414
Bragadeesh153 19:403cb36e22ed 1415 return 0;
sakthipriya 0:7b4c00e3912f 1416 }
sakthipriya 0:7b4c00e3912f 1417
sakthipriya 0:7b4c00e3912f 1418 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 1419 {
sakthipriya 0:7b4c00e3912f 1420 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 1421
sakthipriya 0:7b4c00e3912f 1422 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 1423 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 1424 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 1425 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 1426 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 1427 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 1428
sakthipriya 0:7b4c00e3912f 1429
Bragadeesh153 17:1e1955f3db75 1430 printf("\r\r");
sakthipriya 0:7b4c00e3912f 1431
sakthipriya 0:7b4c00e3912f 1432 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 1433
sakthipriya 0:7b4c00e3912f 1434
sakthipriya 0:7b4c00e3912f 1435 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 1436
sakthipriya 0:7b4c00e3912f 1437 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 1438 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 1439 {
sakthipriya 0:7b4c00e3912f 1440 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 1441 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 1442 }
sakthipriya 0:7b4c00e3912f 1443
sakthipriya 0:7b4c00e3912f 1444 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 19:403cb36e22ed 1445 pc_acs.printf("\r\n current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 1446 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 1447 {
lakshya 10:f93407b97750 1448 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 1449 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1450 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1451 }
lakshya 10:f93407b97750 1452 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 1453 {
lakshya 10:f93407b97750 1454 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 1455 PWM1.period(TIME_PERIOD);
Bragadeesh153 19:403cb36e22ed 1456 PWM1 = l_duty_cycle_x/100;
lakshya 10:f93407b97750 1457 }
lakshya 10:f93407b97750 1458 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 1459 {
lakshya 10:f93407b97750 1460 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 1461 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1462 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1463 }
lakshya 10:f93407b97750 1464 else if(l_current_x==0)
lakshya 10:f93407b97750 1465 {
lakshya 10:f93407b97750 1466 printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 1467 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 10:f93407b97750 1468 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1469 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1470 }
lakshya 10:f93407b97750 1471 else //not necessary
lakshya 10:f93407b97750 1472 {
lakshya 10:f93407b97750 1473 g_err_flag_TR_x = 1;
lakshya 10:f93407b97750 1474 }
Bragadeesh153 19:403cb36e22ed 1475
Bragadeesh153 19:403cb36e22ed 1476 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1477
lakshya 10:f93407b97750 1478 //------------------------------------- y-direction TR--------------------------------------//
lakshya 10:f93407b97750 1479
lakshya 10:f93407b97750 1480
lakshya 10:f93407b97750 1481 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 1482
lakshya 10:f93407b97750 1483 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 1484 if (l_moment_y <0)
lakshya 10:f93407b97750 1485 {
lakshya 10:f93407b97750 1486 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 1487 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 1488 }
lakshya 10:f93407b97750 1489
lakshya 10:f93407b97750 1490
lakshya 10:f93407b97750 1491 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 19:403cb36e22ed 1492 pc_acs.printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 1493 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 1494 {
lakshya 10:f93407b97750 1495 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 1496 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1497 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1498 }
lakshya 10:f93407b97750 1499 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 1500 {
lakshya 10:f93407b97750 1501 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 1502 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1503 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1504 }
lakshya 10:f93407b97750 1505 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 1506 {
lakshya 10:f93407b97750 1507 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 1508 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1509 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1510 }
lakshya 10:f93407b97750 1511 else if(l_current_y==0)
lakshya 10:f93407b97750 1512 {
lakshya 10:f93407b97750 1513 printf("\n \r l_current_y====0");
lakshya 10:f93407b97750 1514 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 10:f93407b97750 1515 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1516 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1517 }
lakshya 10:f93407b97750 1518 else // not necessary
lakshya 10:f93407b97750 1519 {
lakshya 10:f93407b97750 1520 g_err_flag_TR_y = 1;
lakshya 10:f93407b97750 1521 }
Bragadeesh153 19:403cb36e22ed 1522
Bragadeesh153 19:403cb36e22ed 1523 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1524
lakshya 10:f93407b97750 1525 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 1526
Bragadeesh153 17:1e1955f3db75 1527
Bragadeesh153 17:1e1955f3db75 1528
lakshya 10:f93407b97750 1529 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 1530
Bragadeesh153 17:1e1955f3db75 1531 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 1532 if (l_moment_z <0)
lakshya 10:f93407b97750 1533 {
Bragadeesh153 17:1e1955f3db75 1534 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 1535 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 1536 }
lakshya 10:f93407b97750 1537
lakshya 10:f93407b97750 1538
lakshya 10:f93407b97750 1539 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
Bragadeesh153 19:403cb36e22ed 1540 pc_acs.printf("current in trz is %f \r \n",l_current_z);
Bragadeesh153 17:1e1955f3db75 1541 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 1542 {
lakshya 10:f93407b97750 1543 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 1544 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1545 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1546 }
lakshya 10:f93407b97750 1547 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 1548 {
lakshya 10:f93407b97750 1549 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 1550 printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1551 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1552 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1553 }
lakshya 10:f93407b97750 1554 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 1555 {
lakshya 10:f93407b97750 1556 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 1557 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1558 PWM3 = l_duty_cycle_z/100 ;
Bragadeesh153 17:1e1955f3db75 1559 }
lakshya 10:f93407b97750 1560 else if(l_current_z==0)
lakshya 10:f93407b97750 1561 {
lakshya 10:f93407b97750 1562 printf("\n \r l_current_z====0");
lakshya 10:f93407b97750 1563 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 10:f93407b97750 1564 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1565 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1566 }
lakshya 10:f93407b97750 1567 else // not necessary
lakshya 10:f93407b97750 1568 {
Bragadeesh153 17:1e1955f3db75 1569 g_err_flag_TR_z = 1;
Bragadeesh153 17:1e1955f3db75 1570 }
lakshya 10:f93407b97750 1571
Bragadeesh153 19:403cb36e22ed 1572 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
Bragadeesh153 19:403cb36e22ed 1573
lakshya 10:f93407b97750 1574 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 1575
lakshya 10:f93407b97750 1576 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 1577
lakshya 10:f93407b97750 1578 }
lakshya 10:f93407b97750 1579
lakshya 10:f93407b97750 1580
sakthipriya 0:7b4c00e3912f 1581
sakthipriya 0:7b4c00e3912f 1582
sakthipriya 0:7b4c00e3912f 1583