MPU6050 Quaternion

Kaeru no Ojisan · 2014-06-16 19:31

Hi, I am a newcomer for this forums and quite beginner for propeller.
I bought Propeller P8X32A QuickStart and MPU6050 recently and I am trying to transfer FreeIMU_quaternion.ino (Arduino code) + FreeIMU.cpp in Varesano.net to Propeller C.

I have succeeded to communicate between QuickStart and Proces55ing 1.5.1.
But quaternion data itself is not stable.
Plate starts rolling after 0:40.
[video]https://www.youtube.com/edit?o=U&video_id=6PU42hfmA_Y[/video]

I guess this is caused by improper proportional/integral gains or bad reading in I2C communication.

Does anyone help me to fix this ?

Environment:
Propeller P8X32A QuickStart
MPU6050
Windows 7
SimpleIDE
Processing 1.5.1

Propeller C code:
attached

Processing 1.5.1 code:
here

Thanks Kaeru no Ojisan

Kaeru no Ojisan · 2014-06-16 19:37

Please watch this.
Plate starts rolling after 0:40.

Duane Degn · 2014-06-16 20:01

Have you tried this with the Arduino? If so, how does it compare to the Propeller?

Does this rotation occur each time you run the program? If so does it always occur at the same time from the start?
I'm wondering if it's caused by a 32-bit timer rolling over. An error this drastic doesn't seem like it would be caused my P. I. parameter not being set correctly. Your sensor is stationary correct? If so, I don't think the P. and I. parameters really matter.

jazzed · 2014-06-16 20:50

What SimpleIDE version do you have?

Kaeru no Ojisan · 2014-06-16 23:58

Dear Duane & jazzed,

Thank you for your comments.

Have you tried this with the Arduino? If so, how does it compare to the Propeller?

I have checked with Arduino UNO + MPU6050. And confirmed this combination works OK, no rotation occurred.

Does this rotation occur each time you run the program? If so does it always occur at the same time from the start?

Yes, with QuickStart + MPU6050, rotation occurs each time, but it does not occur at the same time from the start. Sometimes it occurs from the beginning.

I'm wondering if it's caused by a 32-bit timer rolling over.

In comptation of quaternion, sampleFreq is calculated as follows;

now = CNT / (CLKFREQ / 1000000.0);
sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0);
lastUpdate = now;

I amended to comment out these and sampleFreq = 12.0; , then tried, but rotation still occurred.

What SimpleIDE version do you have?

SimpleIDE Version 0.9.45

jazzed · 2014-06-17 07:45

Since you are using simple libraries, I recommend updating the Documents\SimpleIDE folder.

That is, remove Documents\SimpleIDE\Learn, and replace it with contents found here: http://learn.parallax.com/propeller-c-set-simpleide/update-your-learn-folder

We have found that in some cases the fpucog.c file found in earlier versions can cause some trouble.

Kaeru no Ojisan · 2014-06-17 15:56

Dear jazzed,

Since you are using simple libraries, I recommend updating the Documents\SimpleIDE folder.

That is, remove Documents\SimpleIDE\Learn, and replace it with contents found here: http://learn.parallax.com/propeller-...r-learn-folder

We have found that in some cases the fpucog.c file found in earlier versions can cause some trouble.

I am using the Latest Learn Folder.
Anyhow, I have replaced, and result is same, still rotating.

jazzed · 2014-06-17 17:58

I've done all I can without having the hardware on hand for trying to duplicate the setup.

Can you recommend a convenient source for buying this? What exactly is the model part number?

Kaeru no Ojisan · 2014-06-17 18:49

Dear jazzed,

Thank you for your help.

My MPU6050 is printed as GY-521 in backside and I think this is very popular one as known as a cheap MPU6050.
I have another MPU6050 also printed GY-521, which I bought sometimes before and works very well with Arduino.

I replaced to this another MPU6050 and tried, but still rotation occurred.

Duane Degn · 2014-06-17 21:04

These are pretty cool sensors. They're amazingly inexpensive. The last time I purchased the sensor, it was from this seller. There are a lot of sellers on ebay selling these.

The datasheet of these sensors don't provide enough information to take full advantage of its capabilities but the Arduino code takes care of writing all the necessary data to the sensor to get it to work well.

jazzed · 2014-06-17 22:03

Ok,

I've ordered a module.

Kaeru no Ojisan · 2014-06-17 23:15

Dear jazzed,

Again great thanks for your help.

Propeller C code is in my first thread and you can find Processing 1.5.1 code in;
http://www.varesano.net/files/FreeIMU-20121122_1126.zip
\FreeIMU-20121122_1126\processing\FreeIMU_cube

And it might be better to comment out the line "myPort.write("q" + char(burst));"

rjo__ · 2014-06-20 11:44

Kaeru no Ojisan,

Welcome to the forum.

I love your instructables. http://www.instructables.com/tag/type-id/?sort=none&q=kaeru

Jazzed is the flat out expert here, so your patience will be rewarded.

I too have just ordered some boards and am very interested to see where this thread leads.

I should say that I don't use C very much, and don't know much about it. So this is an opportunity for
me to see C applied to a very interesting problem.

I ran your code in SimpleIDE... same version and with updated libraries.
The code compiles and runs for me, but there is a very long list of warnings... many involving serial communications.
And of course, because I have nothing attached to my board the serial port tells me(as expected) that I have 0 bytes when I should have 15:)

My first thought is that I may not have SimpleIDE set up correctly to run your code.

I am using the C compiler with CMM and optimization set to -OS size.

Thanks,

Rich

Kaeru no Ojisan · 2014-06-20 17:27

Dear Rich,

Thank you for your comments.

I am afraid but I have not informed that my code shows many warnings.
I have thought these are not irrelevant to unstable quaternion, but these might affect so.

My C compiler is also with CMM and optimization -Os size.

I am looking forward to jazzed solving this problem.

Thanks, Kaeru no Ojisan

jazzed · 2014-06-22 23:44

Received a shipping notice today.

The GY-521 module should arrive in a week.

Mark_T · 2014-06-23 14:38

I would get it to output the error values and print them, any problem with PID control will show as
error excursions before loss of control I'd have thought - sorry don't have more time to help, it looks
cool and await a working version

rjo__ · 2014-06-27 14:00

Kaeru,

Thank you for posting the code. I went to every other source I could find on the web and couldn't get any of them to compile. Your 60X0_DMP6 compiles and communicates with Processing:)

But in my hands, it fails to communicate with the MPU6050. I am using an uno, latest revision. I have the I2C clock line connected to A5, the Data to A4, The VCC to 3.3v and the interrupt connected to digital pin 2. I know that I have a good chip because there is a Spin object from the OBEX that generates good results. I am using pull-ups on both lines (have tried various values between 4K and 10K.) This looks like an i2c problem, but I don't know what to tweak.

Any thoughts?

Thanks

Rich

Duane Degn · 2014-06-27 14:51

Rich, I've used the MPU6050 on several different Arduino boards. I have the pins connected the same way you do but I don't use any pull-up resistors. The Arduino has internal pull-ups.

I've used the sensor on an Uno and a couple Pro Mini boards. The Uno was a 5V board but the Minis are 3.3V so I don't need to worry about the 5V lines damaging the sensor. I didn't use level shifters when using the sensor with the 5V board.

Kaeru no Ojisan · 2014-06-27 18:04

Rich,

Thank you for tracing everything.
My wiring between MPU6050 and Arduino UNO is as follows;
I don't use any pull-up resistors.
VDD => 3.3V
GND => GND
INT => D2 or not connected
SCL => A5
SDA => A4

It is appreciated if you could try "FreeIMU_quaternion.pde" (Arduino code @ \FreeIMU\examples\FreeIMU_quaternion\) together with "FreeIMU_cube.pde" (Processing code).
Please amend FreeIMU.h as well.
comment out line 34 //#define FREEIMU_v04
uncomment line 42 #define GEN_MPU6050

Kaeru no Ojisan

jazzed · 2014-06-27 18:13

I got my module and began connecting it. The schematic is here: http://playground.arduino.cc/uploads/Main/MPU6050-V1-SCH.jpg

VDD on the GY-521 should be 5VDC. 5V is regulated to 3.3VDC on the board by the little 5 pin chip. Pull up resistors for SCL/SDA are on the board.

Don't use P28/29 of the Propeller for this module because the pull-ups may be too strong.

jazzed · 2014-06-27 22:24

Hi,

I had mixed results with the program attached near the top of the thread. While it was quite easy to destabilize because zeroGyro() was called only once, I never really saw the constant flipping of the cube seen in the video. There were other stability problems where the cube would float around periodically then get rock solid stable. I did have to change "unsigned char devAddr;" to "int devAddr;" for that program to work for me.

Given the mixed results, I thought I would try another approach, and make a SimpleIDE version of the Arduino code (attached here for reference). Using that approach I found the readings to be practically rock solid. The gyro readings were all +/- 2 on 3 axes, but there was some deviations in the accelerometer readings which seems to be common for accelerometers. The temperature readings were fine.

The attached program has USE_SIMPLEIDE defined. To use Arduino code, undef USE_SIMPLEIDE (fingers crossed ... does that still build on Arduino? not sure).

InvenSense MPU-6050
June 2012
WHO_AM_I : 68, error = 0
PWR_MGMT_1 : 0, error = 0


Pause for the cause .... Errors above should be 0.

MPU-6050
Read accel, temp and gyro, error = 0
accel x,y,z: 98, 181, 3814
temperature: 26.903 degrees Celsius
gyro x,y,z : -62, 14, 9,

Kaeru no Ojisan · 2014-06-28 00:29

Jazzed,

Thank you very much for your help.

I changed MPU6050 connecting (e.g. VCC +5V, SCL P0, SDA P1) and changed code "unsigned char" to "int" as well.
However, in Processing screen, quaternion, yaw, pitch and roll value change a lot and plate is rotating in every 3 axis.

But your ArduMpu6050 code runs as fine as you mentioned.

Kindly inform me your Board, Memory Model and Optimization.

Thanks,

Kaeru no Ojisan

jazzed · 2014-06-28 15:51

Hi.

I was able to stabilize the cube by using a better "mircos()" methodology.

That is, in getQ() replace this code:

//now = CNT / (CLKFREQ / 1000000.0);


  static unsigned long _time = 0;
  unsigned long rc = (CNT - _time);
  _time = CNT;
  now = rc/(CLKFREQ/1000000);

The Cube still wanders slowly, but it never flips like in your video. Hope this answers your query.

Also, I was a bit annoyed by the processing thing holding on to the serial port on stop, so I added a 'q' command.

Just press 'q' before hitting the stop button in the GUI, then you can reload with SimpleIDE without pulling the USB port.

void keyPressed() {
  if(key == 'h') {
    println("pressed h");
    
    // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
    hq = quatConjugate(q);
    
  }
  else if(key == 'n') {
    println("pressed n");
    hq = null;
  }
  else if(key == 'q') {
    println("pressed q");
    hq = null;
    myPort.clear();
    myPort.stop();
  }
}

Cheers.

rjo__ · 2014-06-28 19:59

Steve,

Amazing work. I just want to sort of stand back and look at it... such beauty. Everything compiles on my Prop and I get reports in the terminal. I have only tested the Prop side of the equation, I'm using a Propeller Project Board.

When I hook the MPU-6050 up to VIN(USB)... nothing. Won't even talk to serial. If I connect the MPU6050 up to 3.3V... I get raw values in my terminal that look very handsome.

I don't know if I am supposed to be able to talk to Processing from the Prop, but when I try, I get Java null pointer exception type errors... which I don't get when the arduino is talking to Processing. But I also have an RXTX vers mismatch notices with both the Prop and Arduino. I tried to fix this a few days ago... following instructions on the Web, but the end result of my efforts was no change.

I have previously hooked up my Prop and talked to Processing with very little effort, so I'm not sure what this is all about.

Programming is both an art and a science... your seem to have both "down pat."

Rich

I don't actually know Pat... I'm just guessing here:)

jazzed · 2014-06-29 00:37

Rich,

Thanks for your kind remarks. There are many here with great skills.

My GY-521 is connected to Martin's really nice DNA propeller-platform board. The VCC is connected to 5V, GND to GND, and SCL/SDA are on P0/P1. The board type is "GENERIC" or "ACTIVITYBOARD" ... nothing special, just a 5MHz crystal.

The program attached to the post with the video runs on propeller and sends out blobs of hex values (the minor changes I mentioned are included). The processing code runs on the PC and connects to the propeller (or other device) and interprets the blobs of hex values.

I noticed the processing Java errors happening if I forget to close my propeller terminal. ... And with the default processing code, you'll have to unplug the USB for re-programming the propeller.

rjo__ · 2014-06-29 10:23

Steve,

Everything working.

Not only are there great programmers here, but they actually answer questions. When I was originally trying to find code that would compile on the Arduino, I was asking google to find references to some of the minor errors I was finding. At one point, in a major repository forum, I found my exact errors... and the recommendation was to "wait for the next update"... and that was a year old and there were no further updates. A simple error.... I didn't know what to do with it... but you would have:) And I am sure a lot of other people did too... but they didn't respond.

Thanks again

Rich

Kaeru no Ojisan · 2014-06-29 15:47

Jazzed,

Thank you for your advise.

Unfortunately, my cube is still rotating in every 3 axis, even I use your better "mircos()" methodology.

My board is QuickStart with CMM Main RAM Compact and -Os Size Optimization.

Can you advise me what makes difference with you ?

Rich,
Can you also advise me how is my MPU6050_quaternion code with modification by Jazzed ?
Is it stabilized ?

Thanks,

Kaeru no Ojisan

rjo__ · 2014-06-29 16:53

Kaeru,

I'm looking at it now... what I find is that very reliably, there is an increasing error in the reported roll axis this magnitude of error increases until -45degrees then changes what appears to be a steady rate error to around -90 degrees and decelerates to about -154 degrees... the error then reverses values (increasingly heading from a - direction toward zero) but never gets back to 0 roll angle. If I move around an axis, the amount of movement seems to translate wrong... possible from a faulty assignment of radians rather than degrees. I didn't have time to look for it. Sorry.

So... there seems to be a constant that has the wrong value and an equation that has mixed up radians and degrees... but I could very easily be wrong.

In your getValues function you have values = accgyroval / 16.4f;
I would look carefully at that 16.4 value and see if it is in error. I have no idea where it comes from and I haven't played with it's value. I did play with Steve's timing changes. I put a constant correction into the getQ's sampleFreq = 1.0 / ((now - lastUpdate) / 1000000.0); to sampleFreq = 1.0 / ((now - lastUpdate+constant) / 1000000.0);

and changed the value of the constant, but it didn't seem to do anything significant... again I could be wrong. I'm tired and bouncing from issue to issue tonight.

Rich

rjo__ · 2014-06-29 16:56

By the way, I also found that if I bump the sensor even slightly I can reproduce the flip in rotation that is in your original video.

What I would recommend is that if you can't find the problem, then modify your code (I can't:)... we have some really good programmers here... I'm not one of them...
Report out the raw values and then modify the Processing sketch and have it report them to the screen.

If you get what Steve is getting out of his code, then it is a math problem. If not... maybe there is a language issue that still needs to be identified.

Regards,

Rich

rjo__ · 2014-06-29 19:25

I have some personal progress to report with my Arduino/MPU6050 travails.... God I hate to say that:) . I again downloaded the sources from https://github.com/jrowberg/i2cdevlib
and managed to get rid of the error message for the DMP6 example simply by finding the offending line in the .cpp and getting rid of the "user" at the end of it.
I also got rid of and replaced all the libraries. Then I zero'd the gyro settings in the code and wha la, it runs and actually shows good results except for the yaw axis...

Probably because I need to actually estimate the gyro correction factors.

But everything works, the device is identified, initialized, dmp is started, and everything reports out fine. I used all the same connections. I did use the interrupt on 2 and I did use 5V.

The reason I am happy about this is that I was convinced if I could find some working code it would be fairly simple to port it over to Spin/Pasm... given the fact that we already have working code in the OBEX for the 6050... it is just missing the DMP initialization process and appropriate register identifications and settings.

I'm going to bed:)

Kaeru no Ojisan · 2014-07-01 01:23

Jazzed,

I am afraid but I misunderstood your better "mircos()" methodology code.

Now my cube is stabilized

.

Thank you very much for your great advice and help.

My cube also wanders slowly, but this is also occurred with Arduino("FreeIMU_quaternion.pde").

But here is another problem, the cube in Processing screen responds to GY-521 module rotation in Pitch and Roll, but NOT in Yaw.

I checked with Arduino("FreeIMU_quaternion.pde") and the cube responds to all Yaw, Pitch and Roll.

It is appreciated if you could check the cube response to GY-521 rotation and figure out this problem.

Thanks,

Kaeru no Ojisan

MPU6050 Quaternion

Comments