Team Fox / Mbed 2 deprecated BAE_QM_MAR9

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of workinQM_5thJan_azad by Team Fox

ACS.cpp@41:5df2bed2157d, 2016-07-08 (annotated)

Committer:
Bragadeesh153
Date:
Fri Jul 08 08:14:29 2016 +0000
Revision:
41:5df2bed2157d
Parent:
40:c2538d97e78b 
BAE CODE used for testing telecommands

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