Bug in the code of the ADXL345

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Bug in the code of the ADXL345

Post by wolle »

Hello,

We have found that is a bug in the code at the ADXL345.
It should be called when the sensors as follows:

Code: Select all

// ************************************************************************************************************
// I2C Accelerometer ADXL345
// ************************************************************************************************************
// I2C adress: 0x3A (8bit)    0x1D (7bit)
// Resolution: 10bit (Full range - 14bit, but this is autoscaling 10bit ADC to the range +- 16g)
// principle:
//  1) CS PIN must be linked to VCC to select the I2C mode
//  2) SD0 PIN must be linked to VCC to select the right I2C adress
//  3) bit  b00000100 must be set on register 0x2D to read data (only once at the initialization)
//  4) bits b00001011 must be set on register 0x31 to select the data format (only once at the initialization)
// ************************************************************************************************************
#if defined(ADXL345)
void ACC_init () {
  delay(10);
  i2c_writeReg(ADXL345_ADDRESS,0x2D,1<<3); //  register: Power CTRL  -- value: Set measure bit 3 on
  i2c_writeReg(ADXL345_ADDRESS,0x31,0x0B); //  register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
  i2c_writeReg(ADXL345_ADDRESS,0x2C,0x09); //  register: BW_RATE     -- value: rate=50hz, bw=20hz
  acc_1G = 256;
}

void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, ADXL435 is ok with this speed
  i2c_getSixRawADC(ADXL345_ADDRESS,0x32);

  ACC_ORIENTATION( ((rawADC[1]<<8) | rawADC[0])/8 ,
                   ((rawADC[3]<<8) | rawADC[2])/8 ,
                   ((rawADC[5]<<8) | rawADC[4])/8 );
  ACC_Common();
}
#endif


There is no "/ 8" in the three lines. If this is added, the copter flying great with the ADXL345.
Can you please add this in the code with future husband?

Thank wolle
http://www.wii-copter.de

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

Re: Bug in the code of the ADXL345

Post by Hamburger »

wolle wrote:Hello,

We have found that is a bug in the code at the ADXL345.
...
There is no "/ 8" in the three lines. If this is added, the copter flying great with the ADXL345.

Wolle,
I do not understand:
is this /8 required by the adxl345 reference description (and was wrongly implemented in MWii), or
is it your personal experience of your copter flying better (with the tuned down adc readings)?
Cheers, Hamburger

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

Hello Hamburger,

We use the ADXL345 with us. Just about anyone who has used our controller had big problems with the code for the ADXL345. We have these so wired as he is also listed in the datasheet.

With the code that was previously in the MultiWii, the ACC has flown in all possible directions. It was virtually impossible to hire him clean.
After I had looked at the sheet again and have more accurately compared with the BMA180, I am of the opinion that they differ in the values ​​of hard.

Insert with the "/ 8" of the copter with the ACC is flying very well!

Unfortunately I'm not the best when it comes to programming in C. Therefore, I can not really describe what's led to the ground.
Maybe someone can explain to me but of you.

We have now tested some Copter with these settings. This was all very well.

Greeting wolle

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

Re: Bug in the code of the ADXL345

Post by timecop »

That doesn't make sense, if you /8 each axis acc_1G should also be changed.

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

I've also been thinking about it. At what value should we change this?

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I just test your modifaction @home.
It's true the copter is now really stable like a rock

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

:mrgreen:

Now you should still know why :?:

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I want to make more tests (with and without) because I had few ACC trim but now it's really a rock.
I don't know why ?????

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Bug in the code of the ADXL345

Post by copterrichie »

Makes perfect sense to me! I have been flying the MMA7455 which has a 10 bit resolution and it has always been solid as a rock in both the 2 and 8G modes.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

The only error we can find is on the value of 1g :
FULL_RES Bit
When this bit is set to a value of 1, the device is in full resolution mode, where the output resolution increases with the g range set by the range bits to maintain a 4 mg/LSB scale factor.

in this case acc_1G = 250 but it's not really a problem.
I don't understand why the /8 (a noise rejection ?????)

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

Re: Bug in the code of the ADXL345

Post by timecop »

Since we're picking on ADXL345, there's another tidbit in the datasheet page 33, "Operation at voltages other than 2.5V"

Due to slight changes in the electrostatic forces as supply voltage is varied, the offset and sensitivity change slightly. When operating at a supply voltage of VS = 3.3 V, the x- and y-axis offset is typically 25 mg higher than at Vs = 2.5 V operation. The z-axis is typically 20 mg lower when operating at a supply voltage of 3.3 V than when operating at VS = 2.5 V. Sensitivity on the x- and y-axes typically shifts from a nominal 256 LSB/g (full-resolution or ±2 g, 10-bit operation) at VS = 2.5 V operation to 265 LSB/g when operating with a supply voltage of 3.3 V.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

Well done !!!!
so :
The resolution @2,5V is not 4mg/LSB but 3,9mg/LSB
and the acc_1G is not 256 (2,5V) but 265 (@3V3)

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

You've already tested it so?

Is it then the "/ 8" to remove and put acc_1G = 265?

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

wolle wrote:You've already tested it so?

Is it then the "/ 8" to remove and put acc_1G = 265?

no we just try to understand

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

Re: Bug in the code of the ADXL345

Post by timecop »

Bledi wrote:Well done !!!!
so :
The resolution @2,5V is not 4mg/LSB but 3,9mg/LSB
and the acc_1G is not 256 (2,5V) but 265 (@3V3)


Yes, but accX/Y also decreases.

nicodh
Posts: 44
Joined: Sun Apr 08, 2012 1:42 pm

Re: Bug in the code of the ADXL345

Post by nicodh »

When you divide by 8, all data is divided by 8 so at 90° x and y would show just 30 instead of 1G value (256)

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Bug in the code of the ADXL345

Post by copterrichie »

nicodh wrote:When you divide by 8, all data is divided by 8 so at 90° x and y would show just 30 instead of 1G value (256)


I think if you change the following for the ADXL345 when dividing by 8, all will be well.

acc_1G = 256;

to

acc_1G = 64;

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

Re: Bug in the code of the ADXL345

Post by Hamburger »

"flys better" still is not very scientific for me. I respect your experience of it feeling better but want a solid basis to understand.

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

I would also like to understand.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I go back to the normal version, just change acc_1G = 265.
It looks to be less solid. I don't understand why !

nicodh
Posts: 44
Joined: Sun Apr 08, 2012 1:42 pm

Re: Bug in the code of the ADXL345

Post by nicodh »

I tested it and indeed it seems to work better (less drunken sailor). But I think the reason is because we are using integers. So if you divide by 8 anything beetween -7 and 7 will be zero. So before, you had a fluctuation from -7 to 7 and the FC tried to correct that all the time. After dividing, you have 0 so it doesn't get as much corrections and that seems to work better.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

yes it's my first idea : noise rejection. We eliminate moves under 32mg ( 8 * 4mg)
So normaly it's possible to have the same with

Code: Select all

ACC_ORIENTATION( (int((rawADC[1]<<8) | rawADC[0])/8) * 8 ,
                   (int((rawADC[3]<<8) | rawADC[2])/8) * 8 ,
                   (int((rawADC[5]<<8) | rawADC[4])/8) * 8 );

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Bug in the code of the ADXL345

Post by PatrikE »

Yhe values from my ADXL345 drifts +/- 4 digits in the gui and feels anything but stable in air.
I will test this to.

nicodh
Posts: 44
Joined: Sun Apr 08, 2012 1:42 pm

Re: Bug in the code of the ADXL345

Post by nicodh »

What about the acc1g value? Don't you thing it has to be changed? Because now the 1G is no more 256 but 256/8.

I decided to reduce the limit of the adx, I set it in 4G. I don't know if it worth to have it at 8G. The "problem" would be you have more precision. I also changed the division /8 to /4 since I believe it's related to the limit. In gui, values almost don't chnaged when the board is not moving.
I will need to do a flight test to know if it is better.

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Bug in the code of the ADXL345

Post by marbalon »

i think it is not because better filtering, it is because less correction from ACC, angle is calculated more from gyro data then ACC, so for other sensors it should looks similar. Maybe this is good way, calculate angle more from gyro, and only correct from ACC...

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I had made more tests :
- normal : ok put not perfect
- 1G set to 265 no change
- 1G set to 265/8 and ACC values /8 : trim need to be correct and the result looks worth than normal verion
- 1G set to 265 and ACC values /8 : the result is perfect. I can flight just over my bed with just small corrections

I really don't understand and I don't know the impact of dividing value by 8 and let 1G unchange
Last edited by Bledi on Sun Apr 29, 2012 11:48 am, edited 1 time in total.

User avatar
kos
Posts: 286
Joined: Thu Feb 16, 2012 4:51 am
Location: Fr

Re: Bug in the code of the ADXL345

Post by kos »

dividing acc influence by 8 ?

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I don't try other values but yes when you do that
ACC_ORIENTATION( ((rawADC[1]<<8) | rawADC[0])/8 ,
((rawADC[3]<<8) | rawADC[2])/8 ,
((rawADC[5]<<8) | rawADC[4])/8 );

You really have a great change !

wolle
Posts: 13
Joined: Fri Apr 27, 2012 10:21 am
Location: Germany
Contact:

Re: Bug in the code of the ADXL345

Post by wolle »

Yes, we set the come to pass.

We understand though not quite know why, but some things must not be understood. :lol:

It would be great if it fits with the in the next versions.

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

Re: Bug in the code of the ADXL345

Post by Hamburger »

wolle wrote:Yes, we set the come to pass.
We understand though not quite know why, but some things must not be understood. :lol:
It would be great if it fits with the in the next versions.

difficult to decide when the basis is not fundamental evidence (like datasheet/vendor's reference implementation) but individual symptoms only.
I have used a FreeIMU with adxl345 and could fly that copter without any unusual observations compared to my other copters with other sensors. So I can only speculate on what your mod would do that.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

Is someone had try it and confirm what I saw ?

vectR
Posts: 5
Joined: Mon Apr 30, 2012 3:23 am

Re: Bug in the code of the ADXL345

Post by vectR »

Bledi,
I can confirm that using /8 in the ADXL345 code stabilizes the level mode.
I added this factor into my code along with acc_1g = 256.
This appears to be needed based on the data sheet which specifies using a scale factor for proper conversion of raw LSB units into g (gravity) units.
Several other sensors in multiwii 2.0 including gyros also use similar scale factors when reading raw data.
The exact scale factor 8 or 4, etc. may be open to debate, but I think something should be included in future code upgrades

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

Re: Bug in the code of the ADXL345

Post by timecop »

Are you dudes running 'accel calibration' after flashing with /8 code? cause there's no way its gonna work like that with acc1G being 256.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

No calibration for me, just acc trim

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

Re: Bug in the code of the ADXL345

Post by Alexinparis »

Hi,
With a wrong acc_1G, angles should not be correct anymore, and the value of ZACC when the copter is reverted should not match -acc_1G.
is it the case ?
The correction might be however better near 0 inclination, it's something we should find out why.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

yes acc_1G is totally wong and on the GUI the 3D position is false.
Division by 8 is not a solution but there is really something interesting

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

Re: Bug in the code of the ADXL345

Post by Hamburger »

How does /8 compare to one of the gyro smoothing options already in MWii code?

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Bug in the code of the ADXL345

Post by shikra »

I have a board with a adxl345 which has always been very twitchy / crap compared to my sirius boards. I assumed it was a bad sensor and stopped using. Will retry. I used a high LPF factor #define ACC_LPF_FACTOR (any reason that is hidden in IMU and not in the advanced config section?).

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Bug in the code of the ADXL345

Post by mahowik »

Hi guys,

Let me explain what is going on :)

If you have a noise on ACC (because of vibrations or some other reason e.g. electrical influence) IMU takes this into account via Complementary Filter (CF) and pass this noise to PID regulator where "I" part accumulate/integrate this small inclinations (but actually it's noise) to incorrect compensations. As result you have drift or unnecessary wobbles...

By dividing the raw ACC data to 8 you have:
1. reduced angles measuring. E.g. real 45 deg will be measured as 45/8 deg in ACC. As result IMU integrator will use (and go to) decreased angles for correction and pass decreased angles to PID regulator which will produce the decreased control to the motors...
2. low sensitivity. It means if you have noise in range 1-8, it will be zero by dividing. So as result in noised configuration you have profit...
BUT in spite of some advantages, it gives fully incorrect angles estimation!!!

For noise configuration I made a complex solution a couple of months ago (which is already checked with many guys from Russian forum):
1. float LPF with 1-2Hz cut frequency used for ACC in IMU.

Code: Select all

      #define ACC_LPF_FACTOR 100
...........
      #define lfpFactor (1.0f/ACC_LPF_FACTOR)
      static float accLPF[3];
..........
      accLPF[axis] = accLPF[axis] * (1.0f - lfpFactor) + accADC[axis] * lfpFactor;
      accSmooth[axis] = accLPF[axis];

2. Gyro Weight (GYR_CMPF_FACTOR) was increased to 400-500 to take into account more gyro than acc because gyro less sensitive to vibrations...
3. set gyro LPF to 42Hz in case of itg3200/3205 (#define ITG3200_LPF_42HZ)
4. reduce Level PIDs. P=7.0; I=0.01. Where I=0.045 is to much for most configurations and produce jumps on start or unnecessary wobbles.
5. make acro PID more smooth by increasing D param, where P should be increased accordingly, because big D push and decrease P...
Good are P=5.2; D=35

For details you can find prepared 1.9 and 2.0 sketches (with predefined PID settings) here http://forum.rcdesign.ru/blogs/83206/

I hope it will kill your headache with any drift at all! Enjoy! :)

thx-
Alex
Last edited by mahowik on Mon Apr 30, 2012 10:19 pm, edited 1 time in total.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I love your idea. I will try it because for me too the only explain for the result /8 is the noise rejection and the most important wieght of gyro

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

@mahowik, I just test your mod ! It's really THE SOLUTION.
I just test with a CRIUS LITE on a ARwiiDrone (ARDRONE with keda 20-50s) and it's really crazy the difference.
For my first test I don' set gyro LPF, I undefined the deadzone but it's really a pleasure
Last edited by Bledi on Mon Apr 30, 2012 8:13 pm, edited 1 time in total.

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Bug in the code of the ADXL345

Post by mahowik »

cool!!! ;)

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

A correct my misstake is not a CRIUS SE but a CRIUS LITE

marbalon
Posts: 107
Joined: Thu Aug 18, 2011 10:59 am

Re: Bug in the code of the ADXL345

Post by marbalon »

mahowik wrote:Hi guys,
1. float LPF with 1-2Hz cut frequency used for ACC in IMU.

Code: Select all

      #define ACC_LPF_FACTOR 100
...........
      #define lfpFactor (1.0f/ACC_LPF_FACTOR)
      static float accLPF[3];
..........
      accLPF[axis] = accLPF[axis] * (1.0f - lfpFactor) + accADC[axis] * lfpFactor;
      accSmooth[axis] = accLPF[axis];

2. Gyro Weight (GYR_CMPF_FACTOR) was increased to 400-500 to take into account more gyro than acc because gyro less sensitive to vibrations...
3. set gyro LPF to 42Hz in case of itg3200/3205 (#define ITG3200_LPF_42HZ)
4. reduce Level PIDs. P=7.0; I=0.01. Where I=0.045 is to much for most configurations and produce jumps on start or unnecessary wobbles.
5. make acro PID more smooth by increasing D param, where P should increased accordingly, because big D push and decrease P...
Good are P=5.2; D=35


I can write only few words;
1. Agree
2. Agree
3. Agree
4. Agree
5. Agree

;)

Long version:
1. LFP for ACC and float version should be in main branch it really make difference.
2. Just agree. Gyro is really better sensor than ACC in copters and we can calculate angle using gyro corrected a little with ACC to find flat position.
3. Most gyro have LFP and should be used
4. I always set D about 50-60 for Pitch and Roll, and I really satisfied. no oscilations etc...
5. I think huge i is danger for most new users. They always start by increasing throttle very slowly, so you probably know what happened... I is only useful when someone have unbalanced quad but most users don't need this.

So i thing it is good way to solve some users problem. i have ADXL345 in one of my board and it flies without any problem with settings above for many months.

regards,
Marcin.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

I just finish tests with a small and a big drone equiped with CRIUS LITE and CRIUS SE. There is really a big difference even with default PID.
I think Alex and other developpers have to try it

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

Re: Bug in the code of the ADXL345

Post by timecop »

I also added these same changes to STM32 port, and several people already reported improvement in stable mode.
I put several packs through my test quad with increased gyro influence / heavier filtered ACC, and it was the first time I kept level on during entire flights, it did not fight back and generally behaved very well and allowed me to do some piros and nose-in hovering! :D

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Bug in the code of the ADXL345

Post by PatrikE »

I have made some tests.
using #define ACC_LPF_FACTOR 100
Level ( 5.0 | 0.01 | 43)

QUADX MONGOOSE1_0 ( ADXL345 )
Works much better when using /8.

Mahowik mod.
BIG difference.
But it will still build up a drift after 5 sec.
If i let it drift free it will start drift in another direction soon.
It keeps within 50x50 meters but drifs slowly around like drunken sailor.
--------------------------------------------------------------------------------------

VTAIL4 With ATAVRSBIN1 (BMA020)
Mahowik mod.
And no backlash.
Rocksteady after some Acctrim with TX.
Can only see some drift from vind.

I will do some more tests and increase ACC_LPF_FACTOR

Great mod!.. :lol:
/Patrik

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Bug in the code of the ADXL345

Post by Bledi »

good news PatrikE !

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Bug in the code of the ADXL345

Post by mahowik »

Actually we can't avoid a little drift because there are a lot of factors like: temperature influence to gyro and acc, ESC; not ideal sensors calibrations; accumulated errors in calculations/integrators etc.
If you you wanna have ideal horizontal positioning, you need to use optical flow sensor (or GPS) with solution like here viewtopic.php?f=7&t=1413

p.s. Alexmos provided great workable solution for this. I have already bought ADNS-5050 sensors and going to try ;)

PatrikE
Posts: 1976
Joined: Tue Apr 12, 2011 6:35 pm
Location: Sweden
Contact:

Re: Bug in the code of the ADXL345

Post by PatrikE »

I added Shikras driftkiller.
It set errorAngleI to zero if errorangels diffs.

Code: Select all

 errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);
   #if SHIKRA
   if (errorAngle > 0 && errorAngleI[axis]<0 ) errorAngleI[axis] = 0;
   if (errorAngle < 0 && errorAngleI[axis]>0 ) errorAngleI[axis] = 0;
  #endif
 

It removed almost all drift from with ADXL345.

I can actually fly in Lvelmode now... 8-)

Post Reply