I have been working on a quadcopter that uses both the MPU6050 and the HMC5883L for the IMU. Now, I have been playing with both the freeIMU library by Fabio (RIP) and the direct access libraries written by Jeff Rowberg. Now, since i do like to have more control over my devices, I have utilized Jeff Rowberg’s libraries to access both the MPU6050 and the HMC5883L. I can read raw data from both without any problems. Using a kalman filter I was able to get nice “clean” data from the MPU6050 (x and y) but not the z axis. I could not find a formula that I could implement. Since I have the HMC5883L on board, I figured I didnt need to in the first place.
Now, the problem is as follows. I can read raw data from the HMC5883L without any problems, but when I apply tan2(mag_x,mag_y) to get the heading, then convert it to degrees I get very strange results. here is the code for the math and my results.
// get the raw data mag.getHeading(&mx, &my, &mz); // calculate the heading in radians // need to *0.92f to normalize the raw data double heading = atan2((mx * 0.92f), (my * 0.92f)); // correct the sign if necessary if(heading < 0) heading += 2*PI; heading = heading * 180/PI; // I tried both the M_PI and PI
so that is the code in a nutshell.
here is my output at 0,90,180,270 degrees respectively:
mag: 53.36 -635.72 -512.44 heading: 175.20 mag: -174.80 -439.76 -498.64 heading: 201.68 mag: -366.16 -641.24 -488.52 heading: 209.73 mag: -176.64 -856.52 -496.80 heading: 191.65
This is a custom design though I did follow the datasheet closely, it is possible thought that there is some stray magnetic field that is causing this weird behavior. Im out of ideas at this point and any help would be appreciated.
If I dont figure out a way to solve this problem, I will be relaying solely on the MPU6050. How could I calculate the z axis?