Need better 6050 gyro calibration, any ideas?

Post Reply
User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

During implementation of a headfree mode without mag (idea bradquick) i found the 6050 gyro is drifting quite a bit, always CW, up to 1 deg / sec. The calib code as is does sum up 512 measurements, and shifts it left by 9 bits to divide trough 512. I think there is space for improvement. Maybe add 256 to the sum to avoid truncation?

Any other ideas how to improve the calibration ?

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Explanation:
If the calib sum will be 1020, the division will make 1020 / 512 = 1 out of it. But (1020 + 256) / 512 = 2 which is more accurate.
Same applies to acc calibraton. This is an accuracy improvement which wont break anything else. I have even tested it for neg numbers: -1025 / 512 = -3, but (-1025 + 256) / 512 = -2;

Another idea is to run the calibration at a higher resolution than the measurement, so we get a statistical accuracy improvement.

With both mods, adding 256 and statistically upping the resolution, i have seen a drift of 2 deg / minute on a 20$ nanowii.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

I do propose the following changes in file sensor. Donk kill me if its wrong, first time i do a patch.
Attachments
Sensors.cpp.patch.zip
(543 Bytes) Downloaded 435 times

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

+1 sounds reasonable to me and should be implemented

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Just a quick addition:
MultiWii is called MultiWii because of the use of the sensors from a Wii remote. Those gyros and accelerometers were fine 5 years ago, but the 6050 and currently the 9050 from Invensense run circles around them in terms of accurary, cross-axis error, temperature drift, etc ... and basically EVERY IMU out there uses them instead of the old Wii sensors. While I don't think MultiWii should be renamed MultiMPU it should recognize its popularity and focus on this particular sensor in terms of accuracy.

Ideas for improvement:
  • the 6050's gyro is configured to for full scale +-2000 °/s which corresponds to 16.4 LSB per °/s ... and ultimately just 4.1 LSB per °/s because of division by 4 in Gyro_getADC(). This could be improved to 32.8 or 65.5 LSB per °/s and use the full 16 bit range depending on how MultiWii behaves when the gyro saturates (if noise and vibration is an issue, user can activate averaging or LPF on the sensor itself)
  • same for the accelerometer ... it is configured for a full scale range of +-8 g and values get divided by 8 ... resulting in 512 LSB per g (when 0|0|512 means no tilt then 1|0|511 means tilted 0,11 degrees). Using a range of +-2 g would make far more sense, since ACC values are only used for calculations between +- 1.423 g in IMU.cpp. This would result in 2048 LSB per g (smallest detectable tilt 0,028 degrees) and could be improved further by not dividing by 8 ... noise is a big problem here, but the MPU6050 is pretty noise free ... countering vibrations needs filtering/averaging, not cutting away bits)
  • The structure of computeIMU() can also be improved for MPU6050 usage. Instead of splitting up the measurements into

    Code: Select all

    ACC_getADC()
    getEstimatedAttitude()
    Gyro_getADC()
    annexCode()
    Gyro_getADC() and getting average of both gyro measurements
    doOptionalSmoothing()

    ithis could be optimised (especially when LPF is used) for the MPU with

    Code: Select all

    MPU_getAllValuesInOneI2CCall()
    doOptionalSmoothing()
    getEstimatedAttitude()
    annexCode()

Higher resolution should also enable better accuracy for gyro calibration and the tilt calibration which can be set in smaller steps so the copter can longer stay in level mode without flying away ...

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

+1 :!: that is the way im going with my pocketquad build.

The different gyro range can easily be adapted with the SCALE factor for attitude estimation, while the different acc range doesent matter as long as 1G factor is adapted and the serial output to GUI is scaled.

I have found no clever + simple solution for the gyro factor in PID calculations yet ... and idea?

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Alexinparis »

Plüschi wrote:Explanation:
If the calib sum will be 1020, the division will make 1020 / 512 = 1 out of it. But (1020 + 256) / 512 = 2 which is more accurate.
Same applies to acc calibraton. This is an accuracy improvement which wont break anything else. I have even tested it for neg numbers: -1025 / 512 = -3, but (-1025 + 256) / 512 = -2;

Another idea is to run the calibration at a higher resolution than the measurement, so we get a statistical accuracy improvement.

With both mods, adding 256 and statistically upping the resolution, i have seen a drift of 2 deg / minute on a 20$ nanowii.


ok, see r1546

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Alexinparis »

Sebbi wrote:Just a quick addition:
MultiWii is called MultiWii because of the use of the sensors from a Wii remote. Those gyros and accelerometers were fine 5 years ago, but the 6050 and currently the 9050 from Invensense run circles around them in terms of accurary, cross-axis error, temperature drift, etc ... and basically EVERY IMU out there uses them instead of the old Wii sensors. While I don't think MultiWii should be renamed MultiMPU it should recognize its popularity and focus on this particular sensor in terms of accuracy.

We can in fact thank all all the smartphone community. I believe it is the main driver that brought low price & good perf sensors.


[*]the 6050's gyro is configured to for full scale +-2000 °/s which corresponds to 16.4 LSB per °/s ... and ultimately just 4.1 LSB per °/s because of division by 4 in Gyro_getADC(). This could be improved to 32.8 or 65.5 LSB per °/s and use the full 16 bit range depending on how MultiWii behaves when the gyro saturates (if noise and vibration is an issue, user can activate averaging or LPF on the sensor itself)

Did you ever observe the last bits in full resolution ?
I think the noise is so high that it is not exploitable.
+ it would require more 32 bits operations

[*]same for the accelerometer ... it is configured for a full scale range of +-8 g and values get divided by 8 ... resulting in 512 LSB per g (when 0|0|512 means no tilt then 1|0|511 means tilted 0,11 degrees). Using a range of +-2 g would make far more sense, since ACC values are only used for calculations between +- 1.423 g in IMU.cpp. This would result in 2048 LSB per g (smallest detectable tilt 0,028 degrees) and could be improved further by not dividing by 8 ... noise is a big problem here, but the MPU6050 is pretty noise free ... countering vibrations needs filtering/averaging, not cutting away bits)

There was a huge problem in the past that was difficult to analyse.
It was especially when the range was set to +/-2g.
Due to vibration, this range saturates easily on a multi and produces errored angle calculation in the IMU.
=> it is extremely important to set +/-8g if available even if we cut after some bits for the same reason aforementioned.

[*]The structure of computeIMU() can also be improved for MPU6050 usage. Instead of splitting up the measurements into

Code: Select all

ACC_getADC()
getEstimatedAttitude()
Gyro_getADC()
annexCode()
Gyro_getADC() and getting average of both gyro measurements
doOptionalSmoothing()


2 gyro reading + some averaging on it at each cycle is an empirical formula at the wii sensor time.
So far, it's a successful story :)
1 single gyro reading might produce a similar result on MPU sensor, I don't know.
For sure the result was horrible with WMP sensors.

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Smartphones indeed made sensors both better and cheaper.

Soooo, I conducted some tests with my NanoWii (MPU6050) and these are my results so far (no flight tests were conducted).

First test:
- configured the gyro for a full scale of +-500 °/s and using the value directly
- gyro data (imu.gyroADC) was logged through the guy, so it's not every cycle that was logged, but it shows a trend
- with the default configuration the resolution is 4.1 LSB per °/s and when left still, the gyro values are always 0
- with the improved accuracy configuration the resolution is 65.5 LSB per °/s and gyro values toggle around 0 with a standard deviation of 0.5 to 1.0 depending on the axis (switching on LPF with 98 Hz reduces the std to 0.4 to 0.6)
- imu.gyroData gets devided by 16, so the PID algorithm should behave the same as before
- attitude estimation works as before

=> the last bits are useful with this gyro configuration. Saturation should also be a non-issue, but obviously needs testing. Vibrations while flying can be countered by activating LPF or a custom averaging algorithm like the already implemented GYRO_SMOOTHING.

Second test:
- configured the accelerometer for a full scale of +-2 g and also using the value directly
- resolution was increased from 512 LSB per g to 16384 LSB per g
- standard deviation for raw data (ACC_LPF_FACTOR 0) was between 50 and 80 (0.03 - 0.05 m/s^2)
- standard deviation with default (ACC_LPF_FACTOR 4) was between 9 and 14 (0.005 - 0.0085 m/s^2)
- standard deviation for raw data (ACC_LPF_FACTOR 0 & LPF 98 Hz) was between 30 and 50 (0.02 - 0.03 m/s^2)
- standard deviation for raw data (ACC_LPF_FACTOR 4 & LPF 98 Hz) was between 10 and 13 (0.006 - 0.0077 m/s^2)

=> the last bits of the accelerator measurements are also useful, but are noisier. Reducing the full scale to +-2 g makes sense as higher accelerations aren't used for any calculations in MultiWii. Vibrations can mean more twitching, but since the gyros are pretty stable I am sure a less frequent accelerometer update with averaged values could counter that. If vibrations cause saturation of the sensor there might be something wrong with the copter (unbalanced props) or a higher LPF setting is needed for those copters. Needs testing ...

Third test:
- use special code for the 6050 in computeIMU():

Code: Select all

   #elif defined(MPU6050)
    Gyro_getADC();
    getEstimatedAttitude();
    annexCode();
    for (axis = 0; axis < 3; axis++) {
       imu.gyroData[axis] = imu.gyroADC[axis] / 16;
    }

- Gyro_getADC() now fetches all data in one step (can be used to get mag data as well and since temperature data sits between acc and gyro in the registers that is fetched as well)
- cycle time is now in the 1600 µs range and flash size is also reduced
- works as before

=> why not? needs testing in flight, but I can't see why this would be worse ...

Todos:
- with increased accuracy att.angle and conf.angleTrim resolution should increase (from 10th degrees to 100th degrees) as well
- the GUI should also be modified so it can handle the higher resolution of values
- flight tests

Branch on Github (diff) - only bench tested

P.S.: Could conf.angleTrim be displayed in the GUI somewhere and be a user modifiable value?

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Nice idea.

I have test flown my own version with no division of the gyro / acc values, but i have not upped the sensor resolution yet. I do keep the higher resolution into attitude estimation and PID calculation (except pid I term). Flies very smooth.

The higher resolution makes it necessary to check all the calculations for possible overflows. The pid I term calculation will choke :)

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Higher resolution doesn't seem to affect calculations anywhere (I adjusted for overflows in atan2 and accmag calculations). The PID controller uses imu.gyroData instead of imu.gyroADC which has reduced resolution in my "patch" ... should behave like before, although it doesn't benefit from the increased resolution this way ;-)

I don't know if higher resolution even benefits the flight performance of a copter ... maybe this is all just an academical effort, but it could at least make finer level trims possible (i was never able to trim my copter to not slowly float away in one direction or the other). And I can't see why focusing on the MPUx060 IMUs instead of the - now and in comparison - really bad Wii sensors would be a bad thing ...

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

I have had some tests with "native 6050 resolution" and "only one gyro read / cycle" on my 10cm and 23cm quads. The resolution for gyro is 4 times the normal multiwii and acc is 8 times normal multiwii resolution. Those resolutions go into attitude estimation and into pid calculation (except I term). It flies well.

To know if it's "academical" or not i would need 2 quads, one with normal one with increased resolution and test fly them alternatively. Its very hard to tell differences "from 2 days old memory".

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Crashpilot1000 »

@Sebbi: Thank you VERY much for actually trying out things and pushing the multiwii code ahead AND FOR SHARING YOUR results!!
@Plüschi: Thank you also very much for your impulses and contributions and actually taking mwii ahead.
I think the "official" side is somehow "done" with coding/changing/rethinking core parts. Hopefully my perception is wrong on that.
Recently I tried to get rid of that predefined acc1G stuff. I think it's worth to check out. https://github.com/Crashpilot1000/Summe ... ors.c#L195 . I think the inflight calibration has to be revisited as well because it should not alter the acc calibration but the angleTrim parameters.
A simple temperature compensation for mpu and mag wouldn't hurt either.

What is the frequency - band that is actually needed on acc doing INS stuff? Filtering out vibrations and prop downwash will be a low frequ cutoff. Noise from the bearings etc might be something with high frequency. Does anyone have a publication on appropriate filterdesign / frequency range? Even the kalmanstuff would need some prefiltered data for state prediction? Or is Kalman the only hope on that subject, and does all the magic??

Greetings Rob

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Thank you Rob :D

I suggest your gyro calib code
"gyroZero[axis] = g[axis] / 1000;" should be "gyroZero[axis] = (g[axis]+500) / 1000;",
same for acc calib. :ugeek:

With imu.c i would also suggest sebbi's variant.
The gyro averaging and with it the 650us wait is not necessary with mpu6050 or similar. Yes i have flight tested it.

Code: Select all

void computeIMU () 
{
  uint8_t axis;
  mpu6050_getADC();
  getEstimatedAttitude();
  annexCode();
  for(axis=0;axis<3;axis++)
    gyroData[axis] = gyroADC[axis];
}


Ok,ok, im a pedant :twisted:

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

I should probably do some flight tests and clean up the code (dirty division by 16 for gyrodata) ... so main devs can at least consider this being included into _shared / next release ;-)

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

Maybe fix axis order to allow proper sensor rotation and change calculations to do NED or something and reorient all sensors while you're at it, too. Except I bet noone will accept sensor rotation loop cuz it will add 9 math operations per loop cycle OMG and will be too slow for tarduino328p

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Why is it important to do NED? Only pitch is reversed to what's considered normal for an airplane. Would just need an inversion in the calculations ...

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Crashpilot1000 »

Having NED is not so important in the first place but if redesigning the core from the scratch it would be nice to have it according to some standard thats all - in my opinion. Arducopter is also not following NED on the Z axis (up is positive there). TC is just trolling around and happy by writing three times tarduino in one line, perhaps combining it with some offensive language. Maybe he would be better off looking at your github and seeing the gems you placed there :) .

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

Yes, the kind of stupid inversion I had to figure out when using matrix rotate instead of angle approximation...
Why should programmer have to worry about this?

Crashpilot1000 wrote:blablablablabla
Maybe he would be better off looking at your github and seeing the gems you placed there :) .


Yea, maybe his is better to look at, with actual development history so people could go and look at changes.
Instead of uploading a zip and then deleting it when "releasing" new version.

picstart
Posts: 10
Joined: Fri Aug 09, 2013 9:30 pm
Location: Florida USA

Re: Need better 6050 gyro calibration, any ideas?

Post by picstart »

All GYROs will drift it is just a matter of how much. The GYRO is the principal sensor mostly because it is a first order derivative and the second order derivatve ACC is comingled with the gravitational acceleration. OK so we have angular rate of change where as it would be better to have just the euler angles. So the rate change of the Gyro gets integrated and brings in an unkown constant as all integration does. Now the magnetic field isn't affected by acceleration so a yaw pitch roll correction ( correlation test) is possible . Next in level flight with small accelerations the constant gravitational field can be used to correlate ( adjust for the angular GYRO drift). It is not that more accurate rate change GYROS aren't useful it is just that the integration must introduce a constant drift that needs a correlation correction. I don't see why a 1% per sec drift is too bad. In fact these GYROS are remarkable.
An aside
Knowing your speed with extreme accuracy doesn't give you your absolute distance from an arbitrary point unless you also know a correlation point with equal precision (an example correlation point could be the position of your starting point aka as the integration constant above). The GYRO de-correlates (drifts) from its starting position with time and temperature ( electrical noise or whatever) limit the accuracy of the angular velocity measurement. The error always accumulates so a correlation correction is always needed so the last bit from the GYRO is only meaningful if we have similar bit precision with respect to correlation.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

picstart wrote:Now the magnetic field isn't affected by acceleration


The problem with the magnetic field is the sensors are not very good, and easily disturbed by nearby bat/esc cables. Also the vector doesent go straight down, but zooms into earth at a 62 deg angle here. So the same problem we have with ACC and YAW, Yaw angle is not correctable since its perpendicular to gravity vector, we have with MAG, but on a plane inclined 28 deg affecting all 3 axes. Not easy to work with.

picstart wrote:a 1% per sec drift is too bad. In fact these GYROS are remarkable


what you refer as 100%? 180 deg?
With 2.2 unpatched multiwii it gets as bad as 1 deg / sec, but with patched calib and full gyro resolution it is MUCH MUCH better. On the bench 2 deg / min, outside i have seen 5 deg min. Those gyros are not only remarkable, those gyros consist of black magic :)

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

picstart wrote:All GYROs will drift it is just a matter of how much. The GYRO is the principal sensor mostly because it is a first order derivative and the second order derivatve ACC is comingled with the gravitational acceleration. OK so we have angular rate of change where as it would be better to have just the euler angles. So the rate change of the Gyro gets integrated and brings in an unkown constant as all integration does. Now the magnetic field isn't affected by acceleration so a yaw pitch roll correction ( correlation test) is possible . Next in level flight with small accelerations the constant gravitational field can be used to correlate ( adjust for the angular GYRO drift). It is not that more accurate rate change GYROS aren't useful it is just that the integration must introduce a constant drift that needs a correlation correction. I don't see why a 1% per sec drift is too bad. In fact these GYROS are remarkable.
An aside
Knowing your speed with extreme accuracy doesn't give you your absolute distance from an arbitrary point unless you also know a correlation point with equal precision (an example correlation point could be the position of your starting point aka as the integration constant above). The GYRO de-correlates (drifts) from its starting position with time and temperature ( electrical noise or whatever) limit the accuracy of the angular velocity measurement. The error always accumulates so a correlation correction is always needed so the last bit from the GYRO is only meaningful if we have similar bit precision with respect to correlation.


Three things:
1) there are a lot of pilots who fly gyro only ... more precision wont hurt
2) when a gyro value of 8192 equals 2000 °/s your calibration has an accuracy of 0.244 °/s ... in the case this calibration is off 0.122 °/s (the max rounding error) due to inaccuracy, this would equal a drift of 1° every 8 seconds, which is super bad. Higher accuracy = better calibration = less drift.
3) When you want to fly in modes requiring acc/mag you are correct. Gyro accuracy doesn't play that big a role, but better accelerometer accuracy/resolution helps to find better trims for angle/horizon mode. You can currently only change the trim in 0.1° steps and the resolution of the acc measurements (512 values for 1g) is also in that range. If your copter would be perfectly level exactly between those steps (max error) it will always drift away sideways (0.05° tilted) and can't stay on position for a longer than a few seconds period.
Last edited by Sebbi on Mon Aug 12, 2013 9:54 am, edited 1 time in total.

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by BarneyG »

I was trying to back port this into the regular 2.2 "release" :) ... I'm running into problems with :
i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x08);
in Gyro_init() in Sensors.ino

When it is set like this the board wont arm and in the GUI the Cycle time becomes a large negative number and the graph update slows down drastically . This is on a Crius AIOP v2 board from RC Timer.

Any ideas ?

Any

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Could be a problem with the gyro calibration ... if it can't calibrate it wont start.

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Post by BarneyG »

I don't think so ... Calibration looks like it completes and if I change the x08 back to x18 and upload it will immediately arm.
Besides the lack of arming is only part of the problem ... Why does everything run slow too ?

I'm going to try making the changes to trunk and uploading that to see if it is something in 2.2 causing a conflict

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by BarneyG »

Ok you are right ... it does look like calibration is the problem ... I patched trunk and got the same result. Uploaded normal 2.2 with a cleared eeprom and tried to arm without first having done ACC Calibration ... and got exactly the same slow GUI refresh and cycle time becomes very negative ... ran the ACC calibration and everything started working properly ... so as you said it is a problem with calibration.

Something I noticed on the GUI is the ACC pitch and yaw have a very big fluctuations -10/20 to +10/20 while the Z is pinned at 1000 - this is even after calibration. I had GYROCALIBRATIONFAILSAFE defined ... Removed this and tried again ... success ... looks like with the increased accuracy it never thinks it has stopped moving.

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

GUI should probably be updated to display acc values a 'G' instead of uint16_t...

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by BarneyG »

probably but it would appear to me there is also a problem with the line :

if(abs(imu.gyroADC[axis] - previousGyroADC[axis]) > 8) tilt=1;

in sensors.cpp

At least on my quad abs(imu.gyroADC[axis] - previousGyroADC[axis]) is always greater than 8.

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

Yea, of course. Because it's now full scale without "shifting".

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by BarneyG »

forgive my ignorance ... my low level programming is not what it should be and I'm still getting to grips with multiwii ... so would I need to multiply the 8 by 4 or 32 in order to get things back into line ?

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

Yeah, I think original divide was /16, so you'll need to at least 8*16 in the "moving" check.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Barney,
You have to set acc_1G to the new value (4096).

If you use undivided acc or gyro values you have to scale them down in serial.c where they are sent to the GUI.
This way:

Code: Select all

   case MSP_RAW_IMU:
     headSerialReply(18);
     for(uint8_t i=0;i<3;i++) serialize16(accSmooth[i]>>3);
     for(uint8_t i=0;i<3;i++) serialize16(gyroData[i]>>2);
     for(uint8_t i=0;i<3;i++) serialize16(magADC[i]);
     break;

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by timecop »

for(uint8_t i=0;i<3;i++) serialize16(accSmooth[i]>>3);

for loops on one line, for FASTER EXECUTION.
You heard it here first.

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

MultiWii is using a struct imu ... no accSmooth/gyroData/etc ...

Anyway, you have to adapt ALL changes from the Github branch. Not just some of them. The repo itself is based on trunk, so this should work right away ...

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

timecop wrote:
for(uint8_t i=0;i<3;i++) serialize16(accSmooth[i]>>3);

for loops on one line, for FASTER EXECUTION.
You heard it here first.


And you'll only hear it here ...

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Sebbi wrote:MultiWii is using a struct imu ... no accSmooth/gyroData/etc ...


Dev Version -> struct
Vanilla 2.2 -> accSmooth/gyroData/etc

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Crashpilot1000 »

He will not understand what you say unless you supply him a diff of that in relation to his own baseflight....
BTW: I checked out the "readout all mpu6050 stuff with one I2C call" and using unaveraged and uncrippled gyro data (with altered main pid controller). Working very good so far even with non bitshifted acc data with 4G res.

BarneyG
Posts: 39
Joined: Tue May 07, 2013 4:42 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by BarneyG »

Sebbi wrote:MultiWii is using a struct imu ... no accSmooth/gyroData/etc ...

Anyway, you have to adapt ALL changes from the Github branch. Not just some of them. The repo itself is based on trunk, so this should work right away ...


Admittedly I did the patching by hand so I may have made a mistake but all the changes were put in ... I also made them to trunk and tried that ... got exactly the same result ... if GYROCALIBRATIONFAILSAFE is defined tilt always get set to 1 so the gyro's never calibrate and you can't arm the board.

Plüschi wrote:Barney,
You have to set acc_1G to the new value (4096).


Bit confused ... the patch has this line in :
#define ACC_1G (512*32)

(obviously I used acc_1G when I tried to back port it to 2.2 :)) which is rather more than 4096 :)

If you use undivided acc or gyro values you have to scale them down in serial.c where they are sent to the GUI.
This way:

Code: Select all

   case MSP_RAW_IMU:
     headSerialReply(18);
     for(uint8_t i=0;i<3;i++) serialize16(accSmooth[i]>>3);
     for(uint8_t i=0;i<3;i++) serialize16(gyroData[i]>>2);
     for(uint8_t i=0;i<3;i++) serialize16(magADC[i]);
     break;


Not so worried about the GUI but you say set acc_1G to 4096 but the patch sets acc_1G to #define ACC_1G (512*32)

Sebbi
Posts: 478
Joined: Sun Jul 08, 2012 1:08 am
Location: Germany
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Sebbi »

Indeed ... full scale also changed from 8g to 2g and the division by 8 is gone, too ... that changes the ACC_1G define by a factor of 32 ;-)

To scale the value correctly in the GUI accSmooth should be divided by the same factor. GyroData does not need division, because it's scaled in computeIMU() ... GyroADC however would need division by 16 for the GUI.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Crashpilot1000 wrote:using unaveraged and uncrippled gyro data ... with altered main pid controller


Can you share the PID controller adjustment for higher res?

I just altered the shift values in the PID, and I term stay 16 bit, and i would like something better :mrgreen:

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Need better 6050 gyro calibration, any ideas?

Post by Crashpilot1000 »

@Plüschi:
Well currently I am struggeling with the new pidcontroller I also observed a flip at start on the (stone)ground that was probably due to ACC saturation. So I changed the acc resolution back to 8G (but without bitshifting resolution away).
Here is my current version of the (mwii 2.1) main Pidcontroller with pt1 element and everything on floatpoint variables. Perhaps it is of some use for you. It will work with raw mpu gyrodata.

Cheers
Rob

Code: Select all

    tmp0flt = (float)cycleTime;
    MwiiTimescale = tmp0flt * 3.3333333e-4f;
    dT   = tmp0flt * 0.000001f;                                      // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624
    prop = (float)max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL]));  // range [0;500] Crashpilot: prop is float now

    switch (cfg.mainpidctrl)
    {
    case 1:                                                          // 1 = OriginalMwiiPid pimped by me
        for (axis = 0; axis < 3; axis++)
        {
            if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2)        // MODE relying on ACC 50 degrees max inclination
            {
                errorAngle  = constrain(2.0f * (float)rcCommand[axis] + GPS_angle[axis], -500.0f, +500.0f) - angle[axis] + (float)cfg.angleTrim[axis];
                PTermACC    = errorAngle * (float)cfg.P8[PIDLEVEL] * 0.01f;
                tmp0flt     = (float)cfg.D8[PIDLEVEL] * 5.0f;
                PTermACC    = constrain(PTermACC, -tmp0flt, +tmp0flt);
                errorAngle *= MwiiTimescale;
                errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000.0f, +10000.0f); // WindUp
                ITermACC    = (errorAngleI[axis] * (float)cfg.I8[PIDLEVEL]) / 4096.0f;
            }

            if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2)        // MODE relying on GYRO or YAW axis
            {
                error  = (float)rcCommand[axis] * 320.0f / (float)cfg.P8[axis];
                error -= gyroData[axis];
             
                PTermGYRO = (float)rcCommand[axis];
                error *= MwiiTimescale;
                errorGyroI[axis] = constrain(errorGyroI[axis] + error, -64000.0f, +64000.0f);
                if (abs(gyroData[axis]) > 2560) errorGyroI[axis] = 0;
                ITermGYRO = errorGyroI[axis] * (float)cfg.I8[axis] * 0.00003125f;
            }

            if (f.HORIZON_MODE && axis < 2)
            {
                PTerm = (PTermACC * (500.0f - prop) + PTermGYRO * prop) * 0.002f;
                ITerm = (ITermACC * (500.0f - prop) + ITermGYRO * prop) * 0.002f;
            }
            else
            {
                if (f.ANGLE_MODE && axis < 2)
                {
                    PTerm = PTermACC;
                    ITerm = ITermACC;
                }
                else
                {
                    PTerm = PTermGYRO;
                    ITerm = ITermGYRO;
                }
            }
            PTerm          -= (gyroData[axis] * (float)dynP8[axis] * 0.003125f);
            delta           = (float)(gyroData[axis] - lastGyro[axis]) / (MwiiTimescale * 4);
            lastGyro[axis]  = gyroData[axis];
            deltaSum        = delta1[axis] + delta2[axis] + delta;
            delta2[axis]    = delta1[axis];
            delta1[axis]    = delta;
            if (cfg.mainpt1cut != 0)                                       // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624
            {
                deltaSum        = lastDTerm[axis] + (dT / (MainDpt1Cut + dT)) * (deltaSum - lastDTerm[axis]);
                lastDTerm[axis] = deltaSum;
            }
            DTerm           = (float)deltaSum * (float)dynD8[axis] * 0.03125f;
            axisPID[axis]   = PTerm + ITerm - DTerm;
        }
        break;

case 2:
....   

Calculation of MainDpt1Cut:
MainDpt1Cut = 1.0f / (2.0f * M_PI * (float)cfg.mainpt1cut);

The default is:
cfg.mainpt1cut = 15



EDIT: ACC saturation was not the problem. So back on 4G....
EDIT:EDIT: Back to a mixture of better calibration and the div by 4 stuff but on floatpoint basis after offset substraction.

User avatar
Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

Re: Need better 6050 gyro calibration, any ideas?

Post by Plüschi »

Ty crashpilot. Is arduino float a 32bit type?


User avatar
U.Sentenza
Posts: 17
Joined: Fri May 17, 2013 11:44 am

Re: Need better 6050 gyro calibration, any ideas?

Post by U.Sentenza »

Hello,
I have a question: is it right to change the following lines:

global_conf.accZero[ROLL] = (a[ROLL]+256)>>9;
global_conf.accZero[PITCH] = (a[PITCH]+256)>>9;
global_conf.accZero[YAW] = ((a[YAW]+256)>>9)-ACC_1G; // for nunchuk 200=1G


in this way:

global_conf.accZero[ROLL] = (a[ROLL]+ACC_1G)>>9;
global_conf.accZero[PITCH] = (a[PITCH]+ACC_1G)>>9;
global_conf.accZero[YAW] = ((a[YAW]+ACC_1G)>>9)-ACC_1G; // for nunchuk 200=1G

User avatar
Hamburger
Posts: 2578
Joined: Tue Mar 01, 2011 2:14 pm
Location: air
Contact:

Re: Need better 6050 gyro calibration, any ideas?

Post by Hamburger »

I think not.
It looks to me like the +256 is used for mathematical rounding of the folowing >>9 which is equal to /512

User avatar
U.Sentenza
Posts: 17
Joined: Fri May 17, 2013 11:44 am

Re: Need better 6050 gyro calibration, any ideas?

Post by U.Sentenza »

Ok, thanks for the clarification

Post Reply