TurtleBot / Mbed 2 deprecated TurtleBot_V2

Changing about IMU

Dependencies:   Servo mbed-rtos mbed

Fork of TurtleBot_V1 by TurtleBot

main.cpp@2:d4bd9ff10e8e, 2018-03-23 (annotated)

Committer:
worasuchad
Date:
Fri Mar 23 16:25:17 2018 +0000
Revision:
2:d4bd9ff10e8e
Parent:
1:5609c1795245
Child:
3:5bfa7291c639
change about IMU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 1:5609c1795245 1 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 2 // project: TurtleBot Project //
worasuchad 1:5609c1795245 3 // code v.: 1.0 //
worasuchad 1:5609c1795245 4 // board : NUCLEO-F303KB //
worasuchad 1:5609c1795245 5 // date : 20/2/2018 //
worasuchad 1:5609c1795245 6 // code by: Coding on Earth by Humans //
worasuchad 1:5609c1795245 7 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 8
worasuchad 1:5609c1795245 9 ///////////////////////// init ////////////////////////////////
worasuchad 1:5609c1795245 10 //////////////////////////////////////////////////////////////////
Khanchana 0:812929a5d5ad 11 #include "mbed.h"
Khanchana 0:812929a5d5ad 12 #include "rtos.h"
Khanchana 0:812929a5d5ad 13
Khanchana 0:812929a5d5ad 14 Serial pc(USBTX, USBRX);
worasuchad 1:5609c1795245 15
worasuchad 1:5609c1795245 16 Thread thread1; //control servo left
worasuchad 1:5609c1795245 17 Thread thread2; //control servo right
worasuchad 1:5609c1795245 18 Thread thread3; //read data from IMU
worasuchad 1:5609c1795245 19
worasuchad 1:5609c1795245 20 ///////////////////////// IMU ////////////////////////////////
worasuchad 1:5609c1795245 21 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 22 #include "MPU9250.h"
worasuchad 1:5609c1795245 23
worasuchad 1:5609c1795245 24 float sum = 0;
worasuchad 1:5609c1795245 25 uint32_t sumCount = 0;
worasuchad 1:5609c1795245 26 char buffer[14];
worasuchad 1:5609c1795245 27 float origin = 0;
worasuchad 1:5609c1795245 28
worasuchad 1:5609c1795245 29 MPU9250 mpu9250;
worasuchad 1:5609c1795245 30 Timer t;
worasuchad 1:5609c1795245 31
worasuchad 1:5609c1795245 32
worasuchad 1:5609c1795245 33 ///////////////////////// Servo ////////////////////////////////
worasuchad 1:5609c1795245 34 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 35 #include "Servo.h"
worasuchad 1:5609c1795245 36 Servo Servo1(D10);
Khanchana 0:812929a5d5ad 37 Servo Servo2(D6);
worasuchad 1:5609c1795245 38 Servo Servo3(D8);
worasuchad 1:5609c1795245 39 Servo Servo4(D9);
worasuchad 1:5609c1795245 40 /*
Khanchana 0:812929a5d5ad 41 int pos_up_start;
Khanchana 0:812929a5d5ad 42 int pos_up_end;
Khanchana 0:812929a5d5ad 43 int pos_down_start;
worasuchad 1:5609c1795245 44 int pos_down_end;*/
Khanchana 0:812929a5d5ad 45
worasuchad 1:5609c1795245 46 int pos_down_start = 1400;
worasuchad 1:5609c1795245 47 int pos_down_end = 1600;
worasuchad 1:5609c1795245 48 int pos_up_start = 1000;
worasuchad 1:5609c1795245 49 int pos_up_end = 1600;
Khanchana 0:812929a5d5ad 50
worasuchad 1:5609c1795245 51 ///////////////////////// prototype func ///////////////////////
worasuchad 1:5609c1795245 52 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 53 void myservoLeft();
worasuchad 1:5609c1795245 54 void myservoRight();
worasuchad 1:5609c1795245 55 void IMU();
Khanchana 0:812929a5d5ad 56
worasuchad 1:5609c1795245 57 ///////////////////////// main ////////////////////////////
worasuchad 1:5609c1795245 58 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 59 int main()
worasuchad 1:5609c1795245 60 {
worasuchad 1:5609c1795245 61 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 62 thread2.start(myservoRight);
worasuchad 1:5609c1795245 63 IMU();
worasuchad 1:5609c1795245 64 /* while(1)
worasuchad 1:5609c1795245 65 {
Khanchana 0:812929a5d5ad 66 printf("Hello World! Turtlebot is READY\n");
Khanchana 0:812929a5d5ad 67 printf("case 1-5\n");
worasuchad 1:5609c1795245 68 switch(pc.getc())
worasuchad 1:5609c1795245 69 {
Khanchana 0:812929a5d5ad 70 case '1':
Khanchana 0:812929a5d5ad 71 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 72 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 73 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 74 pos_up_end = 1700;
Khanchana 0:812929a5d5ad 75 break;
Khanchana 0:812929a5d5ad 76 case '2':
Khanchana 0:812929a5d5ad 77 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 78 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 79 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 80 pos_up_end = 1600;
Khanchana 0:812929a5d5ad 81 break;
Khanchana 0:812929a5d5ad 82 case '3':
Khanchana 0:812929a5d5ad 83 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 84 pos_down_end = 1650;
Khanchana 0:812929a5d5ad 85 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 86 pos_up_end = 1500;
Khanchana 0:812929a5d5ad 87 break;
Khanchana 0:812929a5d5ad 88 case '4':
Khanchana 0:812929a5d5ad 89 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 90 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 91 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 92 pos_up_end = 1650;
Khanchana 0:812929a5d5ad 93 break;
Khanchana 0:812929a5d5ad 94 case '5':
Khanchana 0:812929a5d5ad 95 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 96 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 97 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 98 pos_up_end = 1550;
Khanchana 0:812929a5d5ad 99 break;
Khanchana 0:812929a5d5ad 100 }
Khanchana 0:812929a5d5ad 101 printf("position down motor start = %d\n", pos_down_start);
Khanchana 0:812929a5d5ad 102 printf("position down motor end = %d\n", pos_down_end);
Khanchana 0:812929a5d5ad 103 printf("position up motor start = %d\n", pos_up_start);
Khanchana 0:812929a5d5ad 104 printf("position up motor end = %d\n", pos_up_end);
worasuchad 1:5609c1795245 105 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 106 thread2.start(myservoRight);
worasuchad 1:5609c1795245 107 thread3.start(IMU);
worasuchad 1:5609c1795245 108 } */
worasuchad 1:5609c1795245 109 }
worasuchad 1:5609c1795245 110
worasuchad 1:5609c1795245 111
worasuchad 1:5609c1795245 112 ///////////////////////// myservoLeft /////////////////////////
worasuchad 1:5609c1795245 113 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 114 void myservoLeft()
worasuchad 1:5609c1795245 115 {
worasuchad 1:5609c1795245 116 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 117 {
worasuchad 1:5609c1795245 118 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 119 Servo2.Disable();
worasuchad 1:5609c1795245 120
worasuchad 1:5609c1795245 121 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 122 {
worasuchad 1:5609c1795245 123 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 124 wait(0.01);
worasuchad 1:5609c1795245 125 }
worasuchad 1:5609c1795245 126
worasuchad 1:5609c1795245 127 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 128 Servo1.Disable();
worasuchad 1:5609c1795245 129
worasuchad 1:5609c1795245 130 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 131 {
worasuchad 1:5609c1795245 132 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 133 wait(0.01);
worasuchad 1:5609c1795245 134 }
worasuchad 1:5609c1795245 135
worasuchad 1:5609c1795245 136 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 137 Servo2.Disable();
worasuchad 1:5609c1795245 138
worasuchad 1:5609c1795245 139 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 140 {
worasuchad 1:5609c1795245 141 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 142 wait(0.01);
worasuchad 1:5609c1795245 143 }
worasuchad 1:5609c1795245 144
worasuchad 1:5609c1795245 145 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 146 Servo1.Disable();
worasuchad 1:5609c1795245 147
worasuchad 1:5609c1795245 148 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 149 {
worasuchad 1:5609c1795245 150 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 151 wait(0.01);
worasuchad 1:5609c1795245 152 }
worasuchad 1:5609c1795245 153 }
worasuchad 1:5609c1795245 154 }
worasuchad 1:5609c1795245 155
worasuchad 1:5609c1795245 156 ///////////////////////// myservoRight ///////////////////////
worasuchad 1:5609c1795245 157 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 158 void myservoRight()
worasuchad 1:5609c1795245 159 {
worasuchad 1:5609c1795245 160 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 161 {
worasuchad 1:5609c1795245 162 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 163 Servo4.Disable();
worasuchad 1:5609c1795245 164
worasuchad 1:5609c1795245 165 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 166 {
worasuchad 1:5609c1795245 167 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 168 wait(0.01);
worasuchad 1:5609c1795245 169 }
worasuchad 1:5609c1795245 170
worasuchad 1:5609c1795245 171 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 172 Servo3.Disable();
worasuchad 1:5609c1795245 173
worasuchad 1:5609c1795245 174 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 175 {
worasuchad 1:5609c1795245 176 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 177 wait(0.01);
worasuchad 1:5609c1795245 178 }
worasuchad 1:5609c1795245 179
worasuchad 1:5609c1795245 180 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 181 Servo4.Disable();
worasuchad 1:5609c1795245 182
worasuchad 1:5609c1795245 183 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 184 {
worasuchad 1:5609c1795245 185 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 186 wait(0.01);
worasuchad 1:5609c1795245 187 }
worasuchad 1:5609c1795245 188
worasuchad 1:5609c1795245 189 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 190 Servo3.Disable();
worasuchad 1:5609c1795245 191
worasuchad 1:5609c1795245 192 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 193 {
worasuchad 1:5609c1795245 194 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 195 wait(0.01);
worasuchad 1:5609c1795245 196 }
Khanchana 0:812929a5d5ad 197 }
worasuchad 1:5609c1795245 198 }
worasuchad 1:5609c1795245 199
worasuchad 1:5609c1795245 200 ///////////////////////// IMU ///////////////////////
worasuchad 1:5609c1795245 201 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 202 void IMU()
worasuchad 1:5609c1795245 203 {
worasuchad 1:5609c1795245 204 //Set up I2C
worasuchad 1:5609c1795245 205 i2c.frequency(400000); // use fast (400 kHz) I2C
worasuchad 1:5609c1795245 206
worasuchad 1:5609c1795245 207 //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
worasuchad 1:5609c1795245 208 t.start();
worasuchad 1:5609c1795245 209
worasuchad 1:5609c1795245 210 // Read the WHO_AM_I register, this is a good test of communication
worasuchad 1:5609c1795245 211 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
worasuchad 1:5609c1795245 212 //pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
worasuchad 1:5609c1795245 213
worasuchad 1:5609c1795245 214 if (whoami == 0x73 ) // WHO_AM_I should always be 0x68
worasuchad 1:5609c1795245 215 {
worasuchad 1:5609c1795245 216 //pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
worasuchad 1:5609c1795245 217 //pc.printf("MPU9250 is online...\n\r");
worasuchad 1:5609c1795245 218 sprintf(buffer, "0x%x", whoami);
worasuchad 1:5609c1795245 219 wait(1);
worasuchad 1:5609c1795245 220
worasuchad 1:5609c1795245 221 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
worasuchad 1:5609c1795245 222 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
worasuchad 2:d4bd9ff10e8e 223 pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
worasuchad 2:d4bd9ff10e8e 224 pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
worasuchad 2:d4bd9ff10e8e 225 pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
worasuchad 2:d4bd9ff10e8e 226 pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
worasuchad 2:d4bd9ff10e8e 227 pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
worasuchad 2:d4bd9ff10e8e 228 pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
worasuchad 1:5609c1795245 229 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
worasuchad 2:d4bd9ff10e8e 230 pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
worasuchad 2:d4bd9ff10e8e 231 pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
worasuchad 2:d4bd9ff10e8e 232 pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
worasuchad 2:d4bd9ff10e8e 233 pc.printf("x accel bias = %f\n\r", accelBias[0]);
worasuchad 2:d4bd9ff10e8e 234 pc.printf("y accel bias = %f\n\r", accelBias[1]);
worasuchad 2:d4bd9ff10e8e 235 pc.printf("z accel bias = %f\n\r", accelBias[2]);
worasuchad 1:5609c1795245 236 wait(2);
worasuchad 1:5609c1795245 237 mpu9250.initMPU9250();
worasuchad 2:d4bd9ff10e8e 238 pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
worasuchad 1:5609c1795245 239 mpu9250.initAK8963(magCalibration);
worasuchad 2:d4bd9ff10e8e 240 pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
worasuchad 2:d4bd9ff10e8e 241 pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
worasuchad 2:d4bd9ff10e8e 242 pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
worasuchad 1:5609c1795245 243
worasuchad 1:5609c1795245 244 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
worasuchad 1:5609c1795245 245 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
worasuchad 1:5609c1795245 246 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
worasuchad 1:5609c1795245 247 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
worasuchad 1:5609c1795245 248 wait(1);
worasuchad 1:5609c1795245 249 }
worasuchad 1:5609c1795245 250 else
worasuchad 1:5609c1795245 251 {
worasuchad 1:5609c1795245 252 pc.printf("Could not connect to MPU9250: \n\r");
worasuchad 1:5609c1795245 253 pc.printf("%#x \n", whoami);
worasuchad 1:5609c1795245 254
worasuchad 1:5609c1795245 255 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
worasuchad 1:5609c1795245 256
worasuchad 1:5609c1795245 257 while(1) ; // Loop forever if communication doesn't happen
worasuchad 1:5609c1795245 258 }
worasuchad 1:5609c1795245 259
worasuchad 1:5609c1795245 260 mpu9250.getAres(); // Get accelerometer sensitivity
worasuchad 1:5609c1795245 261 mpu9250.getGres(); // Get gyro sensitivity
worasuchad 1:5609c1795245 262 mpu9250.getMres(); // Get magnetometer sensitivity
worasuchad 2:d4bd9ff10e8e 263 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
worasuchad 2:d4bd9ff10e8e 264 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
worasuchad 2:d4bd9ff10e8e 265 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
worasuchad 1:5609c1795245 266 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
worasuchad 1:5609c1795245 267 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
worasuchad 1:5609c1795245 268 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
worasuchad 1:5609c1795245 269
worasuchad 2:d4bd9ff10e8e 270
worasuchad 1:5609c1795245 271 while(1)
worasuchad 1:5609c1795245 272 {
worasuchad 1:5609c1795245 273 // If intPin goes high, all data registers have new data
worasuchad 1:5609c1795245 274 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
worasuchad 1:5609c1795245 275
worasuchad 1:5609c1795245 276 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 277 // Now we'll calculate the accleration value into actual g's
worasuchad 1:5609c1795245 278 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
worasuchad 1:5609c1795245 279 ay = (float)accelCount[1]*aRes - accelBias[1];
worasuchad 1:5609c1795245 280 az = (float)accelCount[2]*aRes - accelBias[2];
worasuchad 1:5609c1795245 281
worasuchad 1:5609c1795245 282 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 283 // Calculate the gyro value into actual degrees per second
worasuchad 1:5609c1795245 284 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
worasuchad 1:5609c1795245 285 gy = (float)gyroCount[1]*gRes - gyroBias[1];
worasuchad 1:5609c1795245 286 gz = (float)gyroCount[2]*gRes - gyroBias[2];
worasuchad 1:5609c1795245 287
worasuchad 1:5609c1795245 288 mpu9250.readMagData(magCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 289 // Calculate the magnetometer values in milliGauss
worasuchad 1:5609c1795245 290 // Include factory calibration per data sheet and user environmental corrections
worasuchad 1:5609c1795245 291 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
worasuchad 1:5609c1795245 292 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
worasuchad 1:5609c1795245 293 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
worasuchad 1:5609c1795245 294 }
worasuchad 1:5609c1795245 295
worasuchad 1:5609c1795245 296 Now = t.read_us();
worasuchad 1:5609c1795245 297 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
worasuchad 1:5609c1795245 298 lastUpdate = Now;
worasuchad 1:5609c1795245 299
worasuchad 1:5609c1795245 300 sum += deltat;
worasuchad 1:5609c1795245 301 sumCount++;
worasuchad 1:5609c1795245 302
worasuchad 1:5609c1795245 303 //if(lastUpdate - firstUpdate > 10000000.0f)
worasuchad 1:5609c1795245 304 //{
worasuchad 1:5609c1795245 305 //beta = 0.04; // decrease filter gain after stabilized
worasuchad 1:5609c1795245 306 //zeta = 0.015; // increasey bias drift gain after stabilized
worasuchad 1:5609c1795245 307 //}
worasuchad 1:5609c1795245 308
worasuchad 1:5609c1795245 309 //Pass gyro rate as rad/s
worasuchad 2:d4bd9ff10e8e 310 mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
worasuchad 2:d4bd9ff10e8e 311 //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
worasuchad 1:5609c1795245 312
worasuchad 1:5609c1795245 313 //Serial print and/or display at 0.5 s rate independent of data rates
worasuchad 1:5609c1795245 314 delt_t = t.read_ms() - count;
worasuchad 1:5609c1795245 315
worasuchad 2:d4bd9ff10e8e 316 if (delt_t > 10)
worasuchad 1:5609c1795245 317 { // update LCD once per half-second independent of read rate
worasuchad 1:5609c1795245 318
worasuchad 1:5609c1795245 319 //pc.printf("ax = %f", 1000*ax);
worasuchad 1:5609c1795245 320 //pc.printf(" ay = %f", 1000*ay);
worasuchad 1:5609c1795245 321 //pc.printf(" az = %f mg\n\r", 1000*az);
worasuchad 1:5609c1795245 322
worasuchad 1:5609c1795245 323 //pc.printf("gx = %f", gx);
worasuchad 1:5609c1795245 324 //pc.printf(" gy = %f", gy);
worasuchad 1:5609c1795245 325 //pc.printf(" gz = %f deg/s\n\r", gz);
worasuchad 1:5609c1795245 326
worasuchad 1:5609c1795245 327 //pc.printf("gx = %f", mx);
worasuchad 1:5609c1795245 328 //pc.printf(" gy = %f", my);
worasuchad 1:5609c1795245 329 //pc.printf(" gz = %f mG\n\r", mz);
worasuchad 1:5609c1795245 330
worasuchad 1:5609c1795245 331 //tempCount = mpu9250.readTempData(); // Read the adc values
worasuchad 1:5609c1795245 332 //temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
worasuchad 1:5609c1795245 333 //pc.printf(" temperature = %f C\n\r", temperature);
worasuchad 1:5609c1795245 334
worasuchad 1:5609c1795245 335 //pc.printf("q0 = %f\n\r", q[0]);
worasuchad 1:5609c1795245 336 //pc.printf("q1 = %f\n\r", q[1]);
worasuchad 1:5609c1795245 337 //pc.printf("q2 = %f\n\r", q[2]);
worasuchad 1:5609c1795245 338 //pc.printf("q3 = %f\n\r", q[3]);
worasuchad 1:5609c1795245 339
worasuchad 1:5609c1795245 340
worasuchad 1:5609c1795245 341 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
worasuchad 1:5609c1795245 342 // In this coordinate system, the positive z-axis is down toward Earth.
worasuchad 1:5609c1795245 343 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
worasuchad 1:5609c1795245 344 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
worasuchad 1:5609c1795245 345 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
worasuchad 1:5609c1795245 346 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
worasuchad 1:5609c1795245 347 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
worasuchad 1:5609c1795245 348 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
worasuchad 1:5609c1795245 349 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
worasuchad 1:5609c1795245 350 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
worasuchad 1:5609c1795245 351 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
worasuchad 1:5609c1795245 352 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
worasuchad 1:5609c1795245 353 pitch *= 180.0f / PI;
worasuchad 1:5609c1795245 354 yaw *= 180.0f / PI;
worasuchad 2:d4bd9ff10e8e 355 //yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
worasuchad 2:d4bd9ff10e8e 356 yaw += 0.8f;
worasuchad 1:5609c1795245 357 roll *= 180.0f / PI;
worasuchad 1:5609c1795245 358
worasuchad 2:d4bd9ff10e8e 359 pc.printf("%f %f %f %f \n\r",roll, pitch, yaw,origin);
worasuchad 1:5609c1795245 360 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
worasuchad 1:5609c1795245 361 //sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
worasuchad 1:5609c1795245 362 //lcd.printString(buffer, 0, 4);
worasuchad 1:5609c1795245 363 //sprintf(buffer, "rate = %f", (float) sumCount/sum);
worasuchad 1:5609c1795245 364 //lcd.printString(buffer, 0, 5);
worasuchad 1:5609c1795245 365
worasuchad 1:5609c1795245 366 myled= !myled;
worasuchad 1:5609c1795245 367 count = t.read_ms();
worasuchad 1:5609c1795245 368
worasuchad 1:5609c1795245 369 if(count > 1<<21)
worasuchad 1:5609c1795245 370 {
worasuchad 1:5609c1795245 371 t.start(); // start the timer over again if ~30 minutes has passed
worasuchad 1:5609c1795245 372 count = 0;
worasuchad 1:5609c1795245 373 deltat= 0;
worasuchad 1:5609c1795245 374 lastUpdate = t.read_us();
worasuchad 1:5609c1795245 375 }
worasuchad 1:5609c1795245 376 sum = 0;
worasuchad 1:5609c1795245 377 sumCount = 0;
worasuchad 1:5609c1795245 378 }
worasuchad 1:5609c1795245 379 }
Khanchana 0:812929a5d5ad 380 }