![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
beacon on/off implemented and working
Fork of i2c_configuring by
ACS.cpp@6:4fd79755f4c9, 2015-05-17 (annotated)
- Committer:
- sakthipriya
- Date:
- Sun May 17 19:40:48 2015 +0000
- Revision:
- 6:4fd79755f4c9
- Parent:
- 3:0931a8800543
beacon on/off implemented and working
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:0931a8800543 | 10 | DigitalOut ssn_gyr (PIN62); //Slave Select pin of gyroscope PTB16 |
sakthipriya | 3:0931a8800543 | 11 | InterruptIn dr(PIN81); //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 |