Support for MPU6050

amourdurisk · Post by **amourdurisk** » Wed Feb 29, 2012 5:05 pm

fax8 wrote:Our initial tests showed that the MPU6050 is quite more stable than the ITG3200.. does the readings seem ok in the GUI??

Hi fax8,

All the readings seems ok in the GUI.
it seems to be more sensitive to vibrations than the ITG.
I feel that the LPF is not effective on the MPU.
When I turn off the LPF on the ITG I have the same problem, the quad will not take off.
LPF42hz with the ITG, the quad flies perfectly.

jstapels · Post by **jstapels** » Sun Mar 04, 2012 8:56 pm

Hi, I'm brand new to the world of quadcopters and currently in the research phase. My initial plan was to use Fabio's wonderful FreeIMU design along with an Arduino mini pro as the base for my first copter. However, I'm having a really hard time finding a place to buy the pressure sensor and they still haven't responded to any of my emails. Anyways, my question is, do I really need a pressure sensor if I'm going to use the MultiWii software?

It seems most builds don't use them and to my noob eyes, I would think the accelerometers would be used to help keep the vertical copter stable vertically. Any help would be greatly appreciated.

Alexinparis · Post by **Alexinparis** » Sun Mar 04, 2012 11:23 pm

Hi,

FreeIMU are again available since few days on flyduino or viacopter web shop.
A pressure sensor is not mandatory for multiwii.

jstapels · Post by **jstapels** » Mon Mar 05, 2012 1:37 am

Alexinparis wrote:Hi,

FreeIMU are again available since few days on flyduino or viacopter web shop.
A pressure sensor is not mandatory for multiwii.

Thanks for the quick response. I was aware that the production boards were available, but I was hoping to save a few bucks by building it myself.

frank26080115 · Post by **frank26080115** » Tue Mar 06, 2012 5:29 am

Hi, I am Frank, I am the creator of the Picopter project

I have DMP almost working, I've almost finished adapting Noah Zerkin's code to use MultiWii's I2C routines, I am in the testing phase

I have the Picopter output the data via serial port. I have quaternion data.

The problem is I have no idea how to process it into roll and pitch angles.

Sitting still on my desk, the quaternion data are 30252 23548 23293 12242, these are 32 bit signed integers
Moving it only slightly turned them into -7376 -8963 -19205 -1322
The documentation says that these numbers are scaled by 2^30
so scaling by that gave me 0.0000 something that doesn't get printed, also the rotation matrix calculation were too low as a consequence
But through experimentation, the quaternion numbers do not ever exceed 32768, so I scaled by that instead
now the numbers are sane, quaternion data in double type is now between +/- 1, rotation matrix is also between +/- 1
I feed a few of the rotation matrix entries through to _atan2 to get roll and pitch angles, but now the numbers don't make sense

Here's the funny thing: I have almost no linear algebra education beyond finding determinants. This code was written by looking at the MotionApp code provided by InvenSens.

So... who wants to help me? I'll give you my test sketch, it should work on any platform as long as it has I2C and UART
I just need somebody to fix the mathematics in the "dmp_processQuat" function

Here is the sketch

DmpTest.zip: (8.15 KiB) Downloaded 3088 times

Note, I'm not using arrays as an optimization, all the postfix numbers are indexed starting from 0, if you believe I should switch to using arrays, tell me so

I don't have any "calibration" routine yet, I have plans on writing them, but I was hoping I can start off by getting the angles to work.

edit: actually, the stuff I copied from MotionApp does not match what I see from http://en.wikipedia.org/wiki/Conversion ... ler_angles at all... err I'm sleepy, I'll try again on Wednesday night

if you have the MPU-60X0, please mess around with it

edit: I need a good arcsin function

fax8 · Post by **fax8** » Tue Mar 06, 2012 11:20 pm

@frank See function sendquat in the Arduino code you find at http://www.varesano.net/blog/fabio/init ... nd-mpu6050

@amourdurisk I'll try investigating your problem.

@jstapels nice you are into building a FreeIMU.. post a picture on my website once your done ;-)

frank26080115 · Post by **frank26080115** » Wed Mar 07, 2012 1:01 am

sendQuat doesn't do any translation into Euler angles

sendQuat also doesn't agree with my endianness, I get steady values with little-endian and it looks like sendQuat uses big-endian. Maybe what appears to be steady value is wrong and I've screwed up. I'll do more checks on Wednesday

I'm working on approximating arcsine with y = x + x*x*x/6, that should be accurate enough, right?

edit: also working on calibration, but the names for registers 0x00 to 0x18 in the header file i'm using does not match what is documented in the register map documentation, in fact, register 0x02 to 0x18 are all missing from the documentation, and 0x01 is some sort of AUX_VDDIO instead of MPU_RA_YG_OFFS_TC

fax8 · Post by **fax8** » Wed Mar 07, 2012 1:26 am

frank26080115 wrote:sendQuat doesn't do any translation into Euler angles

It does convert the fixed point quaternion into a float quaternion. It then streams the floats to the serial port into the Processing code (also on the webpage). That's where you find the Euler conversion.. see quaternionToEuler function.

frank26080115 · Post by **frank26080115** » Wed Mar 07, 2012 1:31 am

I see it, it is disagreeing with what I'm learning from Wikipedia, I'll just try various equations until something seems right I guess

ciskje · Post by **ciskje** » Wed Mar 07, 2012 10:23 pm

if you have q0 q1 q2 q3 (you must normalize it) and want to convert to roll pitch yaw:

// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

y=atan2(2*q1*q2+2*q0*q3,2*q0*q0+2*q1*q1-1);
p=-asin(2 * q1 * q3 - 2 * q0 * q2); // theta
r=atan2(2 * q2 * q3 + 2 * q0 * q1, 2 * q0 * q0 + 2 * q3 * q3 - 1); // phi

frank26080115 · Post by **frank26080115** » Thu Mar 08, 2012 5:16 am

got it working, my _asin approximation is weird at too great of an angle so it probably won't perform aerobatic stunts, it's only using a two term taylor series

the final code looks like

Code: Select all

  int32_t quatl0, quatl1, quatl2, quatl3;
  double quat0, quat1, quat2, quat3;
  
  #define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
  
  quatl0 = dmp_quatTake32(dmp_received_packet, 0);
  quatl1 = dmp_quatTake32(dmp_received_packet, 1);
  quatl2 = dmp_quatTake32(dmp_received_packet, 2);
  quatl3 = dmp_quatTake32(dmp_received_packet, 3);
  
  if (quatl0 > 32767) quatl0 -= 65536;
  if (quatl1 > 32767) quatl1 -= 65536;
  if (quatl2 > 32767) quatl2 -= 65536;
  if (quatl3 > 32767) quatl3 -= 65536;
  
  quat0 = ((double) quatl0) / 16384.0d;
  quat1 = ((double) quatl1) / 16384.0d;
  quat2 = ((double) quatl2) / 16384.0d;
  quat3 = ((double) quatl3) / 16384.0d;

  angle[ROLL] = _atan2(2*((quat0*quat1) + (quat2*quat3)), 1.0d - (2*((quat1*quat1) + (quat2*quat2))));
  angle[PITCH] = _arcsin(2*((quat0*quat2) - (quat3*quat1)));
  
  gyroADC[YAW] = ((dmp_received_packet[4*6+0]<<8) | (dmp_received_packet[4*6+1]<<0));

output sitting flat on my desk

Code: Select all

Quat  14780=0.90  -331=-0.02  -300=-0.02  -7056=-0.43 Final -11 -28 -1
Quat  14780=0.90  -316=-0.02  -295=-0.02  -7056=-0.43 Final -11 -28 -1
Quat  14780=0.90  -334=-0.02  -303=-0.02  -7056=-0.43 Final -11 -29 -1
Quat  14780=0.90  -326=-0.02  -296=-0.02  -7055=-0.43 Final -11 -28 -1
Quat  14780=0.90  -314=-0.02  -306=-0.02  -7056=-0.43 Final -10 -28 -1
Quat  14780=0.90  -324=-0.02  -306=-0.02  -7055=-0.43 Final -11 -29 -1
Quat  14780=0.90  -333=-0.02  -303=-0.02  -7056=-0.43 Final -11 -29 -1

angles are in degrees, scaled up by 10

now to work it into the MultiWii code... and to work on calibration

how should calibration be handled?

should it be handled internally by the MPU-60X0 itself? I mean we zero all the offset registers, take average readings, and set the offset registers to our average?

or should we simply take an average of the current DMP output as an offset, and let our AVR calculate the actual angle using the current reading subtracting the offset?

or do both?

timecop · Post by **timecop** » Thu Mar 08, 2012 12:40 pm

oh, great.
now I can use this code on a real processor (stm32) and not use silly approximations of math functions.

Hamburger · Post by **Hamburger** » Thu Mar 08, 2012 12:49 pm

dongs wrote:now I can use this code on a real processor (stm32) and not use silly approximations of math functions.

does it matter? 'Silly' approximations is what digital computers are destined to do. The math libs and even hardware implementations must use approximations as well. So the question would really be about chosen speed and algorithm; assuming correctness and resolution of implementations.

From past experience with cycle times I would be most concerned about speed and that is where a faster cpu (and hardware implementation) do help indeed.

frank26080115 · Post by **frank26080115** » Thu Mar 08, 2012 5:16 pm

i was under the impression that computers must use approximations, it's just that you can afford to use more taylor series terms in the same given amount of time.

I also have a STM32F4 based design http://aeroquad.com/showthread.php?4215 ... bit-design

I'm not working on it right now, no time, but maybe we can collaborate later?

I'll do a cycle speed measurement soon on the AVR. Meanwhile I hope somebody can advise me on how to do calibration, see my post above.

timecop · Post by **timecop** » Thu Mar 08, 2012 5:21 pm

I'm not working on it right now, no time, but maybe we can collaborate later?
I don’t want to deal with the “Arduino” or “Maple” stuff. I'm also very strict with my own naming and coding conventions.

LOL, awesome.

frank26080115 · Post by **frank26080115** » Fri Mar 09, 2012 1:10 am

DMP test code is running 6603 loops per second, each loop checks if the FIFO contains data

18 out of 6603 times, FIFO does contains data

18 out of those 18 times above, the data is the quaternion data that we are looking for

That's an update rate of only 18 Hz, and the bottleneck is not I2C or serial port debug or internal AVR calculations, as far as I can tell

I can pick 20, 40, 50, 100, or 200 Hz, what do you think? I think 200 is possible from the 6603 figure.

Edit: 200 Hz seems to crash the code, 100 Hz got 5847 loops per second and 100 updates per second

timecop · Post by **timecop** » Fri Mar 16, 2012 3:56 pm

Do you have latest/updated version of dmptest with your fixes?
I just found out MPU6xxx fits into the MPU3050 pinout and can be used, so I now finally have an excuse to try using it on STM32.

frank26080115 · Post by **frank26080115** » Fri Mar 16, 2012 6:09 pm

http://minus.com/mzEyvZjWG#1

timecop · Post by **timecop** » Sat Mar 17, 2012 12:35 am

Thank you sir

jphelirc · Post by **jphelirc** » Tue Mar 20, 2012 5:29 am

It's great to see how MultiWii has evolved so time to give something to it again

Reason why ArduCopter project has not published anything new on DMP stuff is that there are some legal issues from manufacturer that people are trying to solve. In meanwhile project cannot use their digital blob so no DMP officially. Hopefully that will change soon.

Anyways, few my friends have asked from me MultiWii compatible board with all included. So we are working on one now. My question goes do you all want to see MPU-6050 on it or BMA180/ITG3205.

Plans for board are:
- ATmega2560
- ATMega8u2
- Onboard Switching regulator 5V/2A and 3.3v
- MSS5611 pressure sensor (thanks to fabio and roberto)
- HMC5883 compass
- Gyro/Acc packet (either ITG3205/BMA180 or MPU-6050)
- Expansion ports
- Info leds
and other usual stuff.

Why At2560, well it's fully supported but Arduino IDE and it's easily available. STM would be nice too but toolchains are not good for that yet and especially if thinking common users. Arduino is moving towards to ARM platform but who knows when and how. On many projects like MultiWii, AeroQuad, ArduCopter it's been proven that ATMega chips are well enough for doing things especially if software it well written.

Let me know what you all think and would it be reasonable to do it.

--jp

frank26080115 · Post by **frank26080115** » Tue Mar 20, 2012 6:28 am

why not MPU-6050 plus BMA180? if the problem is that the MPU-6050 "is not software ready", then you can use it as a gyroscope only and then use BMA180. It's supposed to be completely compatible with the old ITG sensor anyways. When the DMP is ready, a simple firmware change will be required to use it.

timecop · Post by **timecop** » Tue Mar 20, 2012 10:30 am

jphelirc wrote:It's great to see how MultiWii has evolved so time to give something to it again

Reason why ArduCopter project has not published anything new on DMP stuff is that there are some legal issues from manufacturer that people are trying to solve. In meanwhile project cannot use their digital blob so no DMP officially. Hopefully that will change soon.

Anyways, few my friends have asked from me MultiWii compatible board with all included. So we are working on one now. My question goes do you all want to see MPU-6050 on it or BMA180/ITG3205.

Plans for board are:
- ATmega2560
- ATMega8u2
- Onboard Switching regulator 5V/2A and 3.3v
- MSS5611 pressure sensor (thanks to fabio and roberto)
- HMC5883 compass
- Gyro/Acc packet (either ITG3205/BMA180 or MPU-6050)
- Expansion ports
- Info leds
and other usual stuff.

Why At2560, well it's fully supported but Arduino IDE and it's easily available. STM would be nice too but toolchains are not good for that yet and especially if thinking common users. Arduino is moving towards to ARM platform but who knows when and how. On many projects like MultiWii, AeroQuad, ArduCopter it's been proven that ATMega chips are well enough for doing things especially if software it well written.

Let me know what you all think and would it be reasonable to do it.

--jp

unless it's going to be given away for free you're wasting your time with obsolete 8bit garbage.
it has already been proven anyone can throw a bunch of cheap sensors on a pcb and declare victory.
I suggest focusing on actually writing that "well written" software, as none of the choices mentioned above (except multiwiI) come even close to being "well written". But then again, it's more of platform limitation than software's fault.

Using the "its easy for morons" excuse doesn't work either - nobody will write code for you for free, no matter how much hardware you give away. But if someone is somewhat interested but has to deal with 8bit trash like counting CPU cycles to make sure servo PWM output doesn't jitter, they lose interest quick. So this failmega2560 design is destined to run same old crap, provide zero innovation, and doesn't solve any current problems. Why bother?

I'd be curious to hear what reasoning you could possibly have for having 'yet another worthless 8bit pcba' thrown out there in Q2 2012?

p.s. accel in MPU6k works fine, why would anyone need another accel outside?

ronco · Post by **ronco** » Tue Mar 20, 2012 10:27 pm

Hi,

i saw the MPU-6150 (http://www.lipoly.de/modellbau/elektron ... -6100A.pdf) someone knows the difference to 6050?

regards

felix

frank26080115 · Post by **frank26080115** » Tue Mar 20, 2012 11:57 pm

comparing the datasheets, the noise performance of the two are different

61x0 says 0.2 deg/s-RMS
60x0 says 0.05 deg/s-RMS

low frequency noise and spectral density are not given for 61x0 but are given for 60x0

ronco · Post by **ronco** » Wed Mar 21, 2012 11:27 am

so the 60x0 is better then 61x0?

i dont know much about sensors and how the MWC reads it .. but as far as i know mwc filters the sensor data.. do you think there will be a noticable difference?

i ask because the 61x0 is cheaper then the 60x0

regards felix

frank26080115 · Post by **frank26080115** » Sat Mar 24, 2012 5:52 am

sorry i can't comment on whether or not there will be any noticeable differences

back on the progress on the DMP...

http://www.youtube.com/watch?v=YbdN_C0oreI

Hamburger · Post by **Hamburger** » Sat Mar 24, 2012 2:17 pm

looking good.
When will you integrate your code into current MWii code base?

timecop · Post by **timecop** » Sat Mar 24, 2012 2:20 pm

Indeed, looks nice.
How does dmp (quaternions? I assume) handle angles near 90/180/etc degrees? Do you have a video where you rotate the model fully 360" around one axis (pitch/roll)

Hmm.
Got the code moved to my system.
Should this (theoretically) work with MPU6000 on i2c?
Or would setup be different?

I'm not getting any useful data in fifo (none actually, blah).

frank26080115 · Post by **frank26080115** » Sat Mar 24, 2012 6:14 pm

dongs wrote:Indeed, looks nice.
How does dmp (quaternions? I assume) handle angles near 90/180/etc degrees? Do you have a video where you rotate the model fully 360" around one axis (pitch/roll).

It doesn't on the pitch axis, it won't be accurate beyond 60 degrees right now, only because of my own arcsin approximation

this is already using I2C, without using the interrupt pin. I'm not sure what you are asking

it really helps to double check your initialization. check that you are getting ACK when you are supposed to so you know it's at least replying, make sure the I2C does not become disabled

edit: I don't understand too much about quaternions to provide a better answer, I'm sure the raw data itself is very accurate

timecop · Post by **timecop** » Sun Mar 25, 2012 1:13 am

What I meant is i don't have any MPU6050 chips, so I'm using MPU6000 (SPI version) connected as I2C. It does respond, upload data, etc, I can read raw gyro values from it after doing all the DMP initialization, but I can't get any useful data in fifo.
So I thought if initial setup between 6k and 6050 would be different as in register contents and stuff.

frank26080115 · Post by **frank26080115** » Sun Mar 25, 2012 1:55 am

The FIFO structure is already well defined. Are you getting "FIFO is ready"? Is the FIFO all zeros? Does the FIFO not match what you expect? Does it seem random? Noisy? Double check your endianness?

The registers are mostly the same, I think there are some differences in the internal DMP memory (not the same as registers) between silicon revisions according to the "Motion Processing" code that InvenSense provided. I don't even know how to tell which silicon revision I have but I think you can guess based on the date of manufacturing, I'm pretty sure I'm using the latest.

By the way, the roll axis uses arctan, it goes between -180 and 180 degrees just fine. My pitch axis will only go between -60 and 60 due to the arcsine approximation error.

timecop · Post by **timecop** » Sun Mar 25, 2012 6:30 am

No, I'm not getting fifo data at all (actually one read gets fifo level of like 4, then nothing else after).
I'll look over it again, initialization is fine, they're all standard i2c buffer writes as you can see here
http://code.google.com/p/afrodevices/so ... _mpu6050.c
(under #ifdef MPU6050_DMP)
Unless I messed up pointers to 3d array when I got rid of dumb for loops, I'll look over it again but I think its fine.

frank26080115 · Post by **frank26080115** » Sun Mar 25, 2012 5:55 pm

hmm... personally I would hook the I2C bus to an analyser right now and log the traffic for a second.

timecop · Post by **timecop** » Mon Mar 26, 2012 3:54 am

Yea, that was my next plan. I'm so lazy...

didlawowo69 · Post by **didlawowo69** » Mon Mar 26, 2012 9:05 am

Hi,
i have a question for MPU6050 user's.
do you think this IMU on freeIMU 0.4v is better than an allinone board like sirius from multiwiicopter.com ?

cause i'm going to buy a new IMU for my quad, and have no idea how to compare this both.

thx in advance.

amourdurisk · Post by **amourdurisk** » Mon Mar 26, 2012 5:02 pm

Hi didlawowo69,

I test ITG3200+bma180 and MPU6050, in my case, I find that itg3200 is less sensitive to vibration so I had better results with itg
Would require other users MPU say what they think

Matt.

amourdurisk · Post by **amourdurisk** » Mon Mar 26, 2012 5:47 pm

i have found someone like me who think that the LPF with MPU has no effect
Here http://www.rcgroups.com/forums/showpost.php?p=20902329&postcount=2859

someone else has the same problem?

Matt.

timecop · Post by **timecop** » Mon Mar 26, 2012 6:00 pm

No man, it works fine for me. But I don't use DJI frames.

amourdurisk · Post by **amourdurisk** » Mon Mar 26, 2012 7:41 pm

which lpf do you use dongs ?

timecop · Post by **timecop** » Tue Mar 27, 2012 1:50 am

none, (default) of 256hz

amourdurisk · Post by **amourdurisk** » Tue Mar 27, 2012 7:53 am

If you don't use it how you can know if LPF is working or not for you ?

timecop · Post by **timecop** » Mon Apr 02, 2012 11:53 am

OK, I poked around with DMP code again.
dmpFifoReady() always reads 0x04, 0x00 -> fifo of 1024.
So.. It's always full?...
edit: Got it.
I had a 20ms delay after init, which would immediately saturate the fifo.
since the code never reset it again, it would just stay full.
Now im getting data in dmp_received_packet, time to play with it.
edit2: i'm getting quat data at like ~20Hz.
Is that the best this thing can do?

edit3: all working. still at 20Hz though.
I'm turning on a led each time a successful entry into dmploop w/available data and turning it off on exit.
this takes 1.5ms on STM32/72MHz (read data + calculate), then the rest is blank of ~55ms until next cycle.

frank26080115 · Post by **frank26080115** » Mon Apr 02, 2012 3:37 pm

one of the settings determine the FIFO output rate, look around for "inv_set_fifo_rate" in the code comments and the settings are given in the documentation for "Embedded MotionApps Platform Functional Specification"

timecop · Post by **timecop** » Mon Apr 02, 2012 3:44 pm

Cool, thanks

it's running at 200Hz now.

didlawowo69 · Post by **didlawowo69** » Tue May 22, 2012 3:48 pm

any news from MPU6050 support,
could you tell me if that works correctly, because my quad don't work fine :/

fax8 · Post by **fax8** » Tue May 22, 2012 9:02 pm

working good.. you may wanna try setting some DLPF. Some frames needs it to avoid some kind of resonance problems.

didlawowo69 · Post by **didlawowo69** » Wed May 23, 2012 7:59 am

could you explain how to setup DLPF ?

Arf · Post by **Arf** » Wed May 23, 2012 8:51 am

didlawowo69 wrote:could you explain how to setup DLPF ?

There are some MPU-6050 LPF defines for the gyro in config.h perhaps that what he means?

Specifically:

//#define MPU6050_LPF_256HZ // This is the default setting, no need to uncomment, just for reference
//#define MPU6050_LPF_188HZ
//#define MPU6050_LPF_98HZ
//#define MPU6050_LPF_42HZ
//#define MPU6050_LPF_20HZ
//#define MPU6050_LPF_10HZ // Use this only in extreme cases, rather change motors and/or props

didlawowo69 · Post by **didlawowo69** » Wed May 23, 2012 10:17 am

thx for this answer
could you tell me whiche config.h do you use for MPU > I2C address

fax8 · Post by **fax8** » Wed May 23, 2012 10:33 am

didlawowo69 wrote:could you tell me whiche config.h do you use for MPU > I2C address

Please clearify your question.