latest
Dependencies: Servo mbed-rtos mbed
Fork of TurtleBot_V55 by
attitude.h@1:852156b5cca1, 2018-04-02 (annotated)
- Committer:
- 59010050
- Date:
- Mon Apr 02 10:18:44 2018 +0000
- Revision:
- 1:852156b5cca1
latest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
59010050 | 1:852156b5cca1 | 1 | #include "MPU9250.h" |
59010050 | 1:852156b5cca1 | 2 | |
59010050 | 1:852156b5cca1 | 3 | MPU9250 mpu9250; |
59010050 | 1:852156b5cca1 | 4 | |
59010050 | 1:852156b5cca1 | 5 | Timer t; |
59010050 | 1:852156b5cca1 | 6 | |
59010050 | 1:852156b5cca1 | 7 | void attitude_setup(void) |
59010050 | 1:852156b5cca1 | 8 | { |
59010050 | 1:852156b5cca1 | 9 | i2c.frequency(400000); // use fast (400 kHz) I2C |
59010050 | 1:852156b5cca1 | 10 | |
59010050 | 1:852156b5cca1 | 11 | t.start(); |
59010050 | 1:852156b5cca1 | 12 | |
59010050 | 1:852156b5cca1 | 13 | // Read the WHO_AM_I register, this is a good test of communication |
59010050 | 1:852156b5cca1 | 14 | uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 |
59010050 | 1:852156b5cca1 | 15 | |
59010050 | 1:852156b5cca1 | 16 | |
59010050 | 1:852156b5cca1 | 17 | if (whoami == 0x73) { // WHO_AM_I should always be 0x68 |
59010050 | 1:852156b5cca1 | 18 | wait(1); |
59010050 | 1:852156b5cca1 | 19 | mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration |
59010050 | 1:852156b5cca1 | 20 | mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
59010050 | 1:852156b5cca1 | 21 | wait(2); |
59010050 | 1:852156b5cca1 | 22 | mpu9250.initMPU9250(); |
59010050 | 1:852156b5cca1 | 23 | mpu9250.initAK8963(magCalibration); |
59010050 | 1:852156b5cca1 | 24 | wait(2); |
59010050 | 1:852156b5cca1 | 25 | } else while(1) ; // Loop forever if communication doesn't happen |
59010050 | 1:852156b5cca1 | 26 | |
59010050 | 1:852156b5cca1 | 27 | mpu9250.getAres(); // Get accelerometer sensitivity |
59010050 | 1:852156b5cca1 | 28 | mpu9250.getGres(); // Get gyro sensitivity |
59010050 | 1:852156b5cca1 | 29 | mpu9250.getMres(); // Get magnetometer sensitivity |
59010050 | 1:852156b5cca1 | 30 | |
59010050 | 1:852156b5cca1 | 31 | magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated |
59010050 | 1:852156b5cca1 | 32 | magbias[1] = +120.; // User environmental x-axis correction in milliGauss |
59010050 | 1:852156b5cca1 | 33 | magbias[2] = +125.; // User environmental x-axis correction in milliGauss |
59010050 | 1:852156b5cca1 | 34 | } |
59010050 | 1:852156b5cca1 | 35 | |
59010050 | 1:852156b5cca1 | 36 | int attitude_get(void) |
59010050 | 1:852156b5cca1 | 37 | { |
59010050 | 1:852156b5cca1 | 38 | // If intPin goes high, all data registers have new data |
59010050 | 1:852156b5cca1 | 39 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
59010050 | 1:852156b5cca1 | 40 | |
59010050 | 1:852156b5cca1 | 41 | mpu9250.readAccelData(accelCount); // Read the x/y/z adc values |
59010050 | 1:852156b5cca1 | 42 | // Now we'll calculate the accleration value into actual g's |
59010050 | 1:852156b5cca1 | 43 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
59010050 | 1:852156b5cca1 | 44 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
59010050 | 1:852156b5cca1 | 45 | az = (float)accelCount[2]*aRes - accelBias[2]; |
59010050 | 1:852156b5cca1 | 46 | |
59010050 | 1:852156b5cca1 | 47 | mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values |
59010050 | 1:852156b5cca1 | 48 | // Calculate the gyro value into actual degrees per second |
59010050 | 1:852156b5cca1 | 49 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
59010050 | 1:852156b5cca1 | 50 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
59010050 | 1:852156b5cca1 | 51 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
59010050 | 1:852156b5cca1 | 52 | |
59010050 | 1:852156b5cca1 | 53 | mpu9250.readMagData(magCount); // Read the x/y/z adc values |
59010050 | 1:852156b5cca1 | 54 | // Calculate the magnetometer values in milliGauss |
59010050 | 1:852156b5cca1 | 55 | // Include factory calibration per data sheet and user environmental corrections |
59010050 | 1:852156b5cca1 | 56 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set |
59010050 | 1:852156b5cca1 | 57 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
59010050 | 1:852156b5cca1 | 58 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
59010050 | 1:852156b5cca1 | 59 | |
59010050 | 1:852156b5cca1 | 60 | |
59010050 | 1:852156b5cca1 | 61 | Now = t.read_us(); |
59010050 | 1:852156b5cca1 | 62 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
59010050 | 1:852156b5cca1 | 63 | lastUpdate = Now; |
59010050 | 1:852156b5cca1 | 64 | |
59010050 | 1:852156b5cca1 | 65 | // Pass gyro rate as rad/s |
59010050 | 1:852156b5cca1 | 66 | mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
59010050 | 1:852156b5cca1 | 67 | mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
59010050 | 1:852156b5cca1 | 68 | |
59010050 | 1:852156b5cca1 | 69 | 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]); |
59010050 | 1:852156b5cca1 | 70 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
59010050 | 1:852156b5cca1 | 71 | 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]); |
59010050 | 1:852156b5cca1 | 72 | pitch *= 180.0f / PI; |
59010050 | 1:852156b5cca1 | 73 | yaw *= 180.0f / PI; |
59010050 | 1:852156b5cca1 | 74 | yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 |
59010050 | 1:852156b5cca1 | 75 | roll *= 180.0f / PI; |
59010050 | 1:852156b5cca1 | 76 | |
59010050 | 1:852156b5cca1 | 77 | return 0; |
59010050 | 1:852156b5cca1 | 78 | } |
59010050 | 1:852156b5cca1 | 79 | return -1; |
59010050 | 1:852156b5cca1 | 80 | } |
59010050 | 1:852156b5cca1 | 81 |