Support for MPU6050
Support for MPU6050
Hi everyone,
I finally started writing some code for supporting the MPU6050 gyro+accelerometer in MultiWii. I'm currently in the final stages of FreeIMU v0.4 testing and with the great help of Jussi and his viacopter.eu we should start the production stage of v0.4 soon. Meanwhile, I've sent some prototypes to Alexinparis, warthox, Paul and other people so that hopefully we will have soon enough people willing to help coding/testing for the new MPU6050.
Ideally, this post will be used to coordinate efforts in MPU6050 development, so if you are using this sensor or if your are developing for it, please keep an eye on this post!
I now share here my current status. Patch against today SVN available here: http://sprunge.us/EHAL
This basic support for raw readings from the gyroscope and the accelerometer while the magnetometer is read using the I2C bypass features. There are still some minor issues, but I'm sure a more MultiWii expert developer will be able to fix them in minutes. Code is heavily commented with TODO and personal comments and WTF. NOT FLY READY.
Thanks,
Fabio Varesano
I finally started writing some code for supporting the MPU6050 gyro+accelerometer in MultiWii. I'm currently in the final stages of FreeIMU v0.4 testing and with the great help of Jussi and his viacopter.eu we should start the production stage of v0.4 soon. Meanwhile, I've sent some prototypes to Alexinparis, warthox, Paul and other people so that hopefully we will have soon enough people willing to help coding/testing for the new MPU6050.
Ideally, this post will be used to coordinate efforts in MPU6050 development, so if you are using this sensor or if your are developing for it, please keep an eye on this post!
I now share here my current status. Patch against today SVN available here: http://sprunge.us/EHAL
This basic support for raw readings from the gyroscope and the accelerometer while the magnetometer is read using the I2C bypass features. There are still some minor issues, but I'm sure a more MultiWii expert developer will be able to fix them in minutes. Code is heavily commented with TODO and personal comments and WTF. NOT FLY READY.
Thanks,
Fabio Varesano
Re: Support for MPU6050
Hi !
I ordered the IMU6050 eval board for atmel avr UC3, it has the same form factor as the other UC3 sensor boards (ATAVRSBIN), and contains an IMU6050 plus an AK magnetometer (have place for HMC5883L), for 54usd.
It's not a big deal when used as gyro+acc for raw reading.... however the integrated DMP is able to do the sensor fusion, and outputs already calculated quaternions which can be translated to DCM within a few simple steps... It's not documented well, and most of the information is coming from the reverse engineering of Invensense dev kit. However I found a commit in the Arduplane repsository which implements DMP code... http://code.google.com/p/ardupilot-mega ... cf80f01ca4 (It have been removed from the head recently)
I'll try to implement it in MultiWii once the module is arrived and have time to dig into it....
I ordered the IMU6050 eval board for atmel avr UC3, it has the same form factor as the other UC3 sensor boards (ATAVRSBIN), and contains an IMU6050 plus an AK magnetometer (have place for HMC5883L), for 54usd.
It's not a big deal when used as gyro+acc for raw reading.... however the integrated DMP is able to do the sensor fusion, and outputs already calculated quaternions which can be translated to DCM within a few simple steps... It's not documented well, and most of the information is coming from the reverse engineering of Invensense dev kit. However I found a commit in the Arduplane repsository which implements DMP code... http://code.google.com/p/ardupilot-mega ... cf80f01ca4 (It have been removed from the head recently)
I'll try to implement it in MultiWii once the module is arrived and have time to dig into it....
Re: Support for MPU6050
Thanks for your contribution. Of course, I do know what the DMP is supposed to do but I do also know how poorly it is documented (basically, no documentation at all).
Unfortunately, the code from arducopter doesn't seem to add anything new to what I was already using 2 months ago.. that is reverse engineered code, with a very big part which is a pure binary blob which nobody knows what it does. I don't think this should be the way to correctly implement it. Also, note that it only uses gyro and acc.. no magnetometer used.
My implementation plan was to start simple, by only providing basic raw readings of gyro, acc and slave magnetometer. A second implementation enhancement would start making use of the I2C master features of the MPU6050 so that the magnetometer is no more accessed by the ATMEGA but instead it's read by the MPU6050 directly. This would go in pair with making usage of the internal FIFO. Some docs on registers supposed to allow slave sensors reading is available on document RM-MPU-6000A.pdf but that's quite hard to decipher.
I think that eventually good documentation for the DMP will be released. Implementing it by reverse engineering and blobs is IMHO just a waste of time... Of course I'll be happy to be proven wrong.
Fabio
Unfortunately, the code from arducopter doesn't seem to add anything new to what I was already using 2 months ago.. that is reverse engineered code, with a very big part which is a pure binary blob which nobody knows what it does. I don't think this should be the way to correctly implement it. Also, note that it only uses gyro and acc.. no magnetometer used.
My implementation plan was to start simple, by only providing basic raw readings of gyro, acc and slave magnetometer. A second implementation enhancement would start making use of the I2C master features of the MPU6050 so that the magnetometer is no more accessed by the ATMEGA but instead it's read by the MPU6050 directly. This would go in pair with making usage of the internal FIFO. Some docs on registers supposed to allow slave sensors reading is available on document RM-MPU-6000A.pdf but that's quite hard to decipher.
I think that eventually good documentation for the DMP will be released. Implementing it by reverse engineering and blobs is IMHO just a waste of time... Of course I'll be happy to be proven wrong.
Fabio
-
- Posts: 2261
- Joined: Sat Feb 19, 2011 8:30 pm
Re: Support for MPU6050
Awesome and Thank you for the fantastic job!
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
Thanks a lot fax8
i make the modif but after a CALIB_ACC Z axis goes crazy.
do you have the same mistake ?
Matt.
i make the modif but after a CALIB_ACC Z axis goes crazy.
do you have the same mistake ?
Matt.
Re: Support for MPU6050
+ // TODO: use I2C master capabilities of the MPU6050
+ // Fabio: almost impossible with current lack of documentation.
Documentation was all there. Was just a matter of reading it.
Example of usage:
http://code.google.com/p/afrodevices/so ... fro.c#2464
+ // Fabio: almost impossible with current lack of documentation.
Documentation was all there. Was just a matter of reading it.
Example of usage:
http://code.google.com/p/afrodevices/so ... fro.c#2464
Re: Support for MPU6050
@dongs that's great stuff! I'm sure you had a good time, uh? A couple of thoughts on your code, if I may:
- Renaming MPU6000_getSixRawADC() to something more appropriate. You are actually reading 20 raw regs there, not 6.
- You are calling MPU6000_getSixRawADC() both in ACC_getADC() and Gyro_getADC(). Can't you just cache the result of the first one executed and cache the values for the second one? Looks like you are doing something like that for the magn already.
Re: Support for MPU6050
@amourdurisk yep.. I'm also seeing that.. let me dig into that.
Re: Support for MPU6050
fax8 wrote:@dongs that's great stuff! I'm sure you had a good time, uh? A couple of thoughts on your code, if I may:
- Renaming MPU6000_getSixRawADC() to something more appropriate. You are actually reading 20 raw regs there, not 6.
- You are calling MPU6000_getSixRawADC() both in ACC_getADC() and Gyro_getADC(). Can't you just cache the result of the first one executed and cache the values for the second one? Looks like you are doing something like that for the magn already.
Ya. I kept the naming for hysterical reasons mostly. Should rename eventually.
About calling twice, I thought, well, getting 'newest' data is better but in the context of 8bit micros it probably doesn't matter at all. I should indeed cache it and just read it once.
I've had all the code working for a while (took a bit to decipher slave stuff), but what I forgot was the 'enable i2c master' bit in power register. Slaves 0-3 = repeatedly polled stuff you can use to query up to 4 external sensors, slave4 = committed realtime, can be used to configure any external sensors (the data is sent immediately, whereas slave0..3 wait until next read cycle).
Re: Support for MPU6050
Any experience using the FIFO?
Re: Support for MPU6050
Nah, with the excuse that it would be fairly useless for single-shot samples without filtering in software.
What do you expect to get out of FIFO once its enabled?
What do you expect to get out of FIFO once its enabled?
Re: Support for MPU6050
Right, I did that in an older version of my code and in my non-multiwii attempt at rate/accel flyingthing on STM8, however it just comes down to where you're going to average the data - on the accel/gyro using built in LPF or on CPU, wasting processor time. I ended up just turning down sampling rate on ADXL345 and letting it do digital lpf instead of grabbing 7-12 samples and averaging.
-
- Posts: 1630
- Joined: Wed Jan 19, 2011 9:07 pm
Re: Support for MPU6050
Hi Fabio,
I managed to integrate an initial working implementation (see the last google code rev) with the use of the I2C master mode on AUX port.
The MAG is initially configured in bypass mode to allow its direct configuration like before.
The I2C port is then configured in master mode to retrieve data coming from the MAG.
I will reply to your code comment latter, it's late
I managed to integrate an initial working implementation (see the last google code rev) with the use of the I2C master mode on AUX port.
The MAG is initially configured in bypass mode to allow its direct configuration like before.
The I2C port is then configured in master mode to retrieve data coming from the MAG.
I will reply to your code comment latter, it's late
Re: Support for MPU6050
Great work Alex! Thanks! Regarding your comment:
See http://forum.sparkfun.com/viewtopic.php?f=14&t=30624
I've seen you are doing 3 calls to i2c_getSixRawADC .. wouldn't it be better to access only once per cycle and cache the results?
note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
See http://forum.sparkfun.com/viewtopic.php?f=14&t=30624
I've seen you are doing 3 calls to i2c_getSixRawADC .. wouldn't it be better to access only once per cycle and cache the results?
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
Thx alex,
but which version do you use ?
with the dev_20111220, nothing happen when i push Start button. Any idea ?
Matt.
but which version do you use ?
with the dev_20111220, nothing happen when i push Start button. Any idea ?
Matt.
Re: Support for MPU6050
dev_20111220 means that the version has been packaged on the 20th December 2011. As the code we have been working on is a couple days old you will find it on the next developer release.
If you wanna try the new software, you can get it from SVN as explained in http://code.google.com/p/multiwii/source/checkout otherwise, just get in touch with me and I'll give you code package.
If you wanna try the new software, you can get it from SVN as explained in http://code.google.com/p/multiwii/source/checkout otherwise, just get in touch with me and I'll give you code package.
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
hi fax8,
thank you for your help
I took files in this directory : http://code.google.com/p/multiwii/source/browse/#svn%2Ftrunk%2FMultiWii but i don't know where i can find the multiwiiconf.app for mac ?
otherwise i will wait for your package
thank you for your help
I took files in this directory : http://code.google.com/p/multiwii/source/browse/#svn%2Ftrunk%2FMultiWii but i don't know where i can find the multiwiiconf.app for mac ?
otherwise i will wait for your package
Re: Support for MPU6050
Then you are good to go for Arduino. Regarding the UI, you find the source here: http://code.google.com/p/multiwii/sourc ... iiConf.pde .. In order to execute that you will need to have Processing installed, see http://processing.org/download/
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
Yeahh, thanks again fax8, now it's ok
Do you know where i can find MPU6050 ? i took mine at www.drotek.fr but it's very expensive.
regards,
Matt.
Do you know where i can find MPU6050 ? i took mine at www.drotek.fr but it's very expensive.
regards,
Matt.
Last edited by amourdurisk on Sun Jan 08, 2012 12:41 am, edited 1 time in total.
-
- Posts: 1630
- Joined: Wed Jan 19, 2011 9:07 pm
Re: Support for MPU6050
Hi Fabio,
Here is some comments on your original code:
I suppose it's something to update the register only in case of sync received signal. It should be usefull to ensure the data are coherent regarding the time they were computed.
You can see the code from dongs where the all the I2C command to the MAG are done via the I2C master. (no other solution since he uses the MPU6000 in SPI mode, no way to use an I2C bypass mode)
I choose a mid-way, where the init commands are done via direct access (bypass mode), and where the 6 bytes data are retrieved via the master mode.
For ACC: I'm now sure of one thing: it should not be in 2G mode in order to avoid vibration saturation, 8G seems to be good.
For GYRO: I think we can use something between 500 and 2000deg/s. 2000 is proven to work fine. 500 is proven to work on analogical gyros
I did this to dynamically change the speed of I2C bus. For instance 100kHz for a genuine WMP and 400kHz for an ACC.
The speed mentioned in config.h applies only for WMP or Nunchuk. So not relevant here.
multiwii is designed only for 16MHz arduino.
Some divisions occur directly in sensor part, because we consider any other LSB are noise when the multicopter is running (empirical). There is also another advantage: we can keep some 16bits computation for addition/subtraction because the full 16 bits range is not used. (important to keep good performance on a 8bit processor)
It's handled and documented in IMU part
For GYRO, it's important to have to freshest data => at least for gyro, I'm not convinced about the fifo interrest. Maybe for ACC or MAG.
A full multiwii loop works this way for each cycle:
- 2 set of gyro data are retrieved (2x6 bytes)
- 1 set of acc data is retrieved ( 1x6 bytes)
- mag data are retrieved only at a 10Hz pulse
- for baro data, it's more complicated because we must handle pressure and temperature request/get, but the retrieval rate is also slow.
I did some tests to gather I2C requests and minimize I2C bus usage, but it wasn't significantly more efficient than requesting data separately.
One explanation: if we want to request acc+gyro+mag, we must request 20 bytes 6+6+6+2. +2 because there is the temperature information (16 bits) between acc and gyro. We do not need this info but we must retrieve it as soon as we want to retrieve acc+gyro data via a single request.
Here is some comments on your original code:
+ // Fabio: needs more info on what's the purpouse of FSYNC. Current docs on it simply sucks.
I suppose it's something to update the register only in case of sync received signal. It should be usefull to ensure the data are coherent regarding the time they were computed.
+ // TODO: use I2C master capabilities of the MPU6050
You can see the code from dongs where the all the I2C command to the MAG are done via the I2C master. (no other solution since he uses the MPU6000 in SPI mode, no way to use an I2C bypass mode)
I choose a mid-way, where the init commands are done via direct access (bypass mode), and where the 6 bytes data are retrieved via the master mode.
// Fabio: tests need to find best Full Scale settings for a Multicopter.
For ACC: I'm now sure of one thing: it should not be in 2G mode in order to avoid vibration saturation, 8G seems to be good.
For GYRO: I think we can use something between 500 and 2000deg/s. 2000 is proven to work fine. 500 is proven to work on analogical gyros
// Fabio: is it really good practice to override I2C speed set by the user in config.h? I'd say no.
I did this to dynamically change the speed of I2C bus. For instance 100kHz for a genuine WMP and 400kHz for an ACC.
The speed mentioned in config.h applies only for WMP or Nunchuk. So not relevant here.
// Fabio: TWBR computation above will fail if an 8Mhz Arduino is used. Bug or by design? See twi.c in Arduino distribution for cross freq compatible code.
multiwii is designed only for 16MHz arduino.
// Fabio: what's the point of dividing by 4 above? This really looks like something which should happen in the sensor fusion / PID code, not here.
// Fabio: what's the point of dividing by 16 above? This really looks like something which should happen in the sensor fusion / PID code, not here.
Some divisions occur directly in sensor part, because we consider any other LSB are noise when the multicopter is running (empirical). There is also another advantage: we can keep some 16bits computation for addition/subtraction because the full 16 bits range is not used. (important to keep good performance on a 8bit processor)
// Fabio: no conversion to Rad or Degs? I'm missing something.
It's handled and documented in IMU part
// Fabio: a smart usage of MPU6050's FIFO queue can improve readings accuracy. Eg, using oversampling technique. See http://www.analog.com/static/imported-f ... N-1063.pdf
For GYRO, it's important to have to freshest data => at least for gyro, I'm not convinced about the fifo interrest. Maybe for ACC or MAG.
// TODO: Rewrite to do only a single read for bot gyro and acc values
A full multiwii loop works this way for each cycle:
- 2 set of gyro data are retrieved (2x6 bytes)
- 1 set of acc data is retrieved ( 1x6 bytes)
- mag data are retrieved only at a 10Hz pulse
- for baro data, it's more complicated because we must handle pressure and temperature request/get, but the retrieval rate is also slow.
I did some tests to gather I2C requests and minimize I2C bus usage, but it wasn't significantly more efficient than requesting data separately.
One explanation: if we want to request acc+gyro+mag, we must request 20 bytes 6+6+6+2. +2 because there is the temperature information (16 bits) between acc and gyro. We do not need this info but we must retrieve it as soon as we want to retrieve acc+gyro data via a single request.
Re: Support for MPU6050
Awesome Alex. Thanks for answering. A couple of thoughts.
Wow, than this has to be documented in config.h. I, as almost every MultiWii user out there I've been talking to, always thought that it was the speed of the whole I2C bus used for all sensors.
Gyroscope biases are affected by temperature. That's why Invensense put a thermometer in all of their devices. According to Sebastian Madgwick, making calibration biases dependent on temperature really improve performances. With the help of Felipe, we have been playing with this. See http://code.google.com/p/itg-3200driver ... etail?id=9 .. Have you ever considered doing this in multiwii? What do you think?
Alexinparis wrote:For GYRO: I think we can use something between 500 and 2000deg/s. 2000 is proven to work fine. 500 is proven to work on analogical gyros// Fabio: is it really good practice to override I2C speed set by the user in config.h? I'd say no.
I did this to dynamically change the speed of I2C bus. For instance 100kHz for a genuine WMP and 400kHz for an ACC.
The speed mentioned in config.h applies only for WMP or Nunchuk. So not relevant here.
Wow, than this has to be documented in config.h. I, as almost every MultiWii user out there I've been talking to, always thought that it was the speed of the whole I2C bus used for all sensors.
// TODO: Rewrite to do only a single read for bot gyro and acc values
A full multiwii loop works this way for each cycle:
- 2 set of gyro data are retrieved (2x6 bytes)
- 1 set of acc data is retrieved ( 1x6 bytes)
- mag data are retrieved only at a 10Hz pulse
- for baro data, it's more complicated because we must handle pressure and temperature request/get, but the retrieval rate is also slow.
I did some tests to gather I2C requests and minimize I2C bus usage, but it wasn't significantly more efficient than requesting data separately.
One explanation: if we want to request acc+gyro+mag, we must request 20 bytes 6+6+6+2. +2 because there is the temperature information (16 bits) between acc and gyro. We do not need this info but we must retrieve it as soon as we want to retrieve acc+gyro data via a single request.
Gyroscope biases are affected by temperature. That's why Invensense put a thermometer in all of their devices. According to Sebastian Madgwick, making calibration biases dependent on temperature really improve performances. With the help of Felipe, we have been playing with this. See http://code.google.com/p/itg-3200driver ... etail?id=9 .. Have you ever considered doing this in multiwii? What do you think?
Re: Support for MPU6050
@amourdurisk 35 euro for a BreakOut Board (BOB) for the MPU6050 doesn't look too expensive to, supposing they are paying all the taxes they are supposed to pay. The MPU6050 is still not much available and costs quite a lot, especially if bought in Europe. Eventually, many companies will came up with BOB for the MPU6050, I'm sure Sparkfun will have one. But, I wouldn't expect the price to drop much for it.
Please note that the Drotek BOB for the MPU6050 has a quite remarkable flaw. It seems that it doesn't break out the MPU6050 AUX I2C pins which are needed if you want to use that BOB with a magnetometer and take advantage of the master capabilities of the MPU6050. I don't know why they didn't break those pins out, especially as my open hardware designs for a BOB for the MPU6050 have been around for almost one year now and they could have simply used them as starting point.
Please note that the Drotek BOB for the MPU6050 has a quite remarkable flaw. It seems that it doesn't break out the MPU6050 AUX I2C pins which are needed if you want to use that BOB with a magnetometer and take advantage of the master capabilities of the MPU6050. I don't know why they didn't break those pins out, especially as my open hardware designs for a BOB for the MPU6050 have been around for almost one year now and they could have simply used them as starting point.
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
i think they took the BOB of the ITG3200 ....
anyway i don't want BOB but only the device so 35€ for the device is a little expensive :/
anyway i don't want BOB but only the device so 35€ for the device is a little expensive :/
Re: Support for MPU6050
Probably your best bet for raw sensor are Sparkfun or cdiweb.. See http://www.sparkfun.com/products/10937 and http://www.cdiweb.com/ProductDetail/MPU6050/422200 .. Hope this helps!
Last edited by fax8 on Tue Jan 10, 2012 10:25 pm, edited 1 time in total.
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
Thank you for the links
CDIweb ship only to canada mexico usa ..... grrrr
Sparkfun backordered again and again ..
CDIweb ship only to canada mexico usa ..... grrrr
Sparkfun backordered again and again ..
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
Hello,
I have some question about the LPF with the MPU6050.
My quad flies very well with ITG3200 and LPF42hz but really badly with same setting and the MPU6050.
I tried All LPF but no way.
Anyone have an idea ? anyone have the same problem ?
regards,
MAtt.
I have some question about the LPF with the MPU6050.
My quad flies very well with ITG3200 and LPF42hz but really badly with same setting and the MPU6050.
I tried All LPF but no way.
Anyone have an idea ? anyone have the same problem ?
regards,
MAtt.
Re: Support for MPU6050
Our initial tests showed that the MPU6050 is quite more stable than the ITG3200.. does the readings seem ok in the GUI??
Re: Support for MPU6050
fax8 wrote:Our initial tests showed that the MPU6050 is quite more stable than the ITG3200.. does the readings seem ok in the GUI??
Same here, I use the DroTek breakout.
//Frank
Re: Support for MPU6050
MPU6000 works for me at 256hz lpf (default).
-
- Posts: 31
- Joined: Wed Nov 02, 2011 8:58 pm
Re: Support for MPU6050
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.
Re: Support for MPU6050
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.
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.
-
- Posts: 1630
- Joined: Wed Jan 19, 2011 9:07 pm
Re: Support for MPU6050
Hi,
FreeIMU are again available since few days on flyduino or viacopter web shop.
A pressure sensor is not mandatory for multiwii.
FreeIMU are again available since few days on flyduino or viacopter web shop.
A pressure sensor is not mandatory for multiwii.
Re: Support for MPU6050
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.
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
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
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
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
Re: Support for MPU6050
@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
@amourdurisk I'll try investigating your problem.
@jstapels nice you are into building a FreeIMU.. post a picture on my website once your done
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
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
Re: Support for MPU6050
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.
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
Re: Support for MPU6050
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
// 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
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
output sitting flat on my desk
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?
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?
Re: Support for MPU6050
oh, great.
now I can use this code on a real processor (stm32) and not use silly approximations of math functions.
now I can use this code on a real processor (stm32) and not use silly approximations of math functions.
Last edited by timecop on Tue Mar 20, 2012 2:12 pm, edited 1 time in total.
Re: Support for MPU6050
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.
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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.
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.
Re: Support for MPU6050
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.
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
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
Re: Support for MPU6050
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.
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.
-
- Posts: 26
- Joined: Mon Jul 11, 2011 12:47 pm
Re: Support for MPU6050
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
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