Team Fox / Mbed 2 deprecated QM_BAE_review_1_EPS_updated

Updated EPS code with flowchart v2.3 (CDMS and HW faults)

Dependencies:   FreescaleIAP mbed-rtos mbed

Fork of QM_BAE_review_1_EPS_faults by Mohamed Azad

ACS.cpp@9:194afacf7449, 2016-04-01 (annotated)

Committer:
sakthipriya
Date:
Fri Apr 01 21:13:16 2016 +0000
Revision:
9:194afacf7449
Parent:
8:82250e41da81
Child:
10:f93407b97750
updating 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"
sakthipriya 0:7b4c00e3912f 8 #include "ACS.h"
sakthipriya 6:036d08b62785 9 #include "EPS.h"
sakthipriya 0:7b4c00e3912f 10
sakthipriya 0:7b4c00e3912f 11 //********************************flags******************************************//
sakthipriya 0:7b4c00e3912f 12 extern uint32_t BAE_STATUS;
sakthipriya 0:7b4c00e3912f 13 extern uint32_t BAE_ENABLE;
sakthipriya 9:194afacf7449 14 extern uint8_t ACS_INIT_STATUS;
sakthipriya 9:194afacf7449 15 extern uint8_t ACS_DATA_ACQ_STATUS;
sakthipriya 9:194afacf7449 16 extern uint8_t ACS_ATS_STATUS;
sakthipriya 9:194afacf7449 17 extern uint8_t ACS_MAIN_STATUS;
sakthipriya 9:194afacf7449 18 extern uint8_t ACS_STATUS;
sakthipriya 0:7b4c00e3912f 19
sakthipriya 9:194afacf7449 20 extern uint8_t ACS_ATS_ENABLE;
sakthipriya 9:194afacf7449 21 extern uint8_t ACS_DATA_ACQ_ENABLE;
sakthipriya 9:194afacf7449 22 extern uint8_t ACS_STATE;
sakthipriya 0:7b4c00e3912f 23
sakthipriya 0:7b4c00e3912f 24 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
sakthipriya 0:7b4c00e3912f 25 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
sakthipriya 0:7b4c00e3912f 26 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
sakthipriya 0:7b4c00e3912f 27
sakthipriya 0:7b4c00e3912f 28 extern PwmOut PWM1; //x //Functions used to generate PWM signal
sakthipriya 0:7b4c00e3912f 29 extern PwmOut PWM2; //y
sakthipriya 0:7b4c00e3912f 30 extern PwmOut PWM3; //z //PWM output comes from pins p6
sakthipriya 0:7b4c00e3912f 31
sakthipriya 0:7b4c00e3912f 32 int g_err_flag_TR_x=0; // setting x-flag to zero
sakthipriya 0:7b4c00e3912f 33 int g_err_flag_TR_y=0; // setting y-flag to zero
sakthipriya 0:7b4c00e3912f 34 int g_err_flag_TR_z=0; // setting z-flag to zero
sakthipriya 0:7b4c00e3912f 35
sakthipriya 0:7b4c00e3912f 36 extern float data[6];
sakthipriya 6:036d08b62785 37 extern BAE_HK_actual actual_data;
sakthipriya 0:7b4c00e3912f 38
sakthipriya 0:7b4c00e3912f 39
sakthipriya 9:194afacf7449 40 //DigitalOut ATS1_SW_ENABLE(PTC0); // enable of att sens2 switch
sakthipriya 9:194afacf7449 41 //DigitalOut ATS2_SW_ENABLE(PTC16); // enable of att sens switch
sakthipriya 0:7b4c00e3912f 42
sakthipriya 0:7b4c00e3912f 43
sakthipriya 0:7b4c00e3912f 44 Serial pc_acs(USBTX,USBRX); //for usb communication
sakthipriya 0:7b4c00e3912f 45 void inverse(float mat[3][3],float inv[3][3]);
sakthipriya 0:7b4c00e3912f 46
sakthipriya 0:7b4c00e3912f 47 int ctrl_count = 0;
sakthipriya 0:7b4c00e3912f 48 float bcopy[3];
sakthipriya 0:7b4c00e3912f 49 float moment[3];
sakthipriya 0:7b4c00e3912f 50 ///////algo working well
sakthipriya 0:7b4c00e3912f 51 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
sakthipriya 0:7b4c00e3912f 52 {
sakthipriya 0:7b4c00e3912f 53 float db[3];
sakthipriya 0:7b4c00e3912f 54 float bb[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 55 float d[3]={0,0,0};
sakthipriya 0:7b4c00e3912f 56 float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
sakthipriya 0:7b4c00e3912f 57 float den=0,den2;
sakthipriya 0:7b4c00e3912f 58 int i,j; //temporary variables
sakthipriya 0:7b4c00e3912f 59 float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs
sakthipriya 0:7b4c00e3912f 60 float invJm[3][3];
sakthipriya 0:7b4c00e3912f 61 float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
sakthipriya 0:7b4c00e3912f 62
sakthipriya 0:7b4c00e3912f 63 //................. calculating db values...........................
sakthipriya 0:7b4c00e3912f 64 if(ctrl_count!=0)
sakthipriya 0:7b4c00e3912f 65 {
sakthipriya 0:7b4c00e3912f 66 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 67 db[i]= (b[i]-bcopy[i])/10;
sakthipriya 0:7b4c00e3912f 68 }
sakthipriya 0:7b4c00e3912f 69 else
sakthipriya 0:7b4c00e3912f 70 {
sakthipriya 0:7b4c00e3912f 71 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 72 db[i]= 0;
sakthipriya 0:7b4c00e3912f 73 }
sakthipriya 0:7b4c00e3912f 74 ctrl_count++;
sakthipriya 0:7b4c00e3912f 75 //..................................................................
sakthipriya 0:7b4c00e3912f 76 printf("\n\r Entered cntrl algo\n\r");
sakthipriya 0:7b4c00e3912f 77 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 78 {
sakthipriya 0:7b4c00e3912f 79 printf("%f\t",omega[i]);
sakthipriya 0:7b4c00e3912f 80 }
sakthipriya 0:7b4c00e3912f 81 for(int i=0; i<3; i++)
sakthipriya 0:7b4c00e3912f 82 {
sakthipriya 0:7b4c00e3912f 83 printf("%f\t",b[i]);
sakthipriya 0:7b4c00e3912f 84 }
sakthipriya 0:7b4c00e3912f 85
sakthipriya 0:7b4c00e3912f 86 //.........................algo......................................
sakthipriya 0:7b4c00e3912f 87 den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
sakthipriya 0:7b4c00e3912f 88 den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
sakthipriya 0:7b4c00e3912f 89 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 90 {
sakthipriya 0:7b4c00e3912f 91 db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
sakthipriya 0:7b4c00e3912f 92 //db[i]/=den*den*den;
sakthipriya 0:7b4c00e3912f 93 }
sakthipriya 0:7b4c00e3912f 94 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 95 {
sakthipriya 0:7b4c00e3912f 96 b[i]/=den;
sakthipriya 0:7b4c00e3912f 97 }
sakthipriya 0:7b4c00e3912f 98 // select kz, kmu, gamma
sakthipriya 0:7b4c00e3912f 99 if(b[0]>0.9||b[0]<-0.9)
sakthipriya 0:7b4c00e3912f 100 {
sakthipriya 0:7b4c00e3912f 101 kz=kz2;
sakthipriya 0:7b4c00e3912f 102 kmu=kmu2;
sakthipriya 0:7b4c00e3912f 103 gamma=gamma2;
sakthipriya 0:7b4c00e3912f 104 }
sakthipriya 0:7b4c00e3912f 105 // calculate Mu, v, dv, z, u
sakthipriya 0:7b4c00e3912f 106 for(i=0;i<2;i++)
sakthipriya 0:7b4c00e3912f 107 {
sakthipriya 0:7b4c00e3912f 108 Mu[i]=b[i+1];
sakthipriya 0:7b4c00e3912f 109 v[i]=-kmu*Mu[i];
sakthipriya 0:7b4c00e3912f 110 dv[i]=-kmu*db[i+1];
sakthipriya 0:7b4c00e3912f 111 z[i]=db[i+1]-v[i];
sakthipriya 0:7b4c00e3912f 112 u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
sakthipriya 0:7b4c00e3912f 113 }
sakthipriya 0:7b4c00e3912f 114 inverse(Jm,invJm);
sakthipriya 0:7b4c00e3912f 115 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 116 {
sakthipriya 0:7b4c00e3912f 117 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 118 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
sakthipriya 0:7b4c00e3912f 119 }
sakthipriya 0:7b4c00e3912f 120 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 121 {
sakthipriya 0:7b4c00e3912f 122 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 123 d[i]+=bb[j]*invJm[i][j];
sakthipriya 0:7b4c00e3912f 124 }
sakthipriya 0:7b4c00e3912f 125 bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
sakthipriya 0:7b4c00e3912f 126 bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:7b4c00e3912f 127 bb[0]=0;
sakthipriya 0:7b4c00e3912f 128 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 129 {
sakthipriya 0:7b4c00e3912f 130 d[i]=invJm[1][i];
sakthipriya 0:7b4c00e3912f 131 invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
sakthipriya 0:7b4c00e3912f 132 invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
sakthipriya 0:7b4c00e3912f 133 invJm[0][i]=b[i];
sakthipriya 0:7b4c00e3912f 134 }
sakthipriya 0:7b4c00e3912f 135 inverse(invJm,Jm);
sakthipriya 0:7b4c00e3912f 136 printf("\n \r calculating tauc");
sakthipriya 0:7b4c00e3912f 137 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 138 {
sakthipriya 0:7b4c00e3912f 139 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 140 tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values
sakthipriya 0:7b4c00e3912f 141 printf(" %f \t",tauc[i]);
sakthipriya 0:7b4c00e3912f 142 }
sakthipriya 0:7b4c00e3912f 143 //..........................tauc to moment conversion..........................
sakthipriya 0:7b4c00e3912f 144 printf("\n \r calculating moment");
sakthipriya 0:7b4c00e3912f 145 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 146 bcopy[i]=b[i]*den;
sakthipriya 0:7b4c00e3912f 147 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 148 {
sakthipriya 0:7b4c00e3912f 149 moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
sakthipriya 0:7b4c00e3912f 150 moment[i]/=den;
sakthipriya 0:7b4c00e3912f 151 printf(" %f \t",moment[i]);
sakthipriya 0:7b4c00e3912f 152 }
sakthipriya 0:7b4c00e3912f 153 printf("\n\r exited control algo\n");
sakthipriya 0:7b4c00e3912f 154 }
sakthipriya 0:7b4c00e3912f 155 //..........................function to find inverse..................
sakthipriya 0:7b4c00e3912f 156 void inverse(float mat[3][3],float inv[3][3])
sakthipriya 0:7b4c00e3912f 157 {
sakthipriya 0:7b4c00e3912f 158 int i,j;
sakthipriya 0:7b4c00e3912f 159 float det=0;
sakthipriya 0:7b4c00e3912f 160 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 161 {
sakthipriya 0:7b4c00e3912f 162 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 163 inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
sakthipriya 0:7b4c00e3912f 164 }
sakthipriya 0:7b4c00e3912f 165 det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
sakthipriya 0:7b4c00e3912f 166 for(i=0;i<3;i++)
sakthipriya 0:7b4c00e3912f 167 {
sakthipriya 0:7b4c00e3912f 168 for(j=0;j<3;j++)
sakthipriya 0:7b4c00e3912f 169 inv[i][j]/=det;
sakthipriya 0:7b4c00e3912f 170 }
sakthipriya 0:7b4c00e3912f 171 }
sakthipriya 0:7b4c00e3912f 172
sakthipriya 0:7b4c00e3912f 173
sakthipriya 0:7b4c00e3912f 174 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge
sakthipriya 0:7b4c00e3912f 175
sakthipriya 0:7b4c00e3912f 176 void FCTN_ACS_INIT(void); //initialization of registers happens
sakthipriya 0:7b4c00e3912f 177 void FCTN_ATS_DATA_ACQ(); //data is obtained
sakthipriya 0:7b4c00e3912f 178 void T_OUT(); //timeout function to stop infinite loop
sakthipriya 0:7b4c00e3912f 179 Timeout to; //Timeout variable to
sakthipriya 0:7b4c00e3912f 180 int toFlag;
sakthipriya 0:7b4c00e3912f 181
sakthipriya 0:7b4c00e3912f 182 int count =0; // Time for which the BAE uC is running (in seconds)
sakthipriya 0:7b4c00e3912f 183 void T_OUT()
sakthipriya 0:7b4c00e3912f 184 {
sakthipriya 0:7b4c00e3912f 185 toFlag=0; //as T_OUT function gets called the while loop gets terminated
sakthipriya 0:7b4c00e3912f 186 }
sakthipriya 0:7b4c00e3912f 187
sakthipriya 0:7b4c00e3912f 188
sakthipriya 0:7b4c00e3912f 189 //DEFINING VARIABLES
sakthipriya 0:7b4c00e3912f 190 char cmd[2];
sakthipriya 0:7b4c00e3912f 191 char raw_gyro[6];
sakthipriya 0:7b4c00e3912f 192 char raw_mag[6];
sakthipriya 0:7b4c00e3912f 193 char store,status;
sakthipriya 0:7b4c00e3912f 194 int16_t bit_data;
sakthipriya 0:7b4c00e3912f 195 float gyro_data[3], mag_data[3],combined_values[6];
sakthipriya 0:7b4c00e3912f 196 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
sakthipriya 0:7b4c00e3912f 197 float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla
sakthipriya 0:7b4c00e3912f 198 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
sakthipriya 0:7b4c00e3912f 199
sakthipriya 0:7b4c00e3912f 200 void FCTN_ACS_INIT()
sakthipriya 0:7b4c00e3912f 201 {
sakthipriya 9:194afacf7449 202 ACS_INIT_STATUS = 1; //set ACS_INIT_STATUS flag
sakthipriya 3:07e15677a75c 203 //FLAG();
sakthipriya 0:7b4c00e3912f 204 pc_acs.printf("Attitude sensor init called \n \r");
sakthipriya 0:7b4c00e3912f 205 //FLAG();
sakthipriya 0:7b4c00e3912f 206 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 207 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 208 i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
sakthipriya 0:7b4c00e3912f 209 wait_ms(2000); //waiting for loading configuration file stored in EEPROM
sakthipriya 0:7b4c00e3912f 210 cmd[0]=SENTRALSTATUS;
sakthipriya 0:7b4c00e3912f 211 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 212 i2c.read(SLAVE_ADDR_READ,&store,1);
sakthipriya 0:7b4c00e3912f 213 wait_ms(100);
sakthipriya 0:7b4c00e3912f 214 //to check whether EEPROM is uploaded
sakthipriya 0:7b4c00e3912f 215 switch((int)store) {
sakthipriya 0:7b4c00e3912f 216 case(3): {
sakthipriya 0:7b4c00e3912f 217 break;
sakthipriya 0:7b4c00e3912f 218 }
sakthipriya 0:7b4c00e3912f 219 case(11): {
sakthipriya 0:7b4c00e3912f 220 break;
sakthipriya 0:7b4c00e3912f 221 }
sakthipriya 0:7b4c00e3912f 222 default: {
sakthipriya 0:7b4c00e3912f 223 cmd[0]=RESETREQ;
sakthipriya 0:7b4c00e3912f 224 cmd[1]=BIT_RESREQ;
sakthipriya 0:7b4c00e3912f 225 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 226 wait_ms(2000);
sakthipriya 0:7b4c00e3912f 227 }
sakthipriya 0:7b4c00e3912f 228 }
sakthipriya 0:7b4c00e3912f 229 pc_acs.printf("Sentral Status is %x\n \r",(int)store);
sakthipriya 0:7b4c00e3912f 230 cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
sakthipriya 0:7b4c00e3912f 231 cmd[1]=BIT_RUN_ENB;
sakthipriya 0:7b4c00e3912f 232 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 233 wait_ms(100);
sakthipriya 0:7b4c00e3912f 234 cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
sakthipriya 0:7b4c00e3912f 235 cmd[1]=BIT_MAGODR;
sakthipriya 0:7b4c00e3912f 236 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 237 wait_ms(100);
sakthipriya 0:7b4c00e3912f 238 cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
sakthipriya 0:7b4c00e3912f 239 cmd[1]=BIT_GYROODR;
sakthipriya 0:7b4c00e3912f 240 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 241 wait_ms(100);
sakthipriya 0:7b4c00e3912f 242 cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
sakthipriya 0:7b4c00e3912f 243 cmd[1]=0x00;
sakthipriya 0:7b4c00e3912f 244 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 245 wait_ms(100);
sakthipriya 0:7b4c00e3912f 246 cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
sakthipriya 0:7b4c00e3912f 247 cmd[1]=BIT_EVT_ENB;
sakthipriya 0:7b4c00e3912f 248 i2c.write(SLAVE_ADDR,cmd,2);
sakthipriya 0:7b4c00e3912f 249 wait_ms(100);
sakthipriya 9:194afacf7449 250 ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
sakthipriya 0:7b4c00e3912f 251 }
sakthipriya 0:7b4c00e3912f 252
sakthipriya 0:7b4c00e3912f 253 void FCTN_ATS_DATA_ACQ()
sakthipriya 0:7b4c00e3912f 254 {
sakthipriya 9:194afacf7449 255 ACS_DATA_ACQ_STATUS = 1; //set ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 9:194afacf7449 256 if( ACS_ATS_ENABLE == 1)
sakthipriya 0:7b4c00e3912f 257 {
sakthipriya 0:7b4c00e3912f 258 FLAG();
sakthipriya 0:7b4c00e3912f 259 pc_acs.printf("attitude sensor execution called \n \r");
sakthipriya 0:7b4c00e3912f 260 toFlag=1; //toFlag is set to 1 so that it enters while loop
sakthipriya 0:7b4c00e3912f 261 to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated
sakthipriya 0:7b4c00e3912f 262 while(toFlag) {
sakthipriya 0:7b4c00e3912f 263 cmd[0]=EVT_STATUS;
sakthipriya 0:7b4c00e3912f 264 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 265 i2c.read(SLAVE_ADDR_READ,&status,1);
sakthipriya 0:7b4c00e3912f 266 wait_ms(100);
sakthipriya 0:7b4c00e3912f 267 pc_acs.printf("Event Status is %x\n \r",(int)status);
sakthipriya 0:7b4c00e3912f 268 //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
sakthipriya 0:7b4c00e3912f 269 if(((int)status&40)==40) {
sakthipriya 0:7b4c00e3912f 270 cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x
sakthipriya 0:7b4c00e3912f 271 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 272 i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
sakthipriya 0:7b4c00e3912f 273 cmd[0]=MAG_XOUT_H; //LSB of x
sakthipriya 0:7b4c00e3912f 274 i2c.write(SLAVE_ADDR,cmd,1);
sakthipriya 0:7b4c00e3912f 275 i2c.read(SLAVE_ADDR_READ,raw_mag,6);
sakthipriya 0:7b4c00e3912f 276 // pc_acs.printf("\nGyro Values:\n");
sakthipriya 0:7b4c00e3912f 277 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 278 //concatenating gyro LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 279 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i];
sakthipriya 0:7b4c00e3912f 280 gyro_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 281 gyro_data[i]=gyro_data[i]/senstivity_gyro;
sakthipriya 0:7b4c00e3912f 282 gyro_data[i]+=gyro_error[i];
sakthipriya 0:7b4c00e3912f 283 // pc_acs.printf("%f\t",gyro_data[i]);
sakthipriya 0:7b4c00e3912f 284 }
sakthipriya 0:7b4c00e3912f 285 // pc_acs.printf("\nMag Values:\n");
sakthipriya 0:7b4c00e3912f 286 for(int i=0; i<3; i++) {
sakthipriya 0:7b4c00e3912f 287 //concatenating mag LSB and MSB to get 16 bit signed data values
sakthipriya 0:7b4c00e3912f 288 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
sakthipriya 0:7b4c00e3912f 289 mag_data[i]=(float)bit_data;
sakthipriya 0:7b4c00e3912f 290 mag_data[i]=mag_data[i]/senstivity_mag;
sakthipriya 0:7b4c00e3912f 291 mag_data[i]+=mag_error[i];
sakthipriya 0:7b4c00e3912f 292 // pc_acs.printf("%f\t",mag_data[i]);
sakthipriya 0:7b4c00e3912f 293 }
sakthipriya 0:7b4c00e3912f 294 for(int i=0; i<3; i++) {
sakthipriya 6:036d08b62785 295 // data[i]=gyro_data[i];
sakthipriya 6:036d08b62785 296 actual_data.AngularSpeed_actual[i] = gyro_data[i];
sakthipriya 6:036d08b62785 297 actual_data.Bvalue_actual[i] = mag_data[i];
sakthipriya 6:036d08b62785 298 //data[i+3]=mag_data[i];
sakthipriya 0:7b4c00e3912f 299 }
sakthipriya 0:7b4c00e3912f 300 // return(combined_values); //returning poiter combined values
sakthipriya 0:7b4c00e3912f 301 }
sakthipriya 0:7b4c00e3912f 302 //checking for the error
sakthipriya 0:7b4c00e3912f 303 else if (((int)status&2)==2) {
sakthipriya 0:7b4c00e3912f 304 FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
sakthipriya 0:7b4c00e3912f 305 }
sakthipriya 0:7b4c00e3912f 306 }
sakthipriya 0:7b4c00e3912f 307 }
sakthipriya 0:7b4c00e3912f 308 else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
sakthipriya 0:7b4c00e3912f 309 {
sakthipriya 9:194afacf7449 310 ACS_DATA_ACQ_STATUS = 1;
sakthipriya 0:7b4c00e3912f 311 }
sakthipriya 9:194afacf7449 312 ACS_DATA_ACQ_STATUS = 0; //clear ACS_DATA_ACQ_STATUS flag for att sens 2
sakthipriya 0:7b4c00e3912f 313 }
sakthipriya 0:7b4c00e3912f 314
sakthipriya 0:7b4c00e3912f 315 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
sakthipriya 0:7b4c00e3912f 316 {
sakthipriya 0:7b4c00e3912f 317 printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
sakthipriya 0:7b4c00e3912f 318
sakthipriya 0:7b4c00e3912f 319 float l_duty_cycle_x=0; //Duty cycle of Moment in x direction
sakthipriya 0:7b4c00e3912f 320 float l_current_x=0; //Current sent in x TR's
sakthipriya 0:7b4c00e3912f 321 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction
sakthipriya 0:7b4c00e3912f 322 float l_current_y=0; //Current sent in y TR's
sakthipriya 0:7b4c00e3912f 323 float l_duty_cycle_z=0; //Duty cycle of Moment in z direction
sakthipriya 0:7b4c00e3912f 324 float l_current_z=0; //Current sent in z TR's
sakthipriya 0:7b4c00e3912f 325
sakthipriya 0:7b4c00e3912f 326
sakthipriya 0:7b4c00e3912f 327 for(int i = 0 ; i<3;i++)
sakthipriya 0:7b4c00e3912f 328 {
sakthipriya 0:7b4c00e3912f 329 // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs
sakthipriya 0:7b4c00e3912f 330 }
sakthipriya 0:7b4c00e3912f 331
sakthipriya 0:7b4c00e3912f 332 //----------------------------- x-direction TR --------------------------------------------//
sakthipriya 0:7b4c00e3912f 333
sakthipriya 0:7b4c00e3912f 334
sakthipriya 0:7b4c00e3912f 335 float l_moment_x = Moment[0]; //Moment in x direction
sakthipriya 0:7b4c00e3912f 336
sakthipriya 0:7b4c00e3912f 337 phase_TR_x = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 338 if (l_moment_x <0)
sakthipriya 0:7b4c00e3912f 339 {
sakthipriya 0:7b4c00e3912f 340 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 341 l_moment_x = abs(l_moment_x);
sakthipriya 0:7b4c00e3912f 342 }
sakthipriya 0:7b4c00e3912f 343
sakthipriya 0:7b4c00e3912f 344 l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 345 pc_acs.printf("current in trx is %f \r \n",l_current_x);
sakthipriya 0:7b4c00e3912f 346 if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 347 {
sakthipriya 0:7b4c00e3912f 348 l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 349 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 350 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 351 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 8:82250e41da81 352
sakthipriya 0:7b4c00e3912f 353 }
sakthipriya 0:7b4c00e3912f 354 else if( l_current_x >= 0.006 && l_current_x < 0.0116)
sakthipriya 0:7b4c00e3912f 355 {
sakthipriya 0:7b4c00e3912f 356 l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 357 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 358 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 359 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 360 }
sakthipriya 0:7b4c00e3912f 361 else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
sakthipriya 0:7b4c00e3912f 362 {
sakthipriya 0:7b4c00e3912f 363 l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 364 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 365 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 366 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 367 }
sakthipriya 0:7b4c00e3912f 368 else if(l_current_x >= 0.0624 && l_current_x < 0.555)
sakthipriya 0:7b4c00e3912f 369 {
sakthipriya 0:7b4c00e3912f 370 l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 371 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 372 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 373 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 374 }
sakthipriya 0:7b4c00e3912f 375 else if(l_current_x==0)
sakthipriya 0:7b4c00e3912f 376 {
sakthipriya 0:7b4c00e3912f 377 printf("\n \r l_current_x====0");
sakthipriya 0:7b4c00e3912f 378 l_duty_cycle_x = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 379 pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
sakthipriya 0:7b4c00e3912f 380 PWM1.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 381 PWM1 = l_duty_cycle_x/100 ;
sakthipriya 0:7b4c00e3912f 382 }
sakthipriya 0:7b4c00e3912f 383 else //not necessary
sakthipriya 0:7b4c00e3912f 384 {
sakthipriya 0:7b4c00e3912f 385 g_err_flag_TR_x = 1;
sakthipriya 0:7b4c00e3912f 386 }
sakthipriya 0:7b4c00e3912f 387
sakthipriya 0:7b4c00e3912f 388 //------------------------------------- y-direction TR--------------------------------------//
sakthipriya 0:7b4c00e3912f 389
sakthipriya 0:7b4c00e3912f 390
sakthipriya 0:7b4c00e3912f 391 float l_moment_y = Moment[1]; //Moment in y direction
sakthipriya 0:7b4c00e3912f 392
sakthipriya 0:7b4c00e3912f 393 phase_TR_y = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 394 if (l_moment_y <0)
sakthipriya 0:7b4c00e3912f 395 {
sakthipriya 0:7b4c00e3912f 396 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
sakthipriya 0:7b4c00e3912f 397 l_moment_y = abs(l_moment_y);
sakthipriya 0:7b4c00e3912f 398 }
sakthipriya 0:7b4c00e3912f 399
sakthipriya 0:7b4c00e3912f 400
sakthipriya 0:7b4c00e3912f 401 l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 402 pc_acs.printf("current in try is %f \r \n",l_current_y);
sakthipriya 0:7b4c00e3912f 403 if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 404 {
sakthipriya 0:7b4c00e3912f 405 l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 406 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 407 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 408 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 409 }
sakthipriya 0:7b4c00e3912f 410 else if( l_current_y >= 0.006 && l_current_y < 0.0116)
sakthipriya 0:7b4c00e3912f 411 {
sakthipriya 0:7b4c00e3912f 412 l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 413 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 414 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 415 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 416 }
sakthipriya 0:7b4c00e3912f 417 else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
sakthipriya 0:7b4c00e3912f 418 {
sakthipriya 0:7b4c00e3912f 419 l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 420 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 421 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 422 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 423 }
sakthipriya 0:7b4c00e3912f 424 else if(l_current_y >= 0.0624 && l_current_y < 0.555)
sakthipriya 0:7b4c00e3912f 425 {
sakthipriya 0:7b4c00e3912f 426 l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 427 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 428 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 429 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 430 }
sakthipriya 0:7b4c00e3912f 431 else if(l_current_y==0)
sakthipriya 0:7b4c00e3912f 432 {
sakthipriya 0:7b4c00e3912f 433 printf("\n \r l_current_y====0");
sakthipriya 0:7b4c00e3912f 434 l_duty_cycle_y = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 435 pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
sakthipriya 0:7b4c00e3912f 436 PWM2.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 437 PWM2 = l_duty_cycle_y/100 ;
sakthipriya 0:7b4c00e3912f 438 }
sakthipriya 0:7b4c00e3912f 439 else // not necessary
sakthipriya 0:7b4c00e3912f 440 {
sakthipriya 0:7b4c00e3912f 441 g_err_flag_TR_y = 1;
sakthipriya 0:7b4c00e3912f 442 }
sakthipriya 0:7b4c00e3912f 443
sakthipriya 0:7b4c00e3912f 444 //----------------------------------------------- z-direction TR -------------------------//
sakthipriya 0:7b4c00e3912f 445
sakthipriya 0:7b4c00e3912f 446
sakthipriya 0:7b4c00e3912f 447 float l_moment_z = Moment[2]; //Moment in z direction
sakthipriya 0:7b4c00e3912f 448
sakthipriya 0:7b4c00e3912f 449 phase_TR_z = 1; // setting the default current direction
sakthipriya 0:7b4c00e3912f 450 if (l_moment_z <0)
sakthipriya 0:7b4c00e3912f 451 {
sakthipriya 0:7b4c00e3912f 452 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
sakthipriya 0:7b4c00e3912f 453 l_moment_z = abs(l_moment_z);
sakthipriya 0:7b4c00e3912f 454 }
sakthipriya 0:7b4c00e3912f 455
sakthipriya 0:7b4c00e3912f 456
sakthipriya 0:7b4c00e3912f 457 l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship
sakthipriya 0:7b4c00e3912f 458 pc_acs.printf("current in trz is %f \r \n",l_current_z);
sakthipriya 0:7b4c00e3912f 459 if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:7b4c00e3912f 460 {
sakthipriya 0:7b4c00e3912f 461 l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 462 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 463 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 464 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 465 }
sakthipriya 0:7b4c00e3912f 466 else if( l_current_z >= 0.006 && l_current_z < 0.0116)
sakthipriya 0:7b4c00e3912f 467 {
sakthipriya 0:7b4c00e3912f 468 l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 469 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 470 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 471 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 472 }
sakthipriya 0:7b4c00e3912f 473 else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
sakthipriya 0:7b4c00e3912f 474 {
sakthipriya 0:7b4c00e3912f 475 l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 476 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 477 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 478 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 479 }
sakthipriya 0:7b4c00e3912f 480 else if(l_current_z >= 0.0624 && l_current_z < 0.555)
sakthipriya 0:7b4c00e3912f 481 {
sakthipriya 0:7b4c00e3912f 482 l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation
sakthipriya 0:7b4c00e3912f 483 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 484 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 485 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 486 }
sakthipriya 0:7b4c00e3912f 487 else if(l_current_z==0)
sakthipriya 0:7b4c00e3912f 488 {
sakthipriya 0:7b4c00e3912f 489 printf("\n \r l_current_z====0");
sakthipriya 0:7b4c00e3912f 490 l_duty_cycle_z = 0; // default value of duty cycle
sakthipriya 0:7b4c00e3912f 491 pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
sakthipriya 0:7b4c00e3912f 492 PWM3.period(TIME_PERIOD);
sakthipriya 0:7b4c00e3912f 493 PWM3 = l_duty_cycle_z/100 ;
sakthipriya 0:7b4c00e3912f 494 }
sakthipriya 0:7b4c00e3912f 495 else // not necessary
sakthipriya 0:7b4c00e3912f 496 {
sakthipriya 0:7b4c00e3912f 497 g_err_flag_TR_z = 1;
sakthipriya 0:7b4c00e3912f 498 }
sakthipriya 0:7b4c00e3912f 499
sakthipriya 0:7b4c00e3912f 500 //-----------------------------------------exiting the function-----------------------------------//
sakthipriya 0:7b4c00e3912f 501
sakthipriya 0:7b4c00e3912f 502 printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
sakthipriya 0:7b4c00e3912f 503
sakthipriya 0:7b4c00e3912f 504 }
sakthipriya 0:7b4c00e3912f 505
sakthipriya 0:7b4c00e3912f 506
sakthipriya 0:7b4c00e3912f 507