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@39:670133e7ffd8, 2016-07-05 (annotated)

Committer:
lakshya
Date:
Tue Jul 05 13:44:15 2016 +0000
Revision:
39:670133e7ffd8
Parent:
20:949d13045431
Child:
40:c2538d97e78b
Child:
45:b5bd48ffbb67
Child:
49:61c9f28332ba
bae editing done; testing and debuging; I2C working; ; +watchdog to be implemented

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
lakshya 39:670133e7ffd8 24 extern DigitalInOut ATS1_SW_ENABLE; // enable of att sens2 switch
lakshya 39:670133e7ffd8 25 extern DigitalInOut 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
lakshya 39:670133e7ffd8 31 DigitalInOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
lakshya 39:670133e7ffd8 32 DigitalInOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
lakshya 39:670133e7ffd8 33 DigitalInOut 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
lakshya 20:949d13045431 84 uint16_t ACS_MM_X_COMSN = 1;
lakshya 20:949d13045431 85 uint16_t ACS_MM_Y_COMSN = 1;
lakshya 20:949d13045431 86 uint16_t ACS_MG_X_COMSN = 1;
lakshya 20:949d13045431 87 uint16_t ACS_MG_Y_COMSN = 1;
lakshya 20:949d13045431 88 uint16_t ACS_MM_Z_COMSN = 1;
lakshya 20:949d13045431 89 uint16_t ACS_MG_Z_COMSN = 1;
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 {
lakshya 20:949d13045431 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
lakshya 20:949d13045431 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 {
lakshya 20:949d13045431 570 cmd[0]=RESETREQ;
lakshya 20:949d13045431 571 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 572 ack = i2c.write(SLAVE_ADDR,cmd,2); //repeat
lakshya 20:949d13045431 573 if(ack !=0)
lakshya 20:949d13045431 574 return 0;
lakshya 20:949d13045431 575 }
lakshya 20:949d13045431 576
lakshya 20:949d13045431 577 wait_ms(575);
lakshya 20:949d13045431 578
sakthipriya 0:7b4c00e3912f 579 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 580 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 581
lakshya 20:949d13045431 582 if( ack!=0)
lakshya 20:949d13045431 583 {
lakshya 20:949d13045431 584 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 585 if(ack!=0)
lakshya 20:949d13045431 586 return 0;
lakshya 20:949d13045431 587 }
lakshya 20:949d13045431 588
lakshya 20:949d13045431 589 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 590
lakshya 20:949d13045431 591 if( ack!=0)
lakshya 20:949d13045431 592 {
lakshya 20:949d13045431 593 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 594 if(ack!=0)
lakshya 20:949d13045431 595 return 0;
lakshya 20:949d13045431 596 }
lakshya 20:949d13045431 597
lakshya 20:949d13045431 598 /// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
lakshya 20:949d13045431 599
lakshya 20:949d13045431 600 //to check whether EEPROM is uploaded properly
sakthipriya 0:7b4c00e3912f 601 switch((int)store) {
lakshya 20:949d13045431 602 case(3): {
sakthipriya 0:7b4c00e3912f 603 break;
sakthipriya 0:7b4c00e3912f 604 }
sakthipriya 0:7b4c00e3912f 605 case(11): {
sakthipriya 0:7b4c00e3912f 606 break;
sakthipriya 0:7b4c00e3912f 607 }
sakthipriya 0:7b4c00e3912f 608 default: {
sakthipriya 0:7b4c00e3912f 609 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 610 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 611 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 612 if( ack!=0)
lakshya 20:949d13045431 613 {
lakshya 20:949d13045431 614 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 615 if(ack!=0)
lakshya 20:949d13045431 616 return 0;
lakshya 20:949d13045431 617 }
lakshya 20:949d13045431 618 wait_ms(575);//should be 600
lakshya 20:949d13045431 619
lakshya 20:949d13045431 620 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 621 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 622 if( ack!=0)
lakshya 20:949d13045431 623 {
lakshya 20:949d13045431 624 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 625 if(ack!=0)
lakshya 20:949d13045431 626 return 0;
lakshya 20:949d13045431 627 }
lakshya 20:949d13045431 628 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 629 if( ack!=0)
lakshya 20:949d13045431 630 {
lakshya 20:949d13045431 631 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 632 if(ack!=0)
lakshya 20:949d13045431 633 return 0;
lakshya 20:949d13045431 634 }
lakshya 20:949d13045431 635 /// pc_acs.printf("Sentral Status is %x\n \r",(int)store);
lakshya 20:949d13045431 636
sakthipriya 0:7b4c00e3912f 637 }
sakthipriya 0:7b4c00e3912f 638 }
lakshya 20:949d13045431 639
lakshya 20:949d13045431 640 int manual=0;
lakshya 20:949d13045431 641 if( ((int)store != 11 )&&((int)store != 3))
lakshya 20:949d13045431 642 {
lakshya 20:949d13045431 643
lakshya 20:949d13045431 644 cmd[0]=RESETREQ;
lakshya 20:949d13045431 645 cmd[1]=BIT_RESREQ;
lakshya 20:949d13045431 646 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 647 if( ack!=0)
lakshya 20:949d13045431 648 {
lakshya 20:949d13045431 649 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 650 if(ack!=0)
lakshya 20:949d13045431 651 return 0;
lakshya 20:949d13045431 652 }
lakshya 20:949d13045431 653 wait_ms(575);
lakshya 20:949d13045431 654
lakshya 20:949d13045431 655 manual = CONFIG_UPLOAD();
lakshya 20:949d13045431 656
lakshya 20:949d13045431 657 if(manual == 0)
lakshya 20:949d13045431 658 {
lakshya 20:949d13045431 659 //MANUAL CONFIGURATION FAILED
lakshya 20:949d13045431 660 return 0;
lakshya 20:949d13045431 661 }
lakshya 20:949d13045431 662
lakshya 20:949d13045431 663 }
lakshya 20:949d13045431 664 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
lakshya 20:949d13045431 665 cmd[1]=BIT_RUN_ENB;
lakshya 20:949d13045431 666 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 667 if( ack!=0)
lakshya 20:949d13045431 668 {
lakshya 20:949d13045431 669 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 670 if(ack!=0)
lakshya 20:949d13045431 671 return 0;
lakshya 20:949d13045431 672 }
lakshya 20:949d13045431 673
lakshya 20:949d13045431 674 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
lakshya 20:949d13045431 675 cmd[1]=BIT_MAGODR;
lakshya 20:949d13045431 676 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 677 if( ack!=0)
lakshya 20:949d13045431 678 {
lakshya 20:949d13045431 679 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 680 if(ack!=0)
lakshya 20:949d13045431 681 return 0;
lakshya 20:949d13045431 682 }
lakshya 20:949d13045431 683
lakshya 20:949d13045431 684 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
lakshya 20:949d13045431 685 cmd[1]=BIT_GYROODR;
lakshya 20:949d13045431 686 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 687 if( ack!=0)
lakshya 20:949d13045431 688 {
lakshya 20:949d13045431 689 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 690 if(ack!=0)
lakshya 20:949d13045431 691 return 0;
lakshya 20:949d13045431 692 }
lakshya 20:949d13045431 693
lakshya 20:949d13045431 694 cmd[0]=ACCERATE; //Output data rate of 0 Hz is used to disable accelerometer
lakshya 20:949d13045431 695 cmd[1]=0x00;
lakshya 20:949d13045431 696 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 697 if( ack!=0)
lakshya 20:949d13045431 698 {
lakshya 20:949d13045431 699 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 700 if(ack!=0)
lakshya 20:949d13045431 701 return 0;
lakshya 20:949d13045431 702 }
lakshya 20:949d13045431 703 //wait_ms(20);
lakshya 20:949d13045431 704 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register , to scaled sensor values
lakshya 20:949d13045431 705 cmd[1]=0x00;
lakshya 20:949d13045431 706 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 707 if( ack!=0)
lakshya 20:949d13045431 708 {
lakshya 20:949d13045431 709 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 710 if(ack!=0)
lakshya 20:949d13045431 711 return 0;
lakshya 20:949d13045431 712 }
lakshya 20:949d13045431 713
lakshya 20:949d13045431 714 cmd[0]=ENB_EVT; //Enabling the CPU reset , error,gyro values and magnetometer values
lakshya 20:949d13045431 715 cmd[1]=BIT_EVT_ENB;
lakshya 20:949d13045431 716 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 717 if( ack!=0)
lakshya 20:949d13045431 718 {
lakshya 20:949d13045431 719 ack = i2c.write(SLAVE_ADDR,cmd,2);
lakshya 20:949d13045431 720 if(ack!=0)
lakshya 20:949d13045431 721 return 0;
lakshya 20:949d13045431 722 }
lakshya 20:949d13045431 723
lakshya 20:949d13045431 724 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 725 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 726 if( ack!=0)
lakshya 20:949d13045431 727 {
lakshya 20:949d13045431 728 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 729 if(ack!=0)
lakshya 20:949d13045431 730 return 0;
lakshya 20:949d13045431 731 }
lakshya 20:949d13045431 732
lakshya 20:949d13045431 733 ack = i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 734 if( ack!=0)
lakshya 20:949d13045431 735 {
lakshya 20:949d13045431 736 ack= i2c.read(SLAVE_ADDR_READ,&store,1);
lakshya 20:949d13045431 737 if(ack!=0)
lakshya 20:949d13045431 738 return 0;
lakshya 20:949d13045431 739 }
lakshya 20:949d13045431 740
lakshya 20:949d13045431 741 /// pc_acs.printf("Sentral Status after initialising is %x\n \r",(int)store);
lakshya 20:949d13045431 742
lakshya 20:949d13045431 743 if( (int)store == 3) //Check if initialised properly and not in idle state
lakshya 20:949d13045431 744 {
lakshya 20:949d13045431 745 /// pc_acs.printf("Exited sensor init successfully\n \r");
lakshya 20:949d13045431 746 return 1;
lakshya 20:949d13045431 747 }
lakshya 20:949d13045431 748
lakshya 20:949d13045431 749
lakshya 20:949d13045431 750 //// pc_acs.printf("Sensor init failed \n \r") ;
lakshya 20:949d13045431 751 return 0;
lakshya 20:949d13045431 752 }
lakshya 20:949d13045431 753
lakshya 20:949d13045431 754 int FCTN_ACS_INIT()
lakshya 20:949d13045431 755 {
lakshya 20:949d13045431 756 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
lakshya 20:949d13045431 757
lakshya 17:fc782f7548c6 758
lakshya 20:949d13045431 759 int working=0;
lakshya 20:949d13045431 760
lakshya 20:949d13045431 761 /// pc_acs.printf("Attitude sensor init called \n \r");
lakshya 20:949d13045431 762 /// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 17:fc782f7548c6 763
lakshya 17:fc782f7548c6 764
lakshya 20:949d13045431 765 if(((ACS_ATS_STATUS & 0xC0) != 0xC0)&&( (ACS_ATS_STATUS & 0xC0) != 0x80)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 766 {
lakshya 20:949d13045431 767
lakshya 20:949d13045431 768 /// pc_acs.printf("Sensor 1 marked working \n \r");
lakshya 20:949d13045431 769 working = SENSOR_INIT();
lakshya 20:949d13045431 770 if(working ==1)
lakshya 20:949d13045431 771 {
lakshya 20:949d13045431 772 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 773 /// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS); //Sensor 1 INIT successful
lakshya 20:949d13045431 774 /// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 1\n \r");
lakshya 20:949d13045431 775 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 776 return 1;
lakshya 20:949d13045431 777 }
lakshya 20:949d13045431 778
lakshya 20:949d13045431 779
lakshya 20:949d13045431 780
lakshya 20:949d13045431 781 /// pc_acs.printf("Sensor 1 not working.Powering off.\n \r"); //Sensor 1 INIT failure and power off
lakshya 20:949d13045431 782 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 783 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 784
lakshya 20:949d13045431 785 }
lakshya 17:fc782f7548c6 786
lakshya 20:949d13045431 787 /// pc_acs.printf("Sensor 1 not working. Trying Sensor 2\n \r");
lakshya 17:fc782f7548c6 788
lakshya 20:949d13045431 789 if(( (ACS_ATS_STATUS & 0x0C) != 0x0C)&&( (ACS_ATS_STATUS & 0x0C) != 0x08)) //Sensor1 status is not 10 or 11
lakshya 20:949d13045431 790 {
lakshya 20:949d13045431 791
lakshya 20:949d13045431 792
lakshya 20:949d13045431 793 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 794 wait_ms(5);
lakshya 20:949d13045431 795 working = SENSOR_INIT();
lakshya 20:949d13045431 796 if(working ==1)
lakshya 20:949d13045431 797 {
lakshya 20:949d13045431 798 /// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 799 /// pc_acs.printf("Attitude sensor init exitting. Init successful. Ideal case.Sensor 2\n \r"); //Sensor2 INIT successful
lakshya 20:949d13045431 800 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 801 ACS_INIT_STATUS = 0;
lakshya 20:949d13045431 802 return 2;
lakshya 20:949d13045431 803 }
lakshya 20:949d13045431 804
lakshya 20:949d13045431 805 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 806 wait_ms(5);
lakshya 20:949d13045431 807
lakshya 20:949d13045431 808 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 809
lakshya 20:949d13045431 810
lakshya 20:949d13045431 811 }
lakshya 17:fc782f7548c6 812
lakshya 20:949d13045431 813 /// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 814 /// pc_acs.printf("Sensor 2 also not working.Exit init.\n \r");
lakshya 17:fc782f7548c6 815
lakshya 20:949d13045431 816 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag //Sensor 2 also not working
lakshya 20:949d13045431 817 return 0;
sakthipriya 0:7b4c00e3912f 818 }
sakthipriya 0:7b4c00e3912f 819
lakshya 20:949d13045431 820
lakshya 20:949d13045431 821 int SENSOR_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 822 {
lakshya 20:949d13045431 823 //int mag_only=0;
lakshya 20:949d13045431 824 /// pc_acs.printf("Entering Sensor data acq.\n \r");
lakshya 20:949d13045431 825 char status;
lakshya 20:949d13045431 826 int sentral;
lakshya 20:949d13045431 827 int event;
lakshya 20:949d13045431 828 int sensor;
lakshya 20:949d13045431 829 int error;
lakshya 20:949d13045431 830 int init;
lakshya 20:949d13045431 831
lakshya 20:949d13045431 832 uint8_t gyro_error=0;
lakshya 20:949d13045431 833 uint8_t mag_error=0;
lakshya 20:949d13045431 834
lakshya 20:949d13045431 835 //int ack1;
lakshya 20:949d13045431 836 //int ack2;
lakshya 20:949d13045431 837
sakthipriya 0:7b4c00e3912f 838 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 839 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 840 if(ack!=0)
lakshya 20:949d13045431 841 {
lakshya 20:949d13045431 842 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 843 if(ack!=0)
lakshya 20:949d13045431 844 return 0;
lakshya 20:949d13045431 845 }
lakshya 20:949d13045431 846
lakshya 20:949d13045431 847 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 848 if(ack!=0)
lakshya 20:949d13045431 849 {
lakshya 20:949d13045431 850 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 851 if(ack!=0)
lakshya 20:949d13045431 852 return 0;
lakshya 20:949d13045431 853 }
lakshya 17:fc782f7548c6 854
lakshya 20:949d13045431 855 event = (int)status;
lakshya 20:949d13045431 856
lakshya 20:949d13045431 857 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 858 {
lakshya 20:949d13045431 859 ATS1_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 860 }
lakshya 20:949d13045431 861 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 862 {
lakshya 20:949d13045431 863 ATS2_EVENT_STATUS_RGTR = (uint8_t)event;
lakshya 20:949d13045431 864 }
lakshya 20:949d13045431 865
lakshya 20:949d13045431 866 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 867 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 868 if(ack!=0)
lakshya 20:949d13045431 869 {
lakshya 20:949d13045431 870 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 871 if(ack!=0)
lakshya 20:949d13045431 872 return 0;
lakshya 20:949d13045431 873 }
lakshya 20:949d13045431 874
lakshya 20:949d13045431 875 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 876 if(ack!=0)
lakshya 20:949d13045431 877 {
lakshya 20:949d13045431 878 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 879 if(ack!=0)
lakshya 20:949d13045431 880 return 0;
lakshya 20:949d13045431 881 }
lakshya 20:949d13045431 882
lakshya 17:fc782f7548c6 883
lakshya 20:949d13045431 884 sentral = (int) status;
lakshya 20:949d13045431 885
lakshya 20:949d13045431 886 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 887 {
lakshya 20:949d13045431 888 ATS1_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 889 }
lakshya 20:949d13045431 890 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 891 {
lakshya 20:949d13045431 892 ATS2_SENTRAL_STATUS_RGTR = (uint8_t)sentral;
lakshya 20:949d13045431 893 }
lakshya 20:949d13045431 894
lakshya 20:949d13045431 895 /// pc_acs.printf("Event Status is %x\n \r",event);
lakshya 20:949d13045431 896 /// pc_acs.printf("Sentral Status is %x\n \r",sentral);
lakshya 20:949d13045431 897
lakshya 17:fc782f7548c6 898
lakshya 17:fc782f7548c6 899
lakshya 20:949d13045431 900 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 901 {
lakshya 20:949d13045431 902
lakshya 20:949d13045431 903
lakshya 20:949d13045431 904 init = SENSOR_INIT();
lakshya 20:949d13045431 905
lakshya 20:949d13045431 906 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 907 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 908 if(ack!=0)
lakshya 20:949d13045431 909 {
lakshya 20:949d13045431 910 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 911 if(ack!=0)
lakshya 20:949d13045431 912 return 0;
lakshya 20:949d13045431 913 }
lakshya 20:949d13045431 914
lakshya 20:949d13045431 915 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 916 if(ack!=0)
lakshya 20:949d13045431 917 {
lakshya 20:949d13045431 918 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 919 if(ack!=0)
lakshya 20:949d13045431 920 return 0;
lakshya 20:949d13045431 921 }
lakshya 20:949d13045431 922
lakshya 20:949d13045431 923 event = (int)status;
lakshya 20:949d13045431 924
lakshya 20:949d13045431 925 cmd[0]=SENTRALSTATUS;
lakshya 20:949d13045431 926 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 927 if(ack!=0)
lakshya 20:949d13045431 928 {
lakshya 20:949d13045431 929 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 930 if(ack!=0)
lakshya 20:949d13045431 931 return 0;
lakshya 20:949d13045431 932 }
lakshya 20:949d13045431 933
lakshya 20:949d13045431 934 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 935 if(ack!=0)
lakshya 20:949d13045431 936 {
lakshya 20:949d13045431 937 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 938 if(ack!=0)
lakshya 20:949d13045431 939 return 0;
lakshya 20:949d13045431 940 }
lakshya 20:949d13045431 941
lakshya 20:949d13045431 942 sentral = (int)status;
lakshya 20:949d13045431 943
lakshya 20:949d13045431 944 /// pc_acs.printf("Event Status after resetting and init is %x\n \r",event);
lakshya 20:949d13045431 945
lakshya 20:949d13045431 946 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 947 {
lakshya 20:949d13045431 948
lakshya 20:949d13045431 949 cmd[0]=ERROR;
lakshya 20:949d13045431 950 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 951 if(ack!=0)
lakshya 20:949d13045431 952 {
lakshya 20:949d13045431 953 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 954 if(ack!=0)
lakshya 20:949d13045431 955 return 0;
lakshya 20:949d13045431 956 }
lakshya 20:949d13045431 957
lakshya 20:949d13045431 958 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 959 if(ack!=0)
lakshya 20:949d13045431 960 {
lakshya 20:949d13045431 961
lakshya 20:949d13045431 962 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 963 {
lakshya 20:949d13045431 964 ATS1_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 965 }
lakshya 20:949d13045431 966 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 967 {
lakshya 20:949d13045431 968 ATS2_ERROR_RGTR = 0x01;
lakshya 20:949d13045431 969 }
lakshya 20:949d13045431 970 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 971 if(ack!=0)
lakshya 20:949d13045431 972 return 0;
lakshya 20:949d13045431 973 }
lakshya 20:949d13045431 974
lakshya 20:949d13045431 975 error = (int)status;
lakshya 20:949d13045431 976
lakshya 20:949d13045431 977 if(ACS_ATS_STATUS&0xC0 == 0x40)
lakshya 20:949d13045431 978 {
lakshya 20:949d13045431 979 ATS1_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 980 }
lakshya 20:949d13045431 981 else if(ACS_ATS_STATUS&0x0C == 0x04)
lakshya 20:949d13045431 982 {
lakshya 20:949d13045431 983 ATS2_ERROR_RGTR = (uint8_t)error;
lakshya 20:949d13045431 984 }
lakshya 20:949d13045431 985
lakshya 20:949d13045431 986 cmd[0]=SENSORSTATUS;
lakshya 20:949d13045431 987 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 988 if(ack!=0)
lakshya 20:949d13045431 989 {
lakshya 20:949d13045431 990 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 991 if(ack!=0)
lakshya 20:949d13045431 992 return 0;
lakshya 20:949d13045431 993 }
lakshya 20:949d13045431 994
lakshya 20:949d13045431 995 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 996 if(ack!=0)
lakshya 20:949d13045431 997 {
lakshya 20:949d13045431 998 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 999 if(ack!=0)
lakshya 20:949d13045431 1000 return 0;
lakshya 20:949d13045431 1001 }
lakshya 20:949d13045431 1002
lakshya 20:949d13045431 1003 sensor = (int)status;
lakshya 20:949d13045431 1004
lakshya 20:949d13045431 1005
lakshya 20:949d13045431 1006 if((error!=0) || (sensor!=0))
lakshya 20:949d13045431 1007 {
lakshya 20:949d13045431 1008 if( (error&1 == 1) || (sensor&1 == 1) || (sensor&16 == 16) )
lakshya 20:949d13045431 1009 {
lakshya 20:949d13045431 1010 pc_acs.printf("error in gyro alone..\n \r");
lakshya 20:949d13045431 1011 gyro_error = 1;
lakshya 20:949d13045431 1012 }
lakshya 20:949d13045431 1013
lakshya 20:949d13045431 1014 if( (error&4 == 4) || (sensor&4 == 4) || (sensor&64 == 64) )
lakshya 20:949d13045431 1015 {
lakshya 20:949d13045431 1016
lakshya 20:949d13045431 1017 pc_acs.printf("error in mag alone.Exiting.\n \r");
lakshya 20:949d13045431 1018 mag_error = 1;
lakshya 20:949d13045431 1019 }
lakshya 20:949d13045431 1020 if( (gyro_error!=1)&&(mag_error!=1))
lakshya 20:949d13045431 1021 {
lakshya 20:949d13045431 1022 pc_acs.printf("error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1023 return 0;
lakshya 20:949d13045431 1024
lakshya 20:949d13045431 1025 }
lakshya 20:949d13045431 1026 }
lakshya 20:949d13045431 1027
lakshya 20:949d13045431 1028
lakshya 20:949d13045431 1029 if((event & 1 == 1 ))
lakshya 20:949d13045431 1030 {
lakshya 20:949d13045431 1031 /// pc_acs.printf("error in CPU Reset.\n \r");
lakshya 20:949d13045431 1032 return 0;
lakshya 20:949d13045431 1033
lakshya 20:949d13045431 1034 }
lakshya 20:949d13045431 1035
lakshya 20:949d13045431 1036 if((event & 8 != 8 )||(event & 32 != 32 ))
lakshya 20:949d13045431 1037 {
lakshya 20:949d13045431 1038 pc_acs.printf("Data not ready waiting...\n \r");
lakshya 20:949d13045431 1039 //POLL
lakshya 20:949d13045431 1040 wait_ms(200);
lakshya 20:949d13045431 1041
lakshya 20:949d13045431 1042 cmd[0]=EVT_STATUS;
lakshya 20:949d13045431 1043
lakshya 20:949d13045431 1044 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1045 if(ack!=0)
lakshya 20:949d13045431 1046 {
lakshya 20:949d13045431 1047 ack = i2c.write(SLAVE_ADDR,cmd,1);
lakshya 20:949d13045431 1048 if(ack!=0)
lakshya 20:949d13045431 1049 return 0;
lakshya 20:949d13045431 1050 }
lakshya 20:949d13045431 1051
lakshya 20:949d13045431 1052 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1053 if(ack!=0)
lakshya 20:949d13045431 1054 {
lakshya 20:949d13045431 1055 ack = i2c.read(SLAVE_ADDR_READ,&status,1);
lakshya 20:949d13045431 1056 if(ack!=0)
lakshya 20:949d13045431 1057 return 0;
lakshya 20:949d13045431 1058 }
lakshya 20:949d13045431 1059
lakshya 20:949d13045431 1060 event = (int)status;
lakshya 20:949d13045431 1061 if(event & 32 != 32 )
lakshya 20:949d13045431 1062 {
lakshya 20:949d13045431 1063
lakshya 20:949d13045431 1064 pc_acs.printf("Mag data only ready.Read..\n \r");
lakshya 20:949d13045431 1065 gyro_error = 1;
lakshya 20:949d13045431 1066
lakshya 20:949d13045431 1067 }
lakshya 20:949d13045431 1068
lakshya 20:949d13045431 1069 if(event & 8 != 8 )
lakshya 20:949d13045431 1070 {
lakshya 20:949d13045431 1071 pc_acs.printf("Both data still not ready.Exiting..\n \r");
lakshya 20:949d13045431 1072 mag_error=1;
lakshya 20:949d13045431 1073 }
lakshya 20:949d13045431 1074
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 if((mag_error !=1)&&(gyro_error!=1))
lakshya 20:949d13045431 1082 {
lakshya 20:949d13045431 1083 pc_acs.printf("Error in something else.Exiting.\n \r");
lakshya 20:949d13045431 1084 return 0;
lakshya 20:949d13045431 1085 }
lakshya 20:949d13045431 1086
lakshya 20:949d13045431 1087 if((mag_error ==1)&&(gyro_error==1))
lakshya 20:949d13045431 1088 {
lakshya 20:949d13045431 1089 pc_acs.printf("Error in both gyro and mag.Exiting.\n \r");
lakshya 20:949d13045431 1090 return 0;
lakshya 20:949d13045431 1091 }
lakshya 20:949d13045431 1092
lakshya 20:949d13045431 1093 }
lakshya 20:949d13045431 1094
lakshya 20:949d13045431 1095
lakshya 20:949d13045431 1096 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1097 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1098 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1099 if(ack != 0)
lakshya 20:949d13045431 1100 {
lakshya 20:949d13045431 1101 cmd[0]=MAG_XOUT_H; //LSB of x
lakshya 20:949d13045431 1102 i2c.write(SLAVE_ADDR,cmd,1); //Read gryo and mag registers together
lakshya 20:949d13045431 1103 ack = i2c.read(SLAVE_ADDR_READ,reg_data,24);
lakshya 20:949d13045431 1104 if(ack !=1)
lakshya 20:949d13045431 1105 return 0;
lakshya 20:949d13045431 1106
lakshya 20:949d13045431 1107 }
lakshya 20:949d13045431 1108
lakshya 20:949d13045431 1109
lakshya 20:949d13045431 1110 // pc_acs.printf("\nGyro Values:\n");
lakshya 20:949d13045431 1111 if (gyro_error!=1)
lakshya 20:949d13045431 1112 {
lakshya 20:949d13045431 1113 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1114 //concatenating gyro LSB and MSB to get 16 bit signed data values
lakshya 20:949d13045431 1115 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 1116 gyro_data[i]=(float)actual_data.bit_data_acs_mg[i];
lakshya 20:949d13045431 1117 gyro_data[i]=gyro_data[i]/senstivity_gyro;
lakshya 20:949d13045431 1118 actual_data.AngularSpeed_actual[i] = gyro_data[i];
lakshya 20:949d13045431 1119 }
lakshya 20:949d13045431 1120 }
lakshya 20:949d13045431 1121
lakshya 20:949d13045431 1122 if(mag_error!=1)
lakshya 20:949d13045431 1123 {
lakshya 20:949d13045431 1124 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1125 //concatenating mag LSB and MSB to get 16 bit signed data values Extract data
lakshya 20:949d13045431 1126 actual_data.bit_data_acs_mm[i]= ((int16_t)reg_data[2*i+1]<<8)|(int16_t)reg_data[2*i];
lakshya 20:949d13045431 1127
lakshya 20:949d13045431 1128 mag_data[i]=(float)actual_data.bit_data_acs_mm[i];
lakshya 20:949d13045431 1129 mag_data[i]=mag_data[i]/senstivity_mag;
lakshya 20:949d13045431 1130 actual_data.Bvalue_actual[i] = mag_data[i];
lakshya 20:949d13045431 1131 }
lakshya 20:949d13045431 1132 }
lakshya 20:949d13045431 1133
lakshya 20:949d13045431 1134
lakshya 20:949d13045431 1135 if(mag_error == 1)
lakshya 20:949d13045431 1136 {
lakshya 20:949d13045431 1137
lakshya 20:949d13045431 1138 pc_acs.printf("Gyro only successful.\n \r");
lakshya 20:949d13045431 1139 return 1;
lakshya 20:949d13045431 1140 }
lakshya 20:949d13045431 1141 if(gyro_error == 1)
lakshya 20:949d13045431 1142 {
lakshya 20:949d13045431 1143 pc_acs.printf("Mag only successful.\n \r");
lakshya 20:949d13045431 1144 return 2;
lakshya 20:949d13045431 1145 }
lakshya 17:fc782f7548c6 1146
lakshya 39:670133e7ffd8 1147 //pc_acs.printf("Reading data success.\n \r");
lakshya 20:949d13045431 1148 return 3;
lakshya 20:949d13045431 1149 }
lakshya 20:949d13045431 1150
lakshya 20:949d13045431 1151
lakshya 20:949d13045431 1152 int FCTN_ATS_DATA_ACQ()
lakshya 20:949d13045431 1153 {
lakshya 20:949d13045431 1154 for(int i=0; i<3; i++) {
lakshya 20:949d13045431 1155 actual_data.AngularSpeed_actual[i] = 0;
lakshya 20:949d13045431 1156 actual_data.Bvalue_actual[i] = 0;
lakshya 20:949d13045431 1157 }
lakshya 20:949d13045431 1158
lakshya 20:949d13045431 1159 int acq;
lakshya 20:949d13045431 1160 int init;
lakshya 20:949d13045431 1161
lakshya 20:949d13045431 1162 //// pc_acs.printf("DATA_ACQ called \n \r");
lakshya 20:949d13045431 1163 //// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1164
lakshya 20:949d13045431 1165
lakshya 20:949d13045431 1166 if(( (ACS_ATS_STATUS & 0xC0) == 0x40))
lakshya 20:949d13045431 1167 {
lakshya 20:949d13045431 1168
lakshya 20:949d13045431 1169 acq = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1170 if(acq == 3)
lakshya 20:949d13045431 1171 {
lakshya 20:949d13045431 1172
lakshya 20:949d13045431 1173 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1174
lakshya 20:949d13045431 1175 //??ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
lakshya 20:949d13045431 1176 //// pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1177 //// pc_acs.printf(" Sensor 1 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1178 return 3;
lakshya 20:949d13045431 1179 }
lakshya 20:949d13045431 1180 else if((acq == 2)||(acq==1))
lakshya 20:949d13045431 1181 {
lakshya 20:949d13045431 1182 pc_acs.printf(" Sensor 1 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1183 if( (ACS_ATS_STATUS & 0x0F == 0x03) ||((ACS_ATS_STATUS & 0x0F == 0x02)&&(acq==1))||((ACS_ATS_STATUS & 0x0F == 0x01)&&(acq==2)) )
lakshya 20:949d13045431 1184 {
lakshya 20:949d13045431 1185 //other sensor both working, off or
lakshya 20:949d13045431 1186 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1187 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1188
lakshya 20:949d13045431 1189 ATS1_SW_ENABLE = 1; //switch off sensor 1
lakshya 20:949d13045431 1190 wait_ms(5);
lakshya 20:949d13045431 1191 if(acq == 1)
lakshya 20:949d13045431 1192 {
lakshya 20:949d13045431 1193 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10; //Update sensor 1 status
lakshya 20:949d13045431 1194 }
lakshya 20:949d13045431 1195 if(acq==2)
lakshya 20:949d13045431 1196 {
lakshya 20:949d13045431 1197 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x20;
lakshya 20:949d13045431 1198 }
lakshya 20:949d13045431 1199
lakshya 20:949d13045431 1200 ATS2_SW_ENABLE = 0; //switch on sensor 2
lakshya 20:949d13045431 1201 wait_ms(5);
lakshya 20:949d13045431 1202
lakshya 20:949d13045431 1203 init = SENSOR_INIT(); //sensor 2 init
lakshya 20:949d13045431 1204 if( init == 0)
lakshya 20:949d13045431 1205 {
lakshya 20:949d13045431 1206 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1207 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1208 wait_ms(5);
lakshya 20:949d13045431 1209 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1210 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Update not working and switch back to 1
lakshya 20:949d13045431 1211 if(acq == 1)
lakshya 20:949d13045431 1212 {
lakshya 20:949d13045431 1213 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Update sensor 1 status
lakshya 20:949d13045431 1214 }
lakshya 20:949d13045431 1215 if(acq==2)
lakshya 20:949d13045431 1216 {
lakshya 20:949d13045431 1217 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1218 }
lakshya 20:949d13045431 1219 return acq;
lakshya 20:949d13045431 1220 }
lakshya 20:949d13045431 1221
lakshya 20:949d13045431 1222 int acq2;
lakshya 20:949d13045431 1223 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1224 if(acq2 == 3)
lakshya 20:949d13045431 1225 {
lakshya 20:949d13045431 1226 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 1227 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working, exit
lakshya 20:949d13045431 1228 return 3;
lakshya 20:949d13045431 1229 }
lakshya 20:949d13045431 1230 else if(acq2 == 1)
lakshya 20:949d13045431 1231 {
lakshya 20:949d13045431 1232 if(acq==2)
lakshya 20:949d13045431 1233 {
lakshya 20:949d13045431 1234 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1235 wait_ms(5);
lakshya 20:949d13045431 1236 ATS1_SW_ENABLE = 0; //Sensor 2 gyro only,sensor 1 mag only
lakshya 20:949d13045431 1237 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01;
lakshya 20:949d13045431 1238 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1239 return 3;
lakshya 20:949d13045431 1240 }
lakshya 20:949d13045431 1241 else
lakshya 20:949d13045431 1242 {
lakshya 20:949d13045431 1243 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Sensor 2 gyro only,sensor 1 gyro only
lakshya 20:949d13045431 1244 return 1;
lakshya 20:949d13045431 1245 }
lakshya 20:949d13045431 1246 }
lakshya 20:949d13045431 1247
lakshya 20:949d13045431 1248 else if(acq2==2) //Sensor 2 mag only, exit in both cases
lakshya 20:949d13045431 1249 {
lakshya 20:949d13045431 1250 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1251 return 2;
lakshya 20:949d13045431 1252 }
lakshya 20:949d13045431 1253 else if(acq2 == 0) //Sensor 2 not working, switch back to sensor 1
lakshya 20:949d13045431 1254 {
lakshya 20:949d13045431 1255 pc_acs.printf(" Sensor 2 data acq failure.Go to sensor 1 again.\n \r");
lakshya 20:949d13045431 1256 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1257 wait_ms(5); //In status change 00 to 01 for sensor 1, other two bits are same
lakshya 20:949d13045431 1258 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1259 wait_ms(5);
lakshya 20:949d13045431 1260 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x3F)|0x40;
lakshya 20:949d13045431 1261 return acq;
lakshya 20:949d13045431 1262 }
lakshya 20:949d13045431 1263
lakshya 20:949d13045431 1264 }
lakshya 20:949d13045431 1265 else //Sensor 2 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1266 {
lakshya 20:949d13045431 1267 if(acq == 1)
lakshya 20:949d13045431 1268 {
lakshya 20:949d13045431 1269 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //return Sensor 2 status and update acq
lakshya 20:949d13045431 1270 return 1;
lakshya 20:949d13045431 1271 }
lakshya 20:949d13045431 1272 if(acq==2)
lakshya 20:949d13045431 1273 {
lakshya 20:949d13045431 1274 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1275 return 2;
lakshya 20:949d13045431 1276 }
lakshya 20:949d13045431 1277 pc_acs.printf(" Sensor 1 data partial success.Sensor 2 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1278 return acq;
lakshya 20:949d13045431 1279
lakshya 20:949d13045431 1280 }
sakthipriya 0:7b4c00e3912f 1281 }
lakshya 20:949d13045431 1282
lakshya 20:949d13045431 1283 else if(acq == 0)
lakshya 20:949d13045431 1284 {
lakshya 20:949d13045431 1285 pc_acs.printf(" Sensor 1 data acq failure.Try sensor 2.\n \r"); //Sensor 1 not working at all
lakshya 20:949d13045431 1286 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1287 wait_ms(5); //Switch ON sensor 2
lakshya 20:949d13045431 1288 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1289 wait_ms(5);
lakshya 20:949d13045431 1290 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1291 if( (ACS_ATS_STATUS & 0x0C) == 0x00) //Sensor 2 is 00XX
lakshya 20:949d13045431 1292 {
lakshya 20:949d13045431 1293 init = SENSOR_INIT();
lakshya 20:949d13045431 1294 if( init == 0)
lakshya 20:949d13045431 1295 {
lakshya 20:949d13045431 1296 pc_acs.printf(" Sensor 2 also data acq failure.\n \r");
lakshya 20:949d13045431 1297 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1298 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C; //Sensor 2 also not working exit
lakshya 20:949d13045431 1299 return 0;
lakshya 20:949d13045431 1300 }
lakshya 20:949d13045431 1301
lakshya 20:949d13045431 1302 int acq2;
lakshya 20:949d13045431 1303 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1304 if(acq2 == 3)
lakshya 20:949d13045431 1305 {
lakshya 20:949d13045431 1306 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 1307 pc_acs.printf(" Sensor 2 data acq success.Exiting.\n \r"); //Sensor 2 working
lakshya 20:949d13045431 1308 return 3;
lakshya 20:949d13045431 1309 }
lakshya 20:949d13045431 1310 else if(acq2 == 1)
lakshya 20:949d13045431 1311 {
lakshya 20:949d13045431 1312 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05;
lakshya 20:949d13045431 1313 return 1;
lakshya 20:949d13045431 1314 }
lakshya 20:949d13045431 1315 else if(acq2 == 2)
lakshya 20:949d13045431 1316 {
lakshya 20:949d13045431 1317 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1318 return 2;
lakshya 20:949d13045431 1319 }
lakshya 20:949d13045431 1320 else if(acq2 == 0)
lakshya 20:949d13045431 1321 {
lakshya 20:949d13045431 1322 pc_acs.printf(" Sensor 2 data acq failure..\n \r");
lakshya 20:949d13045431 1323 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1324
lakshya 20:949d13045431 1325 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1326 return 0;
lakshya 20:949d13045431 1327 }
lakshya 20:949d13045431 1328
lakshya 20:949d13045431 1329 }
lakshya 20:949d13045431 1330
sakthipriya 0:7b4c00e3912f 1331 }
lakshya 20:949d13045431 1332
lakshya 20:949d13045431 1333
sakthipriya 0:7b4c00e3912f 1334 }
lakshya 20:949d13045431 1335
lakshya 20:949d13045431 1336 if(( (ACS_ATS_STATUS & 0x0C) == 0x04))
lakshya 20:949d13045431 1337 {
lakshya 20:949d13045431 1338 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 1339 if(acq == 3) //Both available read and exit
lakshya 20:949d13045431 1340 {
lakshya 20:949d13045431 1341 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x07;
lakshya 20:949d13045431 1342 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1343 pc_acs.printf(" Sensor 2 data acq successful.Exit Data ACQ\n \r");
lakshya 20:949d13045431 1344 return 3;
lakshya 20:949d13045431 1345 }
lakshya 20:949d13045431 1346 else if((acq == 2)||(acq==1)) //Only mag or only gyro
lakshya 20:949d13045431 1347 {
lakshya 20:949d13045431 1348 pc_acs.printf(" Sensor 2 data partial success.Try other sensor.\n \r");
lakshya 20:949d13045431 1349 if((ACS_ATS_STATUS & 0xF0 == 0x30) ||((ACS_ATS_STATUS & 0xF0 == 0x20)&&(acq==1))||((ACS_ATS_STATUS & 0xF0 == 0x10)&&(acq==2)) )
lakshya 20:949d13045431 1350 {
lakshya 20:949d13045431 1351 //other sensor both working, off or
lakshya 20:949d13045431 1352 //other sensor gyro working, this sensor not working , off
lakshya 20:949d13045431 1353 //other sensor mag working, this sensor not working,off
lakshya 20:949d13045431 1354 ATS2_SW_ENABLE = 1; //switch off sensor 2
lakshya 20:949d13045431 1355 wait_ms(5);
lakshya 20:949d13045431 1356 if(acq == 1)
lakshya 20:949d13045431 1357 {
lakshya 20:949d13045431 1358 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x01; //Update sensor 2 status
lakshya 20:949d13045431 1359 }
lakshya 20:949d13045431 1360 if(acq==2)
lakshya 20:949d13045431 1361 {
lakshya 20:949d13045431 1362 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x02;
lakshya 20:949d13045431 1363 }
lakshya 20:949d13045431 1364
lakshya 20:949d13045431 1365 ATS1_SW_ENABLE = 0; //switch on sensor 1
lakshya 20:949d13045431 1366 wait_ms(5);
lakshya 20:949d13045431 1367 init = SENSOR_INIT(); //sensor 2 init
lakshya 20:949d13045431 1368
lakshya 20:949d13045431 1369 if( init == 0)
lakshya 20:949d13045431 1370 {
lakshya 20:949d13045431 1371 pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
lakshya 20:949d13045431 1372 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1373 wait_ms(5);
lakshya 20:949d13045431 1374 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1375 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Update not working and switch back to 2
lakshya 20:949d13045431 1376 if(acq == 1)
lakshya 20:949d13045431 1377 {
lakshya 20:949d13045431 1378 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //Update sensor 1 status
lakshya 20:949d13045431 1379 }
lakshya 20:949d13045431 1380 if(acq==2)
lakshya 20:949d13045431 1381 {
lakshya 20:949d13045431 1382 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1383 }
lakshya 20:949d13045431 1384 return acq;
lakshya 20:949d13045431 1385 }
lakshya 20:949d13045431 1386
lakshya 20:949d13045431 1387 int acq2;
lakshya 20:949d13045431 1388 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1389
lakshya 20:949d13045431 1390 if(acq2 == 3)
lakshya 20:949d13045431 1391 {
lakshya 20:949d13045431 1392 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1393 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working, exit
lakshya 20:949d13045431 1394 return 3;
lakshya 20:949d13045431 1395 }
lakshya 20:949d13045431 1396
lakshya 20:949d13045431 1397 else if(acq2 == 1)
lakshya 20:949d13045431 1398 {
lakshya 20:949d13045431 1399 if(acq==2)
lakshya 20:949d13045431 1400 {
lakshya 20:949d13045431 1401 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1402 wait_ms(5);
lakshya 20:949d13045431 1403 ATS2_SW_ENABLE = 0; //Sensor 1 gyro only,sensor 2 mag only
lakshya 20:949d13045431 1404 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x10;
lakshya 20:949d13045431 1405 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1406 return 3;
lakshya 20:949d13045431 1407 }
lakshya 20:949d13045431 1408 else
lakshya 20:949d13045431 1409 {
lakshya 20:949d13045431 1410 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50; //Sensor 1 gyro only,sensor 2 gyro only
lakshya 20:949d13045431 1411 return 1;
lakshya 20:949d13045431 1412 }
lakshya 20:949d13045431 1413 }
lakshya 20:949d13045431 1414
lakshya 20:949d13045431 1415 else if(acq2==2) //Sensor 1 mag only, exit in both cases
lakshya 20:949d13045431 1416 {
lakshya 20:949d13045431 1417 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1418 return 2;
lakshya 20:949d13045431 1419 }
lakshya 20:949d13045431 1420 else if(acq2 == 0) //Sensor 1 not working, switch back to sensor 2
lakshya 20:949d13045431 1421 {
lakshya 20:949d13045431 1422 pc_acs.printf(" Sensor 1 data acq failure.Go to sensor 2 again.\n \r");
lakshya 20:949d13045431 1423 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1424 wait_ms(5); //In status change 00 to 01 for sensor 2, other two bits are same
lakshya 20:949d13045431 1425 ATS2_SW_ENABLE = 0;
lakshya 20:949d13045431 1426 wait_ms(5);
lakshya 20:949d13045431 1427 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF3)|0x04;
lakshya 20:949d13045431 1428 return acq;
lakshya 20:949d13045431 1429 }
lakshya 20:949d13045431 1430
lakshya 20:949d13045431 1431 }
lakshya 20:949d13045431 1432 else //Sensor 1 not working or both sensors gyro/mag ONLY
lakshya 20:949d13045431 1433 {
lakshya 20:949d13045431 1434 if(acq == 1)
lakshya 20:949d13045431 1435 {
lakshya 20:949d13045431 1436 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x05; //return Sensor 1 status and update acq
lakshya 20:949d13045431 1437 return 1;
lakshya 20:949d13045431 1438 }
lakshya 20:949d13045431 1439 if(acq==2)
lakshya 20:949d13045431 1440 {
lakshya 20:949d13045431 1441 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x06;
lakshya 20:949d13045431 1442 return 2;
lakshya 20:949d13045431 1443 }
lakshya 20:949d13045431 1444 pc_acs.printf(" Sensor 2 data partial success.Sensor 1 marked not working.Exiting.\n \r");
lakshya 20:949d13045431 1445 return acq;
lakshya 20:949d13045431 1446
lakshya 20:949d13045431 1447 }
lakshya 20:949d13045431 1448 }
lakshya 20:949d13045431 1449 else if(acq == 0)
lakshya 20:949d13045431 1450 {
lakshya 20:949d13045431 1451 pc_acs.printf(" Sensor 2 data acq failure.Try sensor 1.\n \r"); //Sensor 2 not working at all
lakshya 20:949d13045431 1452 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1453 wait_ms(5); //Switch ON sensor 1
lakshya 20:949d13045431 1454 ATS1_SW_ENABLE = 0;
lakshya 20:949d13045431 1455 wait_ms(5);
lakshya 20:949d13045431 1456 ACS_ATS_STATUS = (ACS_ATS_STATUS&0xF0)|0x0C;
lakshya 20:949d13045431 1457 if((ACS_ATS_STATUS & 0xC0) == 0x00) //Sensor 1 is 00XX
lakshya 20:949d13045431 1458 {
lakshya 20:949d13045431 1459 init = SENSOR_INIT();
lakshya 20:949d13045431 1460 if( init == 0)
lakshya 20:949d13045431 1461 {
lakshya 20:949d13045431 1462 pc_acs.printf(" Sensor 1 also data acq failure.\n \r");
lakshya 20:949d13045431 1463 ATS2_SW_ENABLE = 1;
lakshya 20:949d13045431 1464 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0; //Sensor 1 also not working exit
lakshya 20:949d13045431 1465 return 0;
lakshya 20:949d13045431 1466 }
lakshya 20:949d13045431 1467
lakshya 20:949d13045431 1468 int acq2;
lakshya 20:949d13045431 1469 acq2 = SENSOR_DATA_ACQ();
lakshya 20:949d13045431 1470 if(acq2 == 3)
lakshya 20:949d13045431 1471 {
lakshya 20:949d13045431 1472 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x70;
lakshya 20:949d13045431 1473 pc_acs.printf(" Sensor 1 data acq success.Exiting.\n \r"); //Sensor 1 working
lakshya 20:949d13045431 1474 return 3;
lakshya 20:949d13045431 1475 }
lakshya 20:949d13045431 1476 else if(acq2 == 1)
lakshya 20:949d13045431 1477 {
lakshya 20:949d13045431 1478 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x50;
lakshya 20:949d13045431 1479 return 1;
lakshya 20:949d13045431 1480 }
lakshya 20:949d13045431 1481 else if(acq2 == 2)
lakshya 20:949d13045431 1482 {
lakshya 20:949d13045431 1483 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0x60;
lakshya 20:949d13045431 1484 return 2;
lakshya 20:949d13045431 1485 }
lakshya 20:949d13045431 1486 else if(acq2 == 0)
lakshya 20:949d13045431 1487 {
lakshya 20:949d13045431 1488 pc_acs.printf(" Sensor 1 data acq failure..\n \r");
lakshya 20:949d13045431 1489 ATS1_SW_ENABLE = 1;
lakshya 20:949d13045431 1490 ACS_ATS_STATUS = (ACS_ATS_STATUS&0x0F)|0xC0;
lakshya 20:949d13045431 1491 return 0;
lakshya 20:949d13045431 1492 }
lakshya 20:949d13045431 1493 }
lakshya 20:949d13045431 1494 }
lakshya 20:949d13045431 1495 }
lakshya 20:949d13045431 1496 pc_acs.printf("ATS Status is %x\n\n \r",(int)ACS_ATS_STATUS);
lakshya 20:949d13045431 1497 pc_acs.printf(" Both sensors data acq failure.Exiting.\n \r");
lakshya 20:949d13045431 1498 return 0;
sakthipriya 0:7b4c00e3912f 1499 }
sakthipriya 0:7b4c00e3912f 1500
sakthipriya 0:7b4c00e3912f 1501 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 1502 {
lakshya 20:949d13045431 1503 //// printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 1504
sakthipriya 0:7b4c00e3912f 1505 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 1506 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 1507 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 1508 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 1509 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 1510 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 1511
sakthipriya 0:7b4c00e3912f 1512
lakshya 20:949d13045431 1513 //// printf("\r\r");
sakthipriya 0:7b4c00e3912f 1514
sakthipriya 0:7b4c00e3912f 1515 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 1516
sakthipriya 0:7b4c00e3912f 1517
sakthipriya 0:7b4c00e3912f 1518 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 1519
sakthipriya 0:7b4c00e3912f 1520 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 1521 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 1522 {
sakthipriya 0:7b4c00e3912f 1523 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 1524 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 1525 }
sakthipriya 0:7b4c00e3912f 1526
sakthipriya 0:7b4c00e3912f 1527 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 1528 //// printf("current in trx is %f \r \n",l_current_x);
lakshya 10:f93407b97750 1529 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 1530 {
lakshya 10:f93407b97750 1531 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 1532 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1533 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1534 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1535 }
lakshya 10:f93407b97750 1536 else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
lakshya 10:f93407b97750 1537 {
lakshya 10:f93407b97750 1538 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 1539 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1540 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1541 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1542 }
lakshya 10:f93407b97750 1543 else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
lakshya 10:f93407b97750 1544 {
lakshya 10:f93407b97750 1545 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 1546 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1547 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1548 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1549 }
lakshya 10:f93407b97750 1550 else if(l_current_x==0)
lakshya 10:f93407b97750 1551 {
lakshya 20:949d13045431 1552 //// printf("\n \r l_current_x====0");
lakshya 10:f93407b97750 1553 l_duty_cycle_x = 0; // default value of duty cycle
lakshya 20:949d13045431 1554 //// printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1555 PWM1.period(TIME_PERIOD);
lakshya 10:f93407b97750 1556 PWM1 = l_duty_cycle_x/100 ;
lakshya 10:f93407b97750 1557 }
lakshya 10:f93407b97750 1558 else //not necessary
lakshya 10:f93407b97750 1559 {
lakshya 10:f93407b97750 1560 g_err_flag_TR_x = 1;
lakshya 20:949d13045431 1561 }
lakshya 20:949d13045431 1562 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
lakshya 10:f93407b97750 1563
lakshya 10:f93407b97750 1564 //------------------------------------- y-direction TR--------------------------------------//
lakshya 10:f93407b97750 1565
lakshya 10:f93407b97750 1566
lakshya 10:f93407b97750 1567 float l_moment_y = Moment[1]; //Moment in y direction
lakshya 10:f93407b97750 1568
lakshya 10:f93407b97750 1569 phase_TR_y = 1; // setting the default current direction
lakshya 10:f93407b97750 1570 if (l_moment_y <0)
lakshya 10:f93407b97750 1571 {
lakshya 10:f93407b97750 1572 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 1573 l_moment_y = abs(l_moment_y);
lakshya 10:f93407b97750 1574 }
lakshya 10:f93407b97750 1575
lakshya 10:f93407b97750 1576
lakshya 10:f93407b97750 1577 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 1578 //// printf("current in try is %f \r \n",l_current_y);
lakshya 10:f93407b97750 1579 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 1580 {
lakshya 10:f93407b97750 1581 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 1582 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1583 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1584 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1585 }
lakshya 10:f93407b97750 1586 else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
lakshya 10:f93407b97750 1587 {
lakshya 10:f93407b97750 1588 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 1589 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1590 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1591 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1592 }
lakshya 10:f93407b97750 1593 else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
lakshya 10:f93407b97750 1594 {
lakshya 10:f93407b97750 1595 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 1596 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1597 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1598 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1599 }
lakshya 10:f93407b97750 1600 else if(l_current_y==0)
lakshya 10:f93407b97750 1601 {
lakshya 20:949d13045431 1602 //// printf("\n \r l_current_y====0");
lakshya 10:f93407b97750 1603 l_duty_cycle_y = 0; // default value of duty cycle
lakshya 20:949d13045431 1604 //// printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1605 PWM2.period(TIME_PERIOD);
lakshya 10:f93407b97750 1606 PWM2 = l_duty_cycle_y/100 ;
lakshya 10:f93407b97750 1607 }
lakshya 10:f93407b97750 1608 else // not necessary
lakshya 10:f93407b97750 1609 {
lakshya 10:f93407b97750 1610 g_err_flag_TR_y = 1;
lakshya 10:f93407b97750 1611 }
lakshya 20:949d13045431 1612 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
lakshya 10:f93407b97750 1613 //----------------------------------------------- z-direction TR -------------------------//
lakshya 10:f93407b97750 1614
lakshya 20:949d13045431 1615
lakshya 20:949d13045431 1616
lakshya 10:f93407b97750 1617 float l_moment_z = Moment[2]; //Moment in z direction
lakshya 10:f93407b97750 1618
lakshya 20:949d13045431 1619 phase_TR_z = 1; // setting the default current direction
lakshya 10:f93407b97750 1620 if (l_moment_z <0)
lakshya 10:f93407b97750 1621 {
lakshya 20:949d13045431 1622 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 1623 l_moment_z = abs(l_moment_z);
lakshya 10:f93407b97750 1624 }
lakshya 10:f93407b97750 1625
lakshya 10:f93407b97750 1626
lakshya 10:f93407b97750 1627 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
lakshya 20:949d13045431 1628 //// printf("current in trz is %f \r \n",l_current_z);
lakshya 20:949d13045431 1629 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 1630 {
lakshya 10:f93407b97750 1631 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 1632 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1633 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1634 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1635 }
lakshya 10:f93407b97750 1636 else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
lakshya 10:f93407b97750 1637 {
lakshya 10:f93407b97750 1638 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 1639 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1640 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1641 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1642 }
lakshya 10:f93407b97750 1643 else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
lakshya 10:f93407b97750 1644 {
lakshya 10:f93407b97750 1645 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 1646 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1647 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1648 PWM3 = l_duty_cycle_z/100 ;
lakshya 20:949d13045431 1649 }
lakshya 10:f93407b97750 1650 else if(l_current_z==0)
lakshya 10:f93407b97750 1651 {
lakshya 20:949d13045431 1652 //// printf("\n \r l_current_z====0");
lakshya 10:f93407b97750 1653 l_duty_cycle_z = 0; // default value of duty cycle
lakshya 20:949d13045431 1654 //// printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1655 PWM3.period(TIME_PERIOD);
lakshya 10:f93407b97750 1656 PWM3 = l_duty_cycle_z/100 ;
lakshya 10:f93407b97750 1657 }
lakshya 10:f93407b97750 1658 else // not necessary
lakshya 10:f93407b97750 1659 {
lakshya 20:949d13045431 1660 g_err_flag_TR_z = 1;
lakshya 20:949d13045431 1661 }
lakshya 20:949d13045431 1662 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
lakshya 10:f93407b97750 1663
lakshya 20:949d13045431 1664 //changed
lakshya 20:949d13045431 1665 if(phase_TR_x)
lakshya 20:949d13045431 1666 ACS_TR_X_PWM = float_to_uint8(-1,1,PWM1);
lakshya 20:949d13045431 1667 else
lakshya 20:949d13045431 1668 ACS_TR_X_PWM = float_to_uint8(-1,1,-PWM1);
lakshya 20:949d13045431 1669 if(phase_TR_y)
lakshya 20:949d13045431 1670 ACS_TR_Y_PWM = float_to_uint8(-1,1,PWM2);
lakshya 20:949d13045431 1671 else
lakshya 20:949d13045431 1672 ACS_TR_Y_PWM = float_to_uint8(-1,1,-PWM2);
lakshya 20:949d13045431 1673 if(phase_TR_z)
lakshya 20:949d13045431 1674 ACS_TR_Z_PWM = float_to_uint8(-1,1,PWM3);
lakshya 20:949d13045431 1675 else
lakshya 20:949d13045431 1676 ACS_TR_Z_PWM = float_to_uint8(-1,1,-PWM2);
lakshya 20:949d13045431 1677
lakshya 10:f93407b97750 1678 //-----------------------------------------exiting the function-----------------------------------//
lakshya 10:f93407b97750 1679
lakshya 20:949d13045431 1680 //// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
lakshya 10:f93407b97750 1681
lakshya 10:f93407b97750 1682 }
lakshya 20:949d13045431 1683