Baser Kandehir / Mbed 2 deprecated IMU_fusion Featured

Program can calculate roll, pitch and yaw angles from the raw data that comes from IMU. Yaw angle is compensated for tilt. All the angles are sent to the matlab for further processing.

Dependencies:   HMC5883L MPU6050 ledControl2 mbed

Dependents:   IMU_fusion_9DOF

main.cpp@2:3881894aaff5, 2015-08-06 (annotated)

Committer:
BaserK
Date:
Thu Aug 06 12:25:49 2015 +0000
Revision:
2:3881894aaff5
Parent:
1:823490ecf085 
comment update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:d8d055e8f830 1 /* Calculating Roll, Pitch and Yaw angles from IMU
BaserK 0:d8d055e8f830 2 *
BaserK 0:d8d055e8f830 3 * @author: Baser Kandehir
BaserK 0:d8d055e8f830 4 * @date: August 5, 2015
BaserK 0:d8d055e8f830 5 * @license: MIT license
BaserK 0:d8d055e8f830 6 *
BaserK 0:d8d055e8f830 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 0:d8d055e8f830 8 *
BaserK 0:d8d055e8f830 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 0:d8d055e8f830 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 0:d8d055e8f830 11 * in the Software without restriction, including without limitation the rights
BaserK 0:d8d055e8f830 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 0:d8d055e8f830 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 0:d8d055e8f830 14 * furnished to do so, subject to the following conditions:
BaserK 0:d8d055e8f830 15 *
BaserK 0:d8d055e8f830 16 * The above copyright notice and this permission notice shall be included in
BaserK 0:d8d055e8f830 17 * all copies or substantial portions of the Software.
BaserK 0:d8d055e8f830 18 *
BaserK 0:d8d055e8f830 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 0:d8d055e8f830 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 0:d8d055e8f830 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 0:d8d055e8f830 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 0:d8d055e8f830 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 0:d8d055e8f830 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 0:d8d055e8f830 25 * THE SOFTWARE.
BaserK 0:d8d055e8f830 26 *
BaserK 0:d8d055e8f830 27 * @description of the program:
BaserK 0:d8d055e8f830 28 *
BaserK 0:d8d055e8f830 29 * Program can calculate roll, pitch and yaw angles from the raw data that comes
BaserK 0:d8d055e8f830 30 * from IMU. Yaw angle is compensated for tilt. All the angles are sent to the matlab
BaserK 2:3881894aaff5 31 * for further processing.
BaserK 0:d8d055e8f830 32 *
BaserK 0:d8d055e8f830 33 */
BaserK 0:d8d055e8f830 34
BaserK 0:d8d055e8f830 35 #include "mbed.h"
BaserK 0:d8d055e8f830 36 #include "HMC5883L.h"
BaserK 0:d8d055e8f830 37 #include "MPU6050.h"
BaserK 0:d8d055e8f830 38 #include "ledControl.h"
BaserK 0:d8d055e8f830 39
BaserK 0:d8d055e8f830 40 Serial pc(USBTX,USBRX);
BaserK 0:d8d055e8f830 41 MPU6050 mpu6050;
BaserK 0:d8d055e8f830 42 HMC5883L hmc5883l;
BaserK 0:d8d055e8f830 43 Ticker toggler1;
BaserK 0:d8d055e8f830 44 Ticker filter;
BaserK 0:d8d055e8f830 45 Ticker compass;
BaserK 0:d8d055e8f830 46
BaserK 0:d8d055e8f830 47 void toggle_led1();
BaserK 0:d8d055e8f830 48 void toggle_led2();
BaserK 0:d8d055e8f830 49 void compFilter();
BaserK 0:d8d055e8f830 50 void tiltCompensatedAngle();
BaserK 0:d8d055e8f830 51
BaserK 0:d8d055e8f830 52 float pitchAngle = 0;
BaserK 0:d8d055e8f830 53 float rollAngle = 0;
BaserK 0:d8d055e8f830 54 float yawAngle = 0;
BaserK 0:d8d055e8f830 55
BaserK 0:d8d055e8f830 56 int main()
BaserK 0:d8d055e8f830 57 {
BaserK 0:d8d055e8f830 58 pc.baud(9600); // baud rate: 9600
BaserK 0:d8d055e8f830 59 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
BaserK 0:d8d055e8f830 60 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
BaserK 0:d8d055e8f830 61 mpu6050.init(); // Initialize the sensor
BaserK 0:d8d055e8f830 62 hmc5883l.init();
BaserK 0:d8d055e8f830 63 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
BaserK 0:d8d055e8f830 64 compass.attach(&tiltCompensatedAngle, 0.015); // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period)
BaserK 0:d8d055e8f830 65 while(1)
BaserK 0:d8d055e8f830 66 {
BaserK 0:d8d055e8f830 67 pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle); // send data to matlab
BaserK 0:d8d055e8f830 68 wait_ms(40);
BaserK 0:d8d055e8f830 69 }
BaserK 0:d8d055e8f830 70 }
BaserK 0:d8d055e8f830 71
BaserK 0:d8d055e8f830 72 void toggle_led1() {ledToggle(1);}
BaserK 0:d8d055e8f830 73 void toggle_led2() {ledToggle(2);}
BaserK 0:d8d055e8f830 74
BaserK 0:d8d055e8f830 75 /* This function is created to avoid address error that caused from Ticker.attach func */
BaserK 0:d8d055e8f830 76 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
BaserK 0:d8d055e8f830 77
BaserK 0:d8d055e8f830 78 /* Tilt compensated compass data */
BaserK 0:d8d055e8f830 79 // Works well for tilt in +/- 50 deg range
BaserK 0:d8d055e8f830 80 void tiltCompensatedAngle()
BaserK 0:d8d055e8f830 81 {
BaserK 0:d8d055e8f830 82 float mag_Data[3], Xh, Yh;
BaserK 0:d8d055e8f830 83 hmc5883l.readMagData(mag_Data);
BaserK 0:d8d055e8f830 84
BaserK 0:d8d055e8f830 85 Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ;
BaserK 0:d8d055e8f830 86
BaserK 0:d8d055e8f830 87 Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) +
BaserK 0:d8d055e8f830 88 mag_Data[1] * cos(pitchAngle*PI/180) -
BaserK 0:d8d055e8f830 89 mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ;
BaserK 0:d8d055e8f830 90
BaserK 0:d8d055e8f830 91 /* Calculate the compensated heading angle */
BaserK 0:d8d055e8f830 92 double heading = atan2(Yh, Xh);
BaserK 0:d8d055e8f830 93
BaserK 0:d8d055e8f830 94 // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
BaserK 0:d8d055e8f830 95 // declinationAngle can be found here http://www.magnetic-declination.com/
BaserK 0:d8d055e8f830 96 // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
BaserK 0:d8d055e8f830 97 float declinationAngle = 0.096;
BaserK 0:d8d055e8f830 98 heading += declinationAngle;
BaserK 0:d8d055e8f830 99
BaserK 0:d8d055e8f830 100 // Correct for when signs are reversed.
BaserK 0:d8d055e8f830 101 if(heading < 0)
BaserK 0:d8d055e8f830 102 heading += 2*PI;
BaserK 0:d8d055e8f830 103
BaserK 0:d8d055e8f830 104 // Check for wrap due to addition of declination.
BaserK 0:d8d055e8f830 105 if(heading > 2*PI)
BaserK 0:d8d055e8f830 106 heading -= 2*PI;
BaserK 0:d8d055e8f830 107
BaserK 0:d8d055e8f830 108 /* Convert radian to degrees */
BaserK 0:d8d055e8f830 109 heading = heading * 180 / PI;
BaserK 0:d8d055e8f830 110
BaserK 0:d8d055e8f830 111 yawAngle = heading;
BaserK 0:d8d055e8f830 112 }