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