dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Sun Sep 23, 2018 8:34 pm

hi,
which C/C++ lib for MPU6050 can retrieve yaw, additional to pitch and roll?
The sole gyro or accel values are not interesting actually.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Mon Sep 24, 2018 4:32 am

You question fascinated me so I googled a bit. I could not find any code around that will read roll, pitch and yaw out of the MPU6050. Only raw accel and gyro values.

That's OK. You can always feed those raw accel and gyro readings into a sensor fusion algorithm that will turn them into roll, pitch and yaw. A great example of that is Sebastian Madgwick's IMU algorithm.

The IMU will tend to drift over time so for better long term stability add a compass and mix that data using Madgwick's AHRS algorithm.

Both the IMU and AHRS code are available here: http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ in C# and C. Do watch the videos to see how well this works.

I played with this a while back and made a version in Javascript that shows the 3D model in the browser in real time:
https://github.com/ZiCog/madgwick.js/tree/master

It's pretty easy to copy that C code into your project and use it.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Mon Sep 24, 2018 10:26 am

unfortunately I have not the maths skills to calculate yaw, pitch, and roll angles from gyro and acc values.
Consider that e.g. for pitch=0 and roll=0° then the vertical gyro z axis is significant for yaw, whilst for pitch=+80° then mainly the longitudinal x axis becomes more significant to yaw. Same for e.g. roll=80°, then the transverse y axis becomes most significant to yaw. So the problem about 3D coordinate system transformations is not trivial, and additionally the raw sensor values surely will have to be filtered (some are using extended Kalman filters to accel and gyros against gaussian and non-gaussian noise and high pass filters to eliminate drift).
But as I am no mathematician and don't understand anything about matrix math worth mentioning, so a ready-to-use lib would be highly appreciated 8-)


Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 10:47 am

dsyleixa123,

What, you don't like my suggestion?

You are right this is not trivial. Solutions include Kalman filters, quaternions etc. Admittedly much of that is over my head.
a ready-to-use lib would be highly appreciated
The Madgwick code I linked to is ready to use, just drop it into your C program and call it with gyro, accel and compass readings as parameters.

The output is a quaternion but that can be converted to Euler angles (roll, pith, yaw) in a few lines of code, examples in C and Python here: https://en.wikipedia.org/wiki/Conversio ... ler_angles

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 11:30 am

hmm, I have no idea about quaternions, Kalman filters, and raw values, I am no mathematician and no computer scientist. In the Madgwick link I am completely overburdened by those dozens of files: which are the ones I need to use? I actually only need a lib to be #included, and then calculating yaw, pitch, and roll after attaching my MPU6050 to i2c, ready to use.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 2:23 pm

Do not panic, you do not need to be a mathematician or computer scientist to use the Madgwick algorithm. Just use then as black boxes.

Overwhelmed by what files? If you download the Madgwick C source code from here: http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ there are only 4 files in it. A C source and a header file for each of the the Madgwick and Mahoney algorithms.

Let's use the Madgwick algorithm. You can drop both of those files into your project, MadgwickAHRS.c and MadgwickAHRS.h

Add a "#include MadgwickAHRS.h" wherever you want to use it in your project.

"Raw" values just means the data coming out of your device before doing any processing on it. In this case you need to read acceleration in three directions (ax, ay, az), the three values from you gyro (gx, gy, gz) and, if you have it, the three magnetometer readings (mx, my, mz).

When you have read those sample values make a call to the Madgwick algorithm:

Code: Select all

    MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz);
Or use this if you have no magnetometer:

Code: Select all

    MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
This will update the quaternion values in the global variables: q0, q1 , q2, q3 which are defined in the header file.

The quaternion values might not mean much to us but we can convert them to more familiar Euler angles (roll, pitch, jaw) with the code on the wikipedia page I linked to:

Code: Select all

static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
	// roll (x-axis rotation)
	double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
	double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
	roll = atan2(sinr_cosp, cosr_cosp);

	// pitch (y-axis rotation)
	double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
	if (fabs(sinp) >= 1)
		pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		pitch = asin(sinp);

	// yaw (z-axis rotation)
	double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
	double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());  
	yaw = atan2(siny_cosp, cosy_cosp);
}
Just put your q0, q1, q2, q3 values into a Quaterniond structure and you get roll, pitch and yaw out.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 4:27 pm

thank you, I thought you meant all the files from here:
https://github.com/ZiCog/madgwick.js/tree/master

but what is a Quaterniond structure?

As to read the the basic values from the MPU6050 via i2c: will this code work, reading all values in the while(1) loop and then processing them by certain Madgwick formulas ?
https://github.com/nkolban/PiBook/blob/ ... /mpu6050.c

tbh, I am quite confused about getting all those codes and sources together into 1 program :-/

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 6:04 pm

dsyleixa123,
thank you, I thought you meant all the files from here:...
Sorry, yes, that was a bit confusing. My project has the same C files in it but a bunch of other stuff as well as it is an web application in Javascript.
...but what is a Quaterniond structure?
Good question. The wikipedia article where that code comes from does not define the Quaterniond structure. So we have to infer it by reading the code and seeing how it is used.

There we see that the Quaterniond structure is passed in as the parameter q. The only references to it in the code look like: q.w(), q.y(), q.z(), q.x().

From this we conclude that a Quaterniond is a C++ class that has methods w, x, y and z that return the respective elements of the quaternion. Also we see that w, x, y, and z should return doubles.

That Quaterniond class will need a constructor function that initializes it's w, x, y, and z. And perhaps some methods to set them.

Personally I would change that code to use a regular C struct then one could just use simple assignments to read and write it's elements. For example:

double sinr_cosp = +2.0 * (q.w * q.x + q.y * q.z);
tbh, I am quite confused about getting all those codes and sources together into 1 program :-/
Sorry I cannot advise about reading from the MPU6050, I don't have one and have never used one. But at a click glance the code you linked to looks like it should do the job.

Also I don't think I can start a tutorial on constructing C/C++ programs here. There are thousands of tutorials about that around the net and many books on the subject.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 6:49 pm

thank you for your efforts, but I'm as confused as before, if not even more.
I think I have to get a complete MPU6050 lib, providing filtered yaw+pitch+roll, ready to use, and working out of the box. I can't do that by my own, honestly.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 7:34 pm

Oh I don't know, if you put your mind to it you can do it. It might take a bit of time reading up on and experimenting with C/C++ first though. I'm sure you can find some tutorials, books etc on C/C++ around the net. Take small steps at a time.

Even if you find a ready made library that produces roll, pitch and yaw you are still going to need to know how to use it in your program. if you can do that you can do what is required to use the Madgwick code above. It's really not that much of a different problem.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Wed Sep 26, 2018 8:34 pm

I have to disagree. I have a CMPS11 which provides (absolute compass heading, pitch, and roll, and it works like a charm in non-magnetic environments, without any additional calculatations. Unfortunately I have randomly heavy ferromagnetic and electromagnetic noise, so I can't use the magnetic compass heading, instead I need just relative yaw (and that "relative yaw" would be fine, I can initialize easily to an arbitrary start value).
OTOH, your libs and sources are far too complicated and not matching to each other, and above all, unfortunately totally incomplete for use: no matching wiringPi device polling, no matching variable types, and no stochastic filtering.

So to summarize and as already stated, I don't have the skills to write such a lib, I just want to poll the values by using a ready-to-use lib which does the ugly rest.
I'm totally stunned that it shouldn't already exist yet.
And after all, "I don't have the skills" simply means : " I don't have the skills ", so no other chance.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 3:42 am

dsyleixa123,

The CMPS11 looks like a neat little board. I might have to get one. Thanks for the heads up on that. It uses the wonderful Bosch BNO055 sensor which does everything you want. Except the part bout noisy magnetic fields around I guess.

Without the compass reference any solution will drift over time. Perhaps that is not an issue in your application.

Certainly the code in the Madgwick source is complex. However making use of it is very simple. It's just a function that takes parameters and produces a result

I'd be interested to know what you mean by "not matching to each other". Could you point me to any mismatches? There might be something in there I need to fix. Thanks.

As for "totally incomplete for use" I perhaps see what you mean, taking it into use requires a bit of work but:

It has no wringPi or other external dependencies, which is good thing, it means it can be easily taken into use in any environment and with any sensors. Most people will not be using wiringPi for example.

I see no reason why whatever code one has in wiringPi can't just call this function at a suitable point.

Whilst it may have different variable types than WiringPi uses one can always cast the doubles it uses to whatever one wants.

I'm not so clear on the stochastic filtering thing. There is filtering doing on in that code. How it compares to a Kalman filter or whatever I'm not sure.

If you feel you don't have the skills to take Madgwick into use I guess we are at a dead end with my suggested solution. Hopefully someone will come by with a ready made solution.
I'm totally stunned that it shouldn't already exist yet.
It's a sad state of affairs. However I'm not stunned by it. The MPU-6050 contains a DMP to do what you want but InvenSense does not release any information as to how to use it. As explained here: https://playground.arduino.cc/Main/MPU-6050 and here:
https://www.i2cdevlib.com/devices/mpu6050#source

Hardware companies are often shitty that way and is why the solution you want does not exist. Looks like there are clever guys working on reverse engineering the DMU but that might take some time...

Sorry I don't have anything more helpful to offer.

User avatar
bensimmo
Posts: 3329
Joined: Sun Dec 28, 2014 3:02 pm
Location: East Yorkshire

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 6:40 am

Search GitHub, there seems to be example from a 2 minute search for Arduino, so just transfer and alter the settings.
Or look for something using Kalman or other Compensation.


Or ask your mpu6050 module supplier. 😂

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 7:16 am

bensimmo,

Hmm... A 2 minute google search found me i2cdevlib on github which contains code to read the MPU6050. Seems to work on the Pi as well as Ardunio. https://github.com/jrowberg/i2cdevlib

I notice it includes the firmware code for the MPU6050 required to do what dsyleixa123 wants, get roll, pitch, yaw based on calculation done by the MPU6050 itself.

This is contrary to what I found in my last 2 minute search which sated that reverse engineering was not working yet. Oh well.

The version I'm looking at now has a function:

uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity)

Which requires one to read the quaternion from the chip first. I think with some packet reading function it has in there.

All in all it seems about as complex to use as the solution I was suggesting. It would be interesting to implement both and see how how they compare in tracking accuracy.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 7:17 am

@bensimmo:
I didn't ask for the advice "do it yourself" or "search github" - libs I found are incomplete or don't work or are not suitable for the Pi.

So just a link to a complete and working lib is requested.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 7:33 am

dsyleixa123,
So just a link to a complete and working lib is requested.
OK. Here you go. A complete and working lib:

https://github.com/jrowberg/i2cdevlib

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 7:35 am

dsyleixa123,

I can understand your frustration.
I didn't ask for the advice "do it yourself" or "search github" - libs I found are incomplete or don't work.
Actually you did. Your original question was "which C/C++ lib for MPU6050 can retrieve yaw, additional to pitch and roll?". Even if someone knew exactly the right lib to use they quite likely would have to google it to remind themselves where they got it from and post you the link.

Those of use who get interested in the question but don't know will almost certainly have to google it.
So just a link to a complete and working lib is requested.
The link I gave above https://github.com/jrowberg/i2cdevlib is a complete and working lib. At least as far as I can make out from looking through the code. My apologies if it turns out not to be.

It has instructions on how to use it like any arduino/wiringpi library on the front page there.


To use the library, just place the I2Cdev .cpp/.h or .c/.h source files and any device library .cpp/.h or .c/.h source files in the same folder as your sketch (or a suitable place relative to your project build tool), and include just the device library headers that you need. Arduino users will also need to include <Wire.h> in your main sketch source file. Create a single device object (e.g. "ADXL345 accel();"), place any appropriate init function calls in your setup() routine (e.g. "accel.initialize();"), and off you go! See the example sketches inside many of the device class directories for reference.


Don't be so lazy and expect us to do all the work for you. There will be an element of "do it yourself" no matter what.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 7:45 am

come on, don't insult me of being lazy!
I looked to your link, and even to the MPU6050 examples for Arduinos, but no example can be found which writes yaw (or even pitch an roll) as an output to Serial().
https://github.com/jrowberg/i2cdevlib

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 8:34 am

dsyleixa123,
...no example can be found which writes yaw (or even pitch an roll) as an output to Serial().
Ah, now there you are asking a different question. Not just for a library but for a fully worked example of how to use it. Sorry my googling has not found you one yet.

However the link above contains a working example for the Raspberry Pi: https://github.com/jrowberg/i2cdevlib/b ... mple_1.cpp

It does not go as far as extracting roll, pitch and yaw but would be a very good starting point.

An example of getting roll, pitch, yaw is in the arduino subdirectry: https://github.com/jrowberg/i2cdevlib/b ... 0_DMP6.ino which does indeed output them to serial.

There, everything you asked for.

Heater
Posts: 10003
Joined: Tue Jul 17, 2012 3:02 pm

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 8:59 am

I notice the example linked above contains the comment:

// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)

That may or may not be an issue for your application.

The Madgwick code does not suffer from gimbal lock.

dsyleixa123
Posts: 103
Joined: Mon Jun 11, 2018 11:22 am

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 8:59 am

I tried the DMP6 example already for my Arduino, but it does not even compile for that!
https://github.com/jrowberg/i2cdevlib/b ... 0_DMP6.ino

Bibliothek I2Cdev im Ordner: D:\Arduino\portable\libraries\I2Cdev (legacy) wird verwendet
Bibliothek Wire in Version 1.0 im Ordner: D:\arduino\portable\packages\adafruit\hardware\samd\1.2.3\libraries\Wire wird verwendet
Bibliothek MPU6050 im Ordner: D:\Arduino\portable\libraries\MPU6050 (legacy) wird verwendet
exit status 1
Fehler beim Kompilieren für das Board Adafruit ItsyBitsy M0.

So PLEASE, I need a working (tested!) lib + example for the Pi ! No vague hints and ambiguous untested sources, PLEASE!

User avatar
bensimmo
Posts: 3329
Joined: Sun Dec 28, 2014 3:02 pm
Location: East Yorkshire

Re: which C/C++ lib for MPU6050 can retrieve yaw additional to pitch and roll?

Thu Sep 27, 2018 11:00 am

Wait till some somebody comes along.

If it was Python I could help, but it's not.

Return to “C/C++”