sakthi priya amirtharaj / Mbed 2 deprecated i2c_configuring

cdms i2c working - but not working after hk data is sent

Dependencies:   mbed-rtos mbed

Fork of pcb_test_vr1_1_2 by GOPA KUMAR K C

ACS.cpp@1:bbddd1763652, 2015-04-07 (annotated)

Committer:
sakthipriya
Date:
Tue Apr 07 18:10:19 2015 +0000
Revision:
1:bbddd1763652
Parent:
0:e91ee0e99213
Child:
3:0931a8800543
new pins assigned to all except interrupt. everything works except beacon spi.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakthipriya 0:e91ee0e99213 1 #include "ACS.h"
sakthipriya 0:e91ee0e99213 2 #include "MPU3300.h"
sakthipriya 0:e91ee0e99213 3 #include "pin_config.h"
sakthipriya 0:e91ee0e99213 4 #include "math.h"
sakthipriya 0:e91ee0e99213 5
sakthipriya 0:e91ee0e99213 6 Serial pc1(USBTX, USBRX);
sakthipriya 0:e91ee0e99213 7 SPI spi_acs (PIN16, PIN17, PIN15); // mosi, miso, sclk PTE18,19,17
sakthipriya 0:e91ee0e99213 8 DigitalOut SSN_MAG (PIN61); // ssn for magnetometer PTB11
sakthipriya 0:e91ee0e99213 9 DigitalInOut DRDY (PIN47); // drdy for magnetometer PTA17
sakthipriya 1:bbddd1763652 10 DigitalOut ssn_gyr (PTE2); //Slave Select pin of gyroscope PTB16
sakthipriya 1:bbddd1763652 11 InterruptIn dr(PTC6); //Interrupt pin for gyro PTC5
sakthipriya 0:e91ee0e99213 12 PwmOut PWM1(PIN93); //Functions used to generate PWM signal
sakthipriya 0:e91ee0e99213 13 PwmOut PWM2(PIN94);
sakthipriya 1:bbddd1763652 14 PwmOut PWM3(PIN95); //PWM output comes from pins p6
sakthipriya 0:e91ee0e99213 15 Ticker tr; //Ticker function to give values for limited amount of time for gyro
sakthipriya 0:e91ee0e99213 16 Timeout tr_mag;
sakthipriya 0:e91ee0e99213 17 uint8_t trflag_mag;
sakthipriya 0:e91ee0e99213 18 uint8_t trFlag; //ticker Flag for gyro
sakthipriya 0:e91ee0e99213 19 uint8_t drFlag; //data-ready interrupt flag for gyro
sakthipriya 0:e91ee0e99213 20 float pwm1;
sakthipriya 0:e91ee0e99213 21 float pwm2;
sakthipriya 0:e91ee0e99213 22 float pwm3;
sakthipriya 0:e91ee0e99213 23 //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------//
sakthipriya 0:e91ee0e99213 24
sakthipriya 0:e91ee0e99213 25 void FUNC_ACS_GENPWM(float M[3])
sakthipriya 0:e91ee0e99213 26 {
sakthipriya 0:e91ee0e99213 27
sakthipriya 0:e91ee0e99213 28 printf("\n\rEnterd PWMGEN function\n");
sakthipriya 0:e91ee0e99213 29 for(int i = 0 ; i<3;i++)
sakthipriya 0:e91ee0e99213 30 {
sakthipriya 0:e91ee0e99213 31 printf(" %f \t ",M[i]);
sakthipriya 0:e91ee0e99213 32 }
sakthipriya 0:e91ee0e99213 33 float timep = 0.02 ; //Time period is set to 0.02s
sakthipriya 0:e91ee0e99213 34
sakthipriya 0:e91ee0e99213 35
sakthipriya 0:e91ee0e99213 36 float DCx = 0; //Duty cycle of Moment in x direction
sakthipriya 0:e91ee0e99213 37 float ix = 0; //Current sent in x TR's
sakthipriya 0:e91ee0e99213 38 float Mx=M[0]; //Moment in x direction
sakthipriya 0:e91ee0e99213 39 ix = Mx * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e91ee0e99213 40 if( ix>0&& ix < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e91ee0e99213 41 {
sakthipriya 0:e91ee0e99213 42 DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008;
sakthipriya 0:e91ee0e99213 43 PWM1.period(timep);
sakthipriya 0:e91ee0e99213 44 PWM1 = DCx/100 ;
sakthipriya 0:e91ee0e99213 45 }
sakthipriya 0:e91ee0e99213 46 else if( ix >= 0.006&& ix < 0.0116)
sakthipriya 0:e91ee0e99213 47 {
sakthipriya 0:e91ee0e99213 48 DCx = 1*100000000*pow(ix,4) - 5*1000000*pow(ix,3) + 62603*pow(ix,2) - 199.29*ix + 0.7648;
sakthipriya 0:e91ee0e99213 49 PWM1.period(timep);
sakthipriya 0:e91ee0e99213 50 PWM1 = DCx/100 ;
sakthipriya 0:e91ee0e99213 51 }
sakthipriya 0:e91ee0e99213 52 else if (ix >= 0.0116&& ix < 0.0624)
sakthipriya 0:e91ee0e99213 53 {
sakthipriya 0:e91ee0e99213 54 DCx = 212444*pow(ix,4) - 33244*pow(ix,3) + 1778.4*pow(ix,2) + 120.91*ix + 0.3878;
sakthipriya 0:e91ee0e99213 55 PWM1.period(timep);
sakthipriya 0:e91ee0e99213 56 PWM1 = DCx/100 ;
sakthipriya 0:e91ee0e99213 57 }
sakthipriya 0:e91ee0e99213 58 else if(ix >= 0.0624&& ix < 0.555)
sakthipriya 0:e91ee0e99213 59 {
sakthipriya 0:e91ee0e99213 60 printf("\n\rACS entered if\n\r"); //gotta check what this means
sakthipriya 0:e91ee0e99213 61 DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338;
sakthipriya 0:e91ee0e99213 62 PWM1.period(timep);
sakthipriya 0:e91ee0e99213 63 PWM1 = DCx/100 ;
sakthipriya 0:e91ee0e99213 64 }
sakthipriya 0:e91ee0e99213 65 else if(ix==0)
sakthipriya 0:e91ee0e99213 66 {
sakthipriya 0:e91ee0e99213 67 printf("\n \r ix====0");
sakthipriya 0:e91ee0e99213 68 DCx = 75;
sakthipriya 0:e91ee0e99213 69 PWM1.period(timep);
sakthipriya 0:e91ee0e99213 70 PWM1 = DCx/100 ;
sakthipriya 0:e91ee0e99213 71 }
sakthipriya 0:e91ee0e99213 72 else //not necessary
sakthipriya 0:e91ee0e99213 73 {
sakthipriya 0:e91ee0e99213 74 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e91ee0e99213 75 }
sakthipriya 0:e91ee0e99213 76 pwm1 = PWM1;
sakthipriya 0:e91ee0e99213 77 printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1);
sakthipriya 0:e91ee0e99213 78
sakthipriya 0:e91ee0e99213 79
sakthipriya 0:e91ee0e99213 80 float DCy = 0; //Duty cycle of Moment in y direction
sakthipriya 0:e91ee0e99213 81 float iy = 0; //Current sent in y TR's
sakthipriya 0:e91ee0e99213 82 float My=M[1]; //Moment in y direction
sakthipriya 0:e91ee0e99213 83 iy = My * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e91ee0e99213 84 if( iy>0&& iy < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e91ee0e99213 85 {
sakthipriya 0:e91ee0e99213 86 DCy = 6*1000000*pow(iy,4) - 377291*pow(iy,3) + 4689.6*pow(iy,2) + 149.19*iy - 0.0008;
sakthipriya 0:e91ee0e99213 87 PWM2.period(timep);
sakthipriya 0:e91ee0e99213 88 PWM2 = DCy/100 ;
sakthipriya 0:e91ee0e99213 89 }
sakthipriya 0:e91ee0e99213 90 else if( iy >= 0.006&& iy < 0.0116)
sakthipriya 0:e91ee0e99213 91 {
sakthipriya 0:e91ee0e99213 92 DCy = 1*100000000*pow(iy,4) - 5*1000000*pow(iy,3) + 62603*pow(iy,2) - 199.29*iy + 0.7648;
sakthipriya 0:e91ee0e99213 93 PWM2.period(timep);
sakthipriya 0:e91ee0e99213 94 PWM2 = DCy/100 ;
sakthipriya 0:e91ee0e99213 95 }
sakthipriya 0:e91ee0e99213 96 else if (iy >= 0.0116&& iy < 0.0624)
sakthipriya 0:e91ee0e99213 97 {
sakthipriya 0:e91ee0e99213 98 DCy = 212444*pow(iy,4) - 33244*pow(iy,3) + 1778.4*pow(iy,2) + 120.91*iy + 0.3878;
sakthipriya 0:e91ee0e99213 99 PWM2.period(timep);
sakthipriya 0:e91ee0e99213 100 PWM2 = DCy/100 ;
sakthipriya 0:e91ee0e99213 101 }
sakthipriya 0:e91ee0e99213 102 else if(iy >= 0.0624&& iy < 0.555)
sakthipriya 0:e91ee0e99213 103 {
sakthipriya 0:e91ee0e99213 104 printf("\n\rACS entered if\n\r");
sakthipriya 0:e91ee0e99213 105 DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338;
sakthipriya 0:e91ee0e99213 106 PWM2.period(timep);
sakthipriya 0:e91ee0e99213 107 PWM2 = DCy/100 ;
sakthipriya 0:e91ee0e99213 108 }
sakthipriya 0:e91ee0e99213 109 else if(iy==0)
sakthipriya 0:e91ee0e99213 110 {
sakthipriya 0:e91ee0e99213 111 DCy = 0;
sakthipriya 0:e91ee0e99213 112 PWM2.period(timep);
sakthipriya 0:e91ee0e99213 113 PWM2 = DCy/100 ;
sakthipriya 0:e91ee0e99213 114 }
sakthipriya 0:e91ee0e99213 115 else
sakthipriya 0:e91ee0e99213 116 {
sakthipriya 0:e91ee0e99213 117 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e91ee0e99213 118 }
sakthipriya 0:e91ee0e99213 119 pwm2 = PWM2;
sakthipriya 0:e91ee0e99213 120 printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2);
sakthipriya 0:e91ee0e99213 121
sakthipriya 0:e91ee0e99213 122
sakthipriya 0:e91ee0e99213 123 float DCz = 0; //Duty cycle of Moment in z direction
sakthipriya 0:e91ee0e99213 124 float iz = 0; //Current sent in z TR's
sakthipriya 0:e91ee0e99213 125 float Mz=M[2]; //Moment in z direction
sakthipriya 0:e91ee0e99213 126 iz = Mz * 0.3 ; //Moment and Current always have the linear relationship
sakthipriya 0:e91ee0e99213 127 if( iz>0&& iz < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
sakthipriya 0:e91ee0e99213 128 {
sakthipriya 0:e91ee0e99213 129 DCz = 6*1000000*pow(iz,4) - 377291*pow(iz,3) + 4689.6*pow(iz,2) + 149.19*iz - 0.0008;
sakthipriya 0:e91ee0e99213 130 PWM3.period(timep);
sakthipriya 0:e91ee0e99213 131 PWM3 = DCz/100 ;
sakthipriya 0:e91ee0e99213 132 }
sakthipriya 0:e91ee0e99213 133 else if( iz >= 0.006&& iz < 0.0116)
sakthipriya 0:e91ee0e99213 134 {
sakthipriya 0:e91ee0e99213 135 DCz = 1*100000000*pow(iz,4) - 5*1000000*pow(iz,3) + 62603*pow(iz,2) - 199.29*iz + 0.7648;
sakthipriya 0:e91ee0e99213 136 PWM3.period(timep);
sakthipriya 0:e91ee0e99213 137 PWM3 = DCz/100 ;
sakthipriya 0:e91ee0e99213 138 }
sakthipriya 0:e91ee0e99213 139 else if (iz >= 0.0116&& iz < 0.0624)
sakthipriya 0:e91ee0e99213 140 {
sakthipriya 0:e91ee0e99213 141 DCz = 212444*pow(iz,4) - 33244*pow(iz,3) + 1778.4*pow(iz,2) + 120.91*iz + 0.3878;
sakthipriya 0:e91ee0e99213 142 PWM3.period(timep);
sakthipriya 0:e91ee0e99213 143 PWM3 = DCz/100 ;
sakthipriya 0:e91ee0e99213 144 }
sakthipriya 0:e91ee0e99213 145 else if(iz >= 0.0624&& iz < 0.555)
sakthipriya 0:e91ee0e99213 146 {
sakthipriya 0:e91ee0e99213 147 printf("\n\rACS entered if\n\r");
sakthipriya 0:e91ee0e99213 148 DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338;
sakthipriya 0:e91ee0e99213 149 PWM3.period(timep);
sakthipriya 0:e91ee0e99213 150 PWM3 = DCz/100 ;
sakthipriya 0:e91ee0e99213 151 }
sakthipriya 0:e91ee0e99213 152 else if(iz==0)
sakthipriya 0:e91ee0e99213 153 {
sakthipriya 0:e91ee0e99213 154 DCz = 0;
sakthipriya 0:e91ee0e99213 155 PWM3.period(timep);
sakthipriya 0:e91ee0e99213 156 PWM3 = DCz/100 ;
sakthipriya 0:e91ee0e99213 157 }
sakthipriya 0:e91ee0e99213 158 else
sakthipriya 0:e91ee0e99213 159 {
sakthipriya 0:e91ee0e99213 160 // printf("!!!!!!!!!!Error!!!!!!!!!");
sakthipriya 0:e91ee0e99213 161 }
sakthipriya 0:e91ee0e99213 162 pwm3 = PWM3;
sakthipriya 0:e91ee0e99213 163 printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3);
sakthipriya 0:e91ee0e99213 164
sakthipriya 0:e91ee0e99213 165 printf("\n\rExited PWMGEN function\n\r");
sakthipriya 0:e91ee0e99213 166 }
sakthipriya 0:e91ee0e99213 167 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e91ee0e99213 168 -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/
sakthipriya 0:e91ee0e99213 169
sakthipriya 0:e91ee0e99213 170 void trsub_mag()
sakthipriya 0:e91ee0e99213 171 {
sakthipriya 0:e91ee0e99213 172 trflag_mag=0;
sakthipriya 0:e91ee0e99213 173 }
sakthipriya 0:e91ee0e99213 174
sakthipriya 0:e91ee0e99213 175 void FUNC_ACS_MAG_INIT()
sakthipriya 0:e91ee0e99213 176 {
sakthipriya 0:e91ee0e99213 177 DRDY = 0;
sakthipriya 0:e91ee0e99213 178 int a ;
sakthipriya 0:e91ee0e99213 179 a=DRDY;
sakthipriya 0:e91ee0e99213 180 printf("\n\r DRDY is %d\n\r",a);
sakthipriya 0:e91ee0e99213 181 SSN_MAG=1; //pin is disabled
sakthipriya 0:e91ee0e99213 182 spi_acs.format(8,0); //8bits,Mode 0
sakthipriya 0:e91ee0e99213 183 spi_acs.frequency(100000); //clock frequency
sakthipriya 0:e91ee0e99213 184 SSN_MAG=0; // Selecting pin
sakthipriya 0:e91ee0e99213 185 wait_ms(10); //accounts for delay.can be minimised.
sakthipriya 0:e91ee0e99213 186 spi_acs.write(0x83);
sakthipriya 0:e91ee0e99213 187 wait_ms(10);
sakthipriya 0:e91ee0e99213 188 unsigned char i;
sakthipriya 0:e91ee0e99213 189 for(i=0;i<3;i++) //initialising values.
sakthipriya 0:e91ee0e99213 190 {
sakthipriya 0:e91ee0e99213 191 spi_acs.write(0x00); //MSB of X,y,Z
sakthipriya 0:e91ee0e99213 192 spi_acs.write(0xc8); //LSB of X,Y,z;pointer increases automatically.
sakthipriya 0:e91ee0e99213 193 }
sakthipriya 0:e91ee0e99213 194 SSN_MAG=1;
sakthipriya 0:e91ee0e99213 195 }
sakthipriya 0:e91ee0e99213 196
sakthipriya 0:e91ee0e99213 197 void FUNC_ACS_MAG_EXEC(float mag_field[])
sakthipriya 0:e91ee0e99213 198 {
sakthipriya 0:e91ee0e99213 199 printf("\n\rEntered magnetometer function\n\r");
sakthipriya 0:e91ee0e99213 200 DRDY.write(0);
sakthipriya 0:e91ee0e99213 201 int a;
sakthipriya 0:e91ee0e99213 202 a = DRDY;
sakthipriya 0:e91ee0e99213 203 printf("\n\r DRDY is %d\n\r",a);
sakthipriya 0:e91ee0e99213 204 SSN_MAG=0; //enabling slave to measure the values
sakthipriya 0:e91ee0e99213 205 wait_ms(10);
sakthipriya 0:e91ee0e99213 206 spi_acs.write(0x82); //initiates measurement
sakthipriya 0:e91ee0e99213 207 wait_ms(10);
sakthipriya 0:e91ee0e99213 208 spi_acs.write(0x01); //selecting x,y and z axes, measurement starts now
sakthipriya 0:e91ee0e99213 209 SSN_MAG=1;
sakthipriya 0:e91ee0e99213 210 wait_ms(10);
sakthipriya 0:e91ee0e99213 211
sakthipriya 0:e91ee0e99213 212 trflag_mag=1;
sakthipriya 0:e91ee0e99213 213 tr_mag.attach(&trsub_mag,1); //runs in background,makes trflag_mag=0 after 1s
sakthipriya 0:e91ee0e99213 214 DRDY.input();
sakthipriya 0:e91ee0e99213 215 while(trflag_mag) /*initially flag is 1,so loop is executed,if DRDY is high,then data is retrieved and programme ends,else
sakthipriya 0:e91ee0e99213 216 loop runs for at the max 1s and if still DRDY is zero,the flag becomes 0 and loop is not executed and
sakthipriya 0:e91ee0e99213 217 programme is terminated*/
sakthipriya 0:e91ee0e99213 218 {
sakthipriya 0:e91ee0e99213 219 wait_ms(5);
sakthipriya 0:e91ee0e99213 220 if(DRDY==1)
sakthipriya 0:e91ee0e99213 221 {
sakthipriya 0:e91ee0e99213 222 printf("\n\r DRDY is high\n");
sakthipriya 0:e91ee0e99213 223 SSN_MAG=0;
sakthipriya 0:e91ee0e99213 224 spi_acs.write(0xc9); //command byte for retrieving data
sakthipriya 0:e91ee0e99213 225 unsigned char axis;
sakthipriya 0:e91ee0e99213 226 float Bnewvalue[3]={0.0,0.0,0.0};
sakthipriya 0:e91ee0e99213 227 int32_t Bvalue[3]={0,0,0};
sakthipriya 0:e91ee0e99213 228 int32_t a= pow(2.0,24.0);
sakthipriya 0:e91ee0e99213 229 int32_t b= pow(2.0,23.0);
sakthipriya 0:e91ee0e99213 230 for(axis=0;axis<3;axis++)
sakthipriya 0:e91ee0e99213 231 {
sakthipriya 0:e91ee0e99213 232 Bvalue[axis]=spi_acs.write(0x00)<<16; //MSB 1 is send first
sakthipriya 0:e91ee0e99213 233 wait_ms(10);
sakthipriya 0:e91ee0e99213 234 Bvalue[axis]|=spi_acs.write(0x00)<<8; //MSB 2 is send next
sakthipriya 0:e91ee0e99213 235 wait_ms(10);
sakthipriya 0:e91ee0e99213 236 Bvalue[axis]|=spi_acs.write(0x00); //LSB is send.....total length is 24 bits(3*8bits)...which are appended to get actual bit configuration
sakthipriya 0:e91ee0e99213 237 if((Bvalue[axis]&b)==b)
sakthipriya 0:e91ee0e99213 238 {
sakthipriya 0:e91ee0e99213 239 Bvalue[axis]=Bvalue[axis]-a; //converting 2s complement to signed decimal
sakthipriya 0:e91ee0e99213 240
sakthipriya 0:e91ee0e99213 241 }
sakthipriya 0:e91ee0e99213 242 Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla
sakthipriya 0:e91ee0e99213 243 wait_ms(10);
sakthipriya 0:e91ee0e99213 244
sakthipriya 0:e91ee0e99213 245 mag_field[axis] = Bnewvalue[axis];
sakthipriya 0:e91ee0e99213 246 printf("\t%lf\n\r",mag_field[axis]);
sakthipriya 0:e91ee0e99213 247 }
sakthipriya 0:e91ee0e99213 248 SSN_MAG=1;
sakthipriya 0:e91ee0e99213 249 printf("\n\r exited magnetometer function\n");
sakthipriya 0:e91ee0e99213 250 //return Bnewvalue; //return here? doubt..
sakthipriya 0:e91ee0e99213 251
sakthipriya 0:e91ee0e99213 252 break;
sakthipriya 0:e91ee0e99213 253 }
sakthipriya 0:e91ee0e99213 254 }
sakthipriya 0:e91ee0e99213 255 }
sakthipriya 0:e91ee0e99213 256 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e91ee0e99213 257 -------------------------------------------torque to moment conversion------------------------------------------------------------------------------------------*/
sakthipriya 0:e91ee0e99213 258 void moment_calc (float tauc[3], float b[3], float moment[3])
sakthipriya 0:e91ee0e99213 259 {
sakthipriya 0:e91ee0e99213 260 float b1;
sakthipriya 0:e91ee0e99213 261 b1 = pow(b[0],2) + pow(b[1],2) +pow(b[2],2) ;
sakthipriya 0:e91ee0e99213 262 printf("\nvalue of b is %f\n",b1);
sakthipriya 0:e91ee0e99213 263 moment[0] = ((tauc[1]*b[2])-(tauc[2]*b[1]))/b1;
sakthipriya 0:e91ee0e99213 264 moment[1] = ((tauc[2]*b[0])-(tauc[0]*b[2]))/b1;
sakthipriya 0:e91ee0e99213 265 moment[2] = ((tauc[0]*b[1])-(tauc[1]*b[0]))/b1;
sakthipriya 0:e91ee0e99213 266
sakthipriya 0:e91ee0e99213 267 }
sakthipriya 0:e91ee0e99213 268
sakthipriya 0:e91ee0e99213 269
sakthipriya 0:e91ee0e99213 270 /*------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e91ee0e99213 271 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
sakthipriya 0:e91ee0e99213 272
sakthipriya 0:e91ee0e99213 273 void FUNC_ACS_CNTRLALGO(float *b,float *omega,float tauc[3])
sakthipriya 0:e91ee0e99213 274 {
sakthipriya 0:e91ee0e99213 275
sakthipriya 0:e91ee0e99213 276 float db[3]; // inputs
sakthipriya 0:e91ee0e99213 277 //initialization
sakthipriya 0:e91ee0e99213 278 float bb[3] = {0, 0, 0};
sakthipriya 0:e91ee0e99213 279 float d[3] = {0, 0, 0};
sakthipriya 0:e91ee0e99213 280 float Jm[3][3] = {{0.2730, 0, 0}, {0, 0.3018, 0}, {0, 0, 0.3031}};
sakthipriya 0:e91ee0e99213 281 float den = 0;
sakthipriya 0:e91ee0e99213 282 float den2;
sakthipriya 0:e91ee0e99213 283 int i, j; //temporary variables
sakthipriya 0:e91ee0e99213 284 float Mu[2], z[2], dv[2], v[2], u[2]; //outputs
sakthipriya 0:e91ee0e99213 285 float invJm[3][3];
sakthipriya 0:e91ee0e99213 286 float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4;
sakthipriya 0:e91ee0e99213 287 printf("\n\r Entered cntrl algo\n\r");
sakthipriya 0:e91ee0e99213 288 for(int i=0; i<3; i++)
sakthipriya 0:e91ee0e99213 289 {
sakthipriya 0:e91ee0e99213 290 printf("%f\t",omega[i]);
sakthipriya 0:e91ee0e99213 291 }
sakthipriya 0:e91ee0e99213 292 for(int i=0; i<3; i++)
sakthipriya 0:e91ee0e99213 293 {
sakthipriya 0:e91ee0e99213 294 printf("%f\t",b[i]);
sakthipriya 0:e91ee0e99213 295 }
sakthipriya 0:e91ee0e99213 296 //structure parameters
sakthipriya 0:e91ee0e99213 297 void inverse (float mat[3][3], float inv[3][3]);
sakthipriya 0:e91ee0e99213 298 void getInput (float x[9]);
sakthipriya 0:e91ee0e99213 299 tauc[0] =tauc[1] =tauc[2]=0 ;
sakthipriya 0:e91ee0e99213 300
sakthipriya 0:e91ee0e99213 301 den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2]));
sakthipriya 0:e91ee0e99213 302 den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]);
sakthipriya 0:e91ee0e99213 303 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 304 {
sakthipriya 0:e91ee0e99213 305 db[i] = (db[i]*den*den-b[i]*den2) / (pow(den,3));
sakthipriya 0:e91ee0e99213 306 //db[i]/=den*den*den;
sakthipriya 0:e91ee0e99213 307 }
sakthipriya 0:e91ee0e99213 308 for(i=0;i<3;i++) //gotta check what is this
sakthipriya 0:e91ee0e99213 309 {
sakthipriya 0:e91ee0e99213 310 printf("\n\rreached here\n\r");
sakthipriya 0:e91ee0e99213 311 if(den!=0)
sakthipriya 0:e91ee0e99213 312 b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required
sakthipriya 0:e91ee0e99213 313 }
sakthipriya 0:e91ee0e99213 314
sakthipriya 0:e91ee0e99213 315 // select kz, kmu, gamma
sakthipriya 0:e91ee0e99213 316 if(b[0]>0.9 || b[0]<-0.9)
sakthipriya 0:e91ee0e99213 317 {
sakthipriya 0:e91ee0e99213 318 kz = kz2;
sakthipriya 0:e91ee0e99213 319 kmu = kmu2;
sakthipriya 0:e91ee0e99213 320 gamma = gamma2;
sakthipriya 0:e91ee0e99213 321 }
sakthipriya 0:e91ee0e99213 322 // calculate Mu, v, dv, z, u
sakthipriya 0:e91ee0e99213 323 for(i=0;i<2;i++)
sakthipriya 0:e91ee0e99213 324 {
sakthipriya 0:e91ee0e99213 325 Mu[i] = b[i+1];
sakthipriya 0:e91ee0e99213 326 v[i] = -kmu*Mu[i];
sakthipriya 0:e91ee0e99213 327 dv[i] = -kmu*db[i+1];
sakthipriya 0:e91ee0e99213 328 z[i] = db[i+1] - v[i];
sakthipriya 0:e91ee0e99213 329 u[i] = -kz*z[i] + dv[i]-(Mu[i] / gamma);
sakthipriya 0:e91ee0e99213 330 }
sakthipriya 0:e91ee0e99213 331 inverse(Jm, invJm);
sakthipriya 0:e91ee0e99213 332 // calculate cross(omega,J*omega)for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 333 for(i=0; i<3; i++) // for loop included after checking original code
sakthipriya 0:e91ee0e99213 334 {
sakthipriya 0:e91ee0e99213 335 for(j=0;j<3;j++)
sakthipriya 0:e91ee0e99213 336 bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]);
sakthipriya 0:e91ee0e99213 337 }
sakthipriya 0:e91ee0e99213 338 // calculate invJm*cross(omega,J*omega) store in d
sakthipriya 0:e91ee0e99213 339 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 340 {
sakthipriya 0:e91ee0e99213 341 for(j=0;j<3;j++)
sakthipriya 0:e91ee0e99213 342 d[i] += bb[j]*invJm[i][j];
sakthipriya 0:e91ee0e99213 343 }
sakthipriya 0:e91ee0e99213 344 // calculate d = cross(invJm*cross(omega,J*omega),b) -cross(omega,db)
sakthipriya 0:e91ee0e99213 345 // bb =[0;u-d(2:3)]
sakthipriya 0:e91ee0e99213 346 // store in bb
sakthipriya 0:e91ee0e99213 347 bb[1] = u[0] + (d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2]) + (omega[2]*db[0]);
sakthipriya 0:e91ee0e99213 348 bb[2] = u[1]-(d[0]*b[1]) + (d[1]*b[0]) + (omega[0]*db[1])-(omega[1]*db[0]);
sakthipriya 0:e91ee0e99213 349 bb[0] = 0;
sakthipriya 0:e91ee0e99213 350 // calculate N
sakthipriya 0:e91ee0e99213 351 // reusing invJm as N
sakthipriya 0:e91ee0e99213 352 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 353 {
sakthipriya 0:e91ee0e99213 354 d[i] = invJm[1][i];
sakthipriya 0:e91ee0e99213 355 invJm[ 1][i] = b[2]*invJm[0][i] - b[0]*invJm[2][i];
sakthipriya 0:e91ee0e99213 356 invJm[2][i] = -b[1]*invJm[0][i] + b[0]*d[i];
sakthipriya 0:e91ee0e99213 357 invJm[0][i] = b[i];
sakthipriya 0:e91ee0e99213 358 }
sakthipriya 0:e91ee0e99213 359 // calculate inv(N) store in Jm
sakthipriya 0:e91ee0e99213 360 inverse(invJm, Jm);
sakthipriya 0:e91ee0e99213 361 // calculate tauc
sakthipriya 0:e91ee0e99213 362 printf("\n \r calculatin tauc");
sakthipriya 0:e91ee0e99213 363 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 364 {
sakthipriya 0:e91ee0e99213 365 for(j=0;j<3;j++)
sakthipriya 0:e91ee0e99213 366 tauc[i] += Jm[i][j]*bb[j];
sakthipriya 0:e91ee0e99213 367 printf(" %f \t",tauc[i]);
sakthipriya 0:e91ee0e99213 368 }
sakthipriya 0:e91ee0e99213 369 }
sakthipriya 0:e91ee0e99213 370
sakthipriya 0:e91ee0e99213 371
sakthipriya 0:e91ee0e99213 372 void inverse(float mat[3][3], float inv[3][3])
sakthipriya 0:e91ee0e99213 373 {
sakthipriya 0:e91ee0e99213 374 int i, j;
sakthipriya 0:e91ee0e99213 375 float det = 0;
sakthipriya 0:e91ee0e99213 376 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 377 { for(j=0;j<3;j++)
sakthipriya 0:e91ee0e99213 378 inv[j][i] = (mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3]) - (mat[(i+2)%3]
sakthipriya 0:e91ee0e99213 379 [(j+1)%3]*mat[(i+1)%3][(j+2)%3]);
sakthipriya 0:e91ee0e99213 380 }
sakthipriya 0:e91ee0e99213 381 det += (mat[0][0]*inv[0][0]) + (mat[0][1]*inv[1][0]) + (mat[0][2]*inv[2][0]);
sakthipriya 0:e91ee0e99213 382 for(i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 383 { for(j=0;j<3;j++)
sakthipriya 0:e91ee0e99213 384 inv[i][j] /= det;
sakthipriya 0:e91ee0e99213 385 }
sakthipriya 0:e91ee0e99213 386 }
sakthipriya 0:e91ee0e99213 387
sakthipriya 0:e91ee0e99213 388
sakthipriya 0:e91ee0e99213 389 /*-------------------------------------------------------------------------------------------------------------------------------------------------------
sakthipriya 0:e91ee0e99213 390 -------------------------------------------GYROSCOPE-------------------------------------------------------------------------------------------------*/
sakthipriya 0:e91ee0e99213 391
sakthipriya 0:e91ee0e99213 392 void trSub();
sakthipriya 0:e91ee0e99213 393 void drSub();
sakthipriya 0:e91ee0e99213 394 void init_gyro();
sakthipriya 0:e91ee0e99213 395 float * FUNC_ACS_EXEC_GYR();
sakthipriya 0:e91ee0e99213 396
sakthipriya 0:e91ee0e99213 397 void drSub() //In this function we setting data-ready flag to 1
sakthipriya 0:e91ee0e99213 398 {
sakthipriya 0:e91ee0e99213 399 drFlag=1;
sakthipriya 0:e91ee0e99213 400 //printf("\n dr interrupt detected");
sakthipriya 0:e91ee0e99213 401 }
sakthipriya 0:e91ee0e99213 402 void trSub() //In this function we are setting ticker flag to 0
sakthipriya 0:e91ee0e99213 403 {
sakthipriya 0:e91ee0e99213 404 trFlag=0;
sakthipriya 0:e91ee0e99213 405 }
sakthipriya 0:e91ee0e99213 406 void FUNC_ACS_INIT_GYR()
sakthipriya 0:e91ee0e99213 407 {
sakthipriya 0:e91ee0e99213 408 uint8_t response;
sakthipriya 0:e91ee0e99213 409 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:e91ee0e99213 410 spi_acs.format(8,0); // Spi format is 8 bits, and clock mode 3
sakthipriya 0:e91ee0e99213 411 spi_acs.frequency(1000000); //frequency to be set as 1MHz
sakthipriya 0:e91ee0e99213 412 drFlag=0; //Intially defining data-ready flag to be 0
sakthipriya 0:e91ee0e99213 413 dr.mode(PullDown);
sakthipriya 0:e91ee0e99213 414 dr.rise(&drSub);
sakthipriya 0:e91ee0e99213 415 __disable_irq();
sakthipriya 0:e91ee0e99213 416
sakthipriya 0:e91ee0e99213 417 /*initializing the register and changing its configuration*/
sakthipriya 0:e91ee0e99213 418 ssn_gyr=0; //Selecting chip(Mpu-3300)
sakthipriya 0:e91ee0e99213 419 spi_acs.write(USER_CTRL|READFLAG); //sending USER_CTRL address with read bit
sakthipriya 0:e91ee0e99213 420 response=spi_acs.write(DUMMYBIT); //sending dummy bit to get default values of the register
sakthipriya 0:e91ee0e99213 421
sakthipriya 0:e91ee0e99213 422 ssn_gyr=1; //Deselecting the chip
sakthipriya 0:e91ee0e99213 423 wait(0.1); //waiting according the product specifications
sakthipriya 0:e91ee0e99213 424
sakthipriya 0:e91ee0e99213 425 ssn_gyr=0; //again selecting the chip
sakthipriya 0:e91ee0e99213 426 spi_acs.write(USER_CTRL); //sending USER_CTRL address without read bit
sakthipriya 0:e91ee0e99213 427 spi_acs.write(response|BIT_I2C_IF_DIS); //disabling the I2C mode in the register
sakthipriya 0:e91ee0e99213 428 ssn_gyr=1; //deselecting the chip
sakthipriya 0:e91ee0e99213 429 wait(0.1); // waiting for 100ms before going for another register
sakthipriya 0:e91ee0e99213 430
sakthipriya 0:e91ee0e99213 431 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 432 spi_acs.write(PWR_MGMT_1|READFLAG); //Power Management register-1
sakthipriya 0:e91ee0e99213 433 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e91ee0e99213 434 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 435 wait(0.1);
sakthipriya 0:e91ee0e99213 436
sakthipriya 0:e91ee0e99213 437 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 438 spi_acs.write(PWR_MGMT_1);
sakthipriya 0:e91ee0e99213 439 response=spi_acs.write(response|BIT_CLKSEL_X); //Selecting the X axis gyroscope as clock as mentioned above
sakthipriya 0:e91ee0e99213 440 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 441 wait(0.1);
sakthipriya 0:e91ee0e99213 442
sakthipriya 0:e91ee0e99213 443 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 444 spi_acs.write(GYRO_CONFIG|READFLAG); //sending GYRO_CONFIG address with read bit
sakthipriya 0:e91ee0e99213 445 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e91ee0e99213 446 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 447 wait(0.1);
sakthipriya 0:e91ee0e99213 448
sakthipriya 0:e91ee0e99213 449 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 450 spi_acs.write(GYRO_CONFIG); //sending GYRO_CONFIG address to write to register
sakthipriya 0:e91ee0e99213 451 spi_acs.write(response&(~(BITS_FS_SEL_3|BITS_FS_SEL_4))); //selecting a full scale mode of +/=225 deg/sec
sakthipriya 0:e91ee0e99213 452 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 453 wait(0.1);
sakthipriya 0:e91ee0e99213 454
sakthipriya 0:e91ee0e99213 455 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 456 spi_acs.write(CONFIG|READFLAG); //sending CONFIG address with read bit
sakthipriya 0:e91ee0e99213 457 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e91ee0e99213 458 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 459 wait(0.1);
sakthipriya 0:e91ee0e99213 460
sakthipriya 0:e91ee0e99213 461 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 462 spi_acs.write(CONFIG); //sending CONFIG address to write to register
sakthipriya 0:e91ee0e99213 463 spi_acs.write(response|BITS_DLPF_CFG); //selecting a bandwidth of 42 hz and delay of 4.8ms
sakthipriya 0:e91ee0e99213 464 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 465 wait(0.1);
sakthipriya 0:e91ee0e99213 466
sakthipriya 0:e91ee0e99213 467 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 468 spi_acs.write(SMPLRT_DIV|READFLAG); //sending SMPLRT_DIV address with read bit
sakthipriya 0:e91ee0e99213 469 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e91ee0e99213 470 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 471 wait(0.1);
sakthipriya 0:e91ee0e99213 472
sakthipriya 0:e91ee0e99213 473 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 474 spi_acs.write(SMPLRT_DIV); //sending SMPLRT_DIV address to write to register
sakthipriya 0:e91ee0e99213 475 spi_acs.write(response&BITS_SMPLRT_DIV); //setting the sampling rate division to be 0 to make sample rate = gyroscopic output rate
sakthipriya 0:e91ee0e99213 476 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 477 wait(0.1);
sakthipriya 0:e91ee0e99213 478
sakthipriya 0:e91ee0e99213 479 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 480 spi_acs.write(INT_ENABLE|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:e91ee0e99213 481 response=spi_acs.write(DUMMYBIT); //sending dummy byte to get the default values of the
sakthipriya 0:e91ee0e99213 482 // regiser
sakthipriya 0:e91ee0e99213 483 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 484 wait(0.1);
sakthipriya 0:e91ee0e99213 485
sakthipriya 0:e91ee0e99213 486 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 487 spi_acs.write(INT_ENABLE); //sending INT_ENABLE address to write to register
sakthipriya 0:e91ee0e99213 488 spi_acs.write(response|BIT_DATA_RDY_ENABLE); //Enbling data ready interrupt
sakthipriya 0:e91ee0e99213 489 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 490 wait(0.1);
sakthipriya 0:e91ee0e99213 491
sakthipriya 0:e91ee0e99213 492 __enable_irq();
sakthipriya 0:e91ee0e99213 493 }
sakthipriya 0:e91ee0e99213 494
sakthipriya 0:e91ee0e99213 495 void FUNC_ACS_EXEC_GYR(float*omega)
sakthipriya 0:e91ee0e99213 496 {
sakthipriya 0:e91ee0e99213 497 printf("\n\rEntered gyro\n\r");
sakthipriya 0:e91ee0e99213 498 uint8_t response;
sakthipriya 0:e91ee0e99213 499 uint8_t MSB,LSB;
sakthipriya 0:e91ee0e99213 500 int16_t bit_data;
sakthipriya 0:e91ee0e99213 501 float data[3],error[3]={0,0,0}; //declaring error array to add to the values when required
sakthipriya 0:e91ee0e99213 502 float senstivity = 145.6; //senstivity is 145.6 for full scale mode of +/-225 deg/sec
sakthipriya 0:e91ee0e99213 503 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 504 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of INT_ENABLE with readflag
sakthipriya 0:e91ee0e99213 505 response=spi_acs.write(DUMMYBIT); //
sakthipriya 0:e91ee0e99213 506 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 507 wait(0.1);
sakthipriya 0:e91ee0e99213 508
sakthipriya 0:e91ee0e99213 509 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 510 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:e91ee0e99213 511 response=spi_acs.write(response&(~(BIT_SLEEP))); //waking up the gyroscope from sleep
sakthipriya 0:e91ee0e99213 512 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 513 wait(0.1);
sakthipriya 0:e91ee0e99213 514
sakthipriya 0:e91ee0e99213 515 trFlag=1;
sakthipriya 0:e91ee0e99213 516 tr.attach(&trSub,1); //executes the function trSub afer 1sec
sakthipriya 0:e91ee0e99213 517
sakthipriya 0:e91ee0e99213 518 while(trFlag)
sakthipriya 0:e91ee0e99213 519 {
sakthipriya 0:e91ee0e99213 520 // printf("\n\r check1");
sakthipriya 0:e91ee0e99213 521 wait_ms(5); //This is required for this while loop to exit. I don't know why.
sakthipriya 0:e91ee0e99213 522 if(drFlag==1)
sakthipriya 0:e91ee0e99213 523 {
sakthipriya 0:e91ee0e99213 524 //printf("\n\r check2");
sakthipriya 0:e91ee0e99213 525 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 526 spi_acs.write(GYRO_XOUT_H|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:e91ee0e99213 527 for(int i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 528 {
sakthipriya 0:e91ee0e99213 529 MSB = spi_acs.write(DUMMYBIT); //reading the MSB values of x,y and z respectively
sakthipriya 0:e91ee0e99213 530 LSB = spi_acs.write(DUMMYBIT); //reading the LSB values of x,y and z respectively
sakthipriya 0:e91ee0e99213 531 bit_data= ((int16_t)MSB<<8)|LSB; //concatenating to get 16 bit 2's complement of the required gyroscope values
sakthipriya 0:e91ee0e99213 532 data[i]=(float)bit_data;
sakthipriya 0:e91ee0e99213 533 data[i]=data[i]/senstivity; //dividing with senstivity to get the readings in deg/sec
sakthipriya 0:e91ee0e99213 534 data[i]+=error[i]; //adding with error to remove offset errors
sakthipriya 0:e91ee0e99213 535 }
sakthipriya 0:e91ee0e99213 536 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 537 for (int i=0;i<3;i++)
sakthipriya 0:e91ee0e99213 538 {
sakthipriya 0:e91ee0e99213 539 printf("%f\t",data[i]); //printing the angular velocity values
sakthipriya 0:e91ee0e99213 540 omega[i] = data[i];
sakthipriya 0:e91ee0e99213 541 }
sakthipriya 0:e91ee0e99213 542 printf("\n\r");
sakthipriya 0:e91ee0e99213 543 break;
sakthipriya 0:e91ee0e99213 544 }
sakthipriya 0:e91ee0e99213 545 drFlag=0;
sakthipriya 0:e91ee0e99213 546 }
sakthipriya 0:e91ee0e99213 547 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 548 spi_acs.write(PWR_MGMT_1|READFLAG); //sending address of PWR_MGMT_1 with readflag
sakthipriya 0:e91ee0e99213 549 response=spi_acs.write(DUMMYBIT);
sakthipriya 0:e91ee0e99213 550 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 551 wait(0.1);
sakthipriya 0:e91ee0e99213 552
sakthipriya 0:e91ee0e99213 553 ssn_gyr=0;
sakthipriya 0:e91ee0e99213 554 spi_acs.write(PWR_MGMT_1); //sending PWR_MGMT_1 address to write to register
sakthipriya 0:e91ee0e99213 555 response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode
sakthipriya 0:e91ee0e99213 556 ssn_gyr=1;
sakthipriya 0:e91ee0e99213 557 wait(0.1);
sakthipriya 0:e91ee0e99213 558 printf("\n\rExited gyro\n\r");
sakthipriya 0:e91ee0e99213 559
sakthipriya 0:e91ee0e99213 560 }
sakthipriya 0:e91ee0e99213 561
sakthipriya 0:e91ee0e99213 562
sakthipriya 0:e91ee0e99213 563
sakthipriya 0:e91ee0e99213 564
sakthipriya 0:e91ee0e99213 565
sakthipriya 0:e91ee0e99213 566