Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Magnetometer and Madgwick or Mahony does not work in case of tilting #180

Closed
mindchanger82 opened this issue Aug 30, 2017 · 19 comments
Closed

Magnetometer and Madgwick or Mahony does not work in case of tilting #180

mindchanger82 opened this issue Aug 30, 2017 · 19 comments

Comments

@mindchanger82
Copy link

Dear Chris.
First of all thanks a lot for your great work for all in this esp and 9250 things. Great Job.

I'm facing a probem right now. I successfully get the imu to run with your example codes but it seems, that the filter functions have no influence to the mag data when i turn the device over some axes.
EG I mountet the imu to a robotic lawn mower. Everytime the mower moves over a small hill it moves to somewhere but not to the should direction.
I tested it on a breakout board inside with another imu and esp32 device with the same result.
Here is my function to print out the heading degrees.
I placed it in the loop, directly under the serial prints of mx,my,mz
`
float heading = atan2(my, mx);
float headingDegrees=0;
float declinationAngle = 30.45/1000;
heading += declinationAngle;

if(heading < 0)
  heading += 2*PI;

if(heading > 2*PI)
  heading -= 2*PI;

// Convert radians to degrees for readability.
//float headingDegrees = 360-(heading * 180/M_PI);
headingDegrees = heading * 180/M_PI;
headingDegrees = 360.00 - headingDegrees;
Serial.print("Heading = "); Serial.print( (int)headingDegrees ); Serial.println(" Degrees!");`

So everytime i softly move the device (yaw or pitch) the mag data changes a lot to unsensful values.

Have you got any idea? Is my code wrong for this case?

PS: I used your latest "MPU9250_MS5637_AHRS_t3.ino" and removed all the display stuff.

Thanks in advance and with best regards

Marius
@kriswiner
Copy link
Owner

kriswiner commented Aug 30, 2017 via email

@mindchanger82
Copy link
Author

mindchanger82 commented Aug 30, 2017 •
edited
Loading

Hi Chris.

Thanks for your reply. That is exactly my problem. Either I don't know how to use the sensor fusion, that's my goal to achive, nor I don't exactly know how to calibrate the sensors. I just used your mag calibration with figure 8 waving.

@TediumRemedy
Copy link

@mindchanger82 the idea is to feed accelerometer, magnetometer and gyroscope measurements into the madgwickQuaternionUpdate, which, when run, fills out the q[0]...q[3] array (quaternion), which you then can convert into 3 angles - heading (yaw), tilt around 1 axis (pitch), tilt around another axis (roll) using simple formulas https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
madgwickQuaternionUpdate is the sensor fusion algorithm which turns raw sensor measurements into more or less reliable estimate of the Euler angles.

@mindchanger82
Copy link
Author

Dear TediumRemedy.
That means:
/from wiki/
// yaw (z-axis rotation)
double siny = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny, cosy);

but from where do I know which arry pos (q[0] or q[xyz] stands for q.w or q.z ?
Sorry but I'm not really seeing through this.

@TediumRemedy
Copy link

TediumRemedy commented Aug 30, 2017 •
edited
Loading

@mindchanger82 you can use pieces of code from: https://github.com/kriswiner/MPU9250/blob/master/MPU9250_MS5637_AHRS_t3.ino

// yaw = atan2f(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]);
// pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
// roll = atan2f(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]);
// pitch *= 180.0f / PI;
// yaw *= 180.0f / PI;
// yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
// if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360
// roll *= 180.0f / PI;

and you get yaw/pitch/roll in degrees (can be negative).

The original code uses another conversion, apparently to save Arduino time on mathematical operations:

a12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
a22 = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
a31 = 2.0f * (q[0] * q[1] + q[2] * q[3]);
a32 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
a33 = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
pitch = -asinf(a32);
roll = atan2f(a31, a33);
yaw = atan2f(a12, a22);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360
roll *= 180.0f / PI;
lin_ax = ax + a31;
lin_ay = ay + a32;
lin_az = az - a33;

@mindchanger82
Copy link
Author

woaaaaah. Thank you so much.

I used, as you said, the following lines:
yaw = atan2f(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]); yaw *= 180.0f / PI; yaw += 1.44f; // Declination at Düsseldorf Germany if(yaw < 0) yaw += 360.0f; // Ensure yaw stays between 0 and 360
The data is now very stable, but false :-) haha, real south is 279 degrees with the code... mhhhhhh where is the error.

@TediumRemedy
Copy link

TediumRemedy commented Aug 30, 2017 •
edited
Loading

That's a good question.
You could "calibrate" it by just adding a number of degrees to yaw that make yaw 0º at the direction where a real compass points to north, instead of adding the declination to yaw.
I am getting a wrong geographical heading value too (I don't need absolute heading though).
Maybe Kris might know why this is happening?

@kriswiner
Copy link
Owner

kriswiner commented Aug 30, 2017 via email

@kriswiner
Copy link
Owner

kriswiner commented Aug 30, 2017 via email

@TediumRemedy
Copy link

Thanks Kris

@mindchanger82
Copy link
Author

Thanks Kris and TediumRemedy.
Everything works fine now.

@henzim
Copy link

henzim commented Sep 9, 2017

@mindchanger82 glad you got your setup working! I am also faced with "yaw" problems. Could you please have a look at [https://github.com//issues/182](my issue) and confirm you don't have this problem in your setup. In brief, the problem is that the yaw angle "misses" a couple of degrees when rotating the setup, e.g., I rotate the MPU by 90° and the yaw changes by 80°... Thanks alot!

@mindchanger82
Copy link
Author

Dear henzim.
Sorry for my late reply. I'm on a biz trip and I'll be back on sunday. I will try to find out if I have the same prob too.
Thanks

@henzim
Copy link

henzim commented Sep 12, 2017

Thanks!

@mindchanger82
Copy link
Author

Hi Henzim. Sorry for my late replay.
I tested it the last weekend and and I don't get these kind of errors. But keep in mind that I use a esp32 and it seems to be a "bit faster" than the arduino mini pro.

Best regards

@henzim
Copy link

henzim commented Oct 17, 2017 via email

@kriswiner
Copy link
Owner

kriswiner commented Nov 1, 2022 via email

@Kaijudo2
Copy link

Kaijudo2 commented Nov 1, 2022

I am using adafruit esp32 feather

What MCU are you using?
…
On Tue, Nov 1, 2022 at 6:30 AM Kaijudo2 @.> wrote: I am having issue with 'TwoWire i2c'. Program says "no matching function for call to 'TwoWire::TwoWire()'". Would really appreciate if anyone could help me! — Reply to this email directly, view it on GitHub <#180 (comment)>, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKVJDGECTRPOMO7YHH3WGELP7ANCNFSM4DY3PIEQ . You are receiving this because you were mentioned.Message ID: @.>

@kriswiner
Copy link
Owner

kriswiner commented Nov 1, 2022 via email

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
5 participants
@TediumRemedy @kriswiner @mindchanger82 @henzim @Kaijudo2