To alex: ACC drift Issue: a bench test:

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi Ziss-dm,

All my measurements are made with my tricopter at rest on the ground, motors at idle (3900rpm).
Would you detail how you get 153W.
On my vibration abacus for a sine wave with frequency 750Hz and 2G max acceleration I get a maximum velocity of 4.3 mm/s and a peak to peak amplitude of 0.018 mm.
On your test with ADXL345, I suggest to do your measurements with range +/-8G then +/-4G and +/-2G. The results should be identical except if you have a saturation of the ADXL345 A to D converter.

Regards,
Phebus

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebus,

You right, shame on me. :oops:

Code: Select all

  m*a*s/t = 0.1*2*9.8*0.000018*750 =~ 0.026W 


In this case it is probably possible.. So, 8g range or foam tape? ;)

regards,
ziss_dm

phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi Ziss-dm,

I have tried foam tape with little results. Because of the low weight of the electronic board, the foam tape needs to be very soft to get a low resonant frequency to filter the vibrations. Then the board wobbles at the resonant frequency and you get more noise on the gyros. Best results were obtained with the electronic board bolted on the frame, with the accelerometer range selected to avoid saturation of the A to D converter by the high frequency motor vibrations and the accelerometer digital filter set to 20Hz.

Regards,

Phebus

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

I have a different setup,
I have a tower of bobs that is fixed with vibration dampeners, also my gopro is mounted to this tower, so the mass of the whole dampened system is very high. This helps.

If you are willing to go deep into this topic you can try replace your alu arms (if you have them) with wooden arms, these kill vibrations. Also mounting the motors on montor mounts or dampeners might help.
I have selfmade motor mounts attached to the arms and this helps killing some vibrations.

Nils

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi,

ADXL345: 1600bw, autorange, 1g=256
Motor: DT750
Prop: APC 10x4.7
No LPF, GUI displaying raw ACC values.
Attachments
adxl345-1600-idle.PNG

phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi Ziss-dm,

On your recording, what is the maximum output data rate of the ADXL345, what is the unit for the cycle time, what is the maximum output data rate to the GUI ?

Regards,

Phebus

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebus,

All around 2ms. (in theory)

regards,
ziss_dm

phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi Ziss-dm,

The ADXL345 data sheet Rev C indicates that due to i2C communication speed limitations, the maximum output data rate when using 400 kHz I2C is 800 Hz (page18) corresponding to a bandwidth of 400Hz (table 7 page 14).
Applying to the ATmega 328 microcontroller with 2mS cycle time or 500Hz the Nyquist-Shannon sampling theorem that shows that a band limited analog signal that has been sampled can be perfectly reconstructed from an infinite sequence of samples if the sampling rate exceeds 2B samples per second, where B is the highest frequency of the original signal, we get a bandwidth of 250Hz.
The RS232 to USB serial link with the PC running the standard GUI has to transmit approximately 102 bytes for each sequence of data. Each byte of data (8 bits) is sent with two framing bits which gives a total of 1020 bits to be sent. With the speed of the serial interface set to 115200 bit/s we get a sampling rate of 113Hz. Applying the Nyquist-Shannon theorem gives a bandwidth of 56 Hz. This means than on the standard GUI any frequency higher than 56Hz cannot be fully reconstructed.
I f for your test you have modified the GUI to limit the number of data transmitted to the 3 raw ACC value, you get a sampling rate of data of 960Hz and a bandwidth of 480Hz.
Because of the above bandwidth limitations (400Hz, 250Hz, 56Hz or 480Hz), it is not possible to see on the GUI the parasitic accelerations due to brushless motor cogging which have a frequency of 780Hz for a motor running in idle at 3900rpm.

Regards,

Phebus

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebus,

You right, we cannot see nice sinusoidal waves. ;) What we can see, is highly aliased noise. But we should be able to roughly estimate amplitude.
Anyway I have managed to read ADXL345 with 3.2K rate (slightly over-clocked i2c) and pass through FFT.

regards,
ziss_dm
Attachments
vib-6000-rpm.PNG
vib-4000-rpm.PNG
vib-3500-rpm.PNG
vib-3000-rpm.PNG
vib-1000-rpm.PNG
vib-0000-rpm.PNG
Last edited by ziss_dm on Tue Nov 29, 2011 11:17 am, edited 1 time in total.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi,
Same test with BMA180 (bw:1.2k mode:0).
So, looks like spike around 900hz is more related ADXL sensor.

regards,
ziss_dm
Attachments
vib-bma180-4300-rpm.PNG

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebus,

Is this close to your results? This is ESC with one of the power stage FETs not working.
So, it is vibrating at Frpm*14.

regards,
ziss_dm
Attachments
vib-3200-rpm-no_phase.PNG

phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi, Ziss_dm

To do a spectrum analysis of brushless motor vibrations on my Tricopter, I have made a new simple test setup that can easily be reproduced.

Equipment required:

-1 PC running Windows XP,
-Nunchuk clone with pcb cut around the ADXL335 3-axis analog accelerometer, wires connected to Gnd, Vcc (3.3V) and Y axis, 100nF filter capacitor on Y output (Cy) replaced by 3.3nF to get 1.6Khz bandwidth at 3db attenuation. Z output is not used because its bandwidth is limited to 500Hz,
-Zelscope software (http://www.zelscope.com/download.html, free 14 days evaluation period). This software uses the computer's sound card as analog-to-digital converter to present a real-time waveform or spectrum of the input signal. Software input calibrated using a +/- 0.5V to +/- 1.0V sine wave or square wave generator connected to the line in jack of the PC through a 32k resistor equal to the ouput impedance of the ADXL335,

Test setup:

-Tricopter fitted with RC Timer BL-2830/11 brushless motors, 9x5 propellers (GWS clone from Hobby King balanced) and sitting on the ground,
-Brushless motors running at idle (around 3900rpm) or at take off rpm (approximately 6000 rpm, throttle at mid travel),
-ADXL 335 accelerometer glued on the electronic board or on the beam supporting the running brushless motor, fitted as close as possible to this motor and with Y axis vertical, output sensitivity : 0.33V/G, max output voltage : +/- 1.5V, max acceleration : +/- 4.5G,
-Y output of the accelerometer connected to the line in jack of the PC,

Results:

1-Accelerometer glued on the electronic board ,all motors running at idle,
11-Acceleration, all motors on at idle_1.jpg
(29.02 KiB) Not downloaded yet

12-FFT, all motors on at idle_1.jpg
(25.34 KiB) Not downloaded yet

2-Accelerometer glued on the electronic board, all motors running at take off rpm,

14-FFT,all motors on at take off rpm_1.jpg
(25.97 KiB) Not downloaded yet

3-Accelerometer glued as close as possible to front right motor running at idle, other motors stopped,

16-FFT, front right motor at idle rpm_1.jpg
(25.41 KiB) Not downloaded yet

4-Accelerometer glued as close as possible to front right motor running at take off rpm, other motors stopped,

18-FFT, front right motor at take off rpm_1.jpg
(24.71 KiB) Not downloaded yet

5-Accelerometer glued as close as possible to front left motor running at idle, other motors stopped,

20-FFT, front left motor at idle rpm_1.jpg
(26 KiB) Not downloaded yet

6-Accelerometer glued as close as possible to front left motor running at take off rpm, other motors stopped,

22-FFT, front left motor at take off rpm_1.jpg
(25.04 KiB) Not downloaded yet


Comments:

When right or left motors are running at idle rpm, the stiffness of quadricopter legs add some resonance and the spectrum analysis is not as clean as when motors are running at take off rpm (legs floating). All spectrum recordings show that the main contributor to motor vibrations is brushless motor cogging.
Maximum acceleration recorded is equal to ADXL 335 output voltage saturation of 4.5G + 1G = 5.5G. This correlates with my BMA020 input range setting of +/- 8G in order to have no variations of Z acceleration with motors on and to have no more drift in level mode.The decrease up to zero of the Z acceleration with a high frequency vibration occurs when the A to D converter of the accelerometer is fully saturated with + and - accelerations, then its mean output value is zero instead of 1G.
This setup can be used to verify the impact on the vibration level of propeller balancing, vibration dampener, motor arm material, motor change etc…

Regards,

Phebus

User avatar
mgros
Posts: 90
Joined: Thu Jan 20, 2011 12:32 am

Re: To alex: ACC drift Issue: a bench test:

Post by mgros »

Fantastic job!!!

we need a dynamic equilibration system

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebus,

Wold you be interested to run FFT on your BMA, to compare results? ;)

Some interesting readings:
http://archives.sensorsmag.com/articles ... main.shtml
http://www.helitune.com/application_cap ... r_overload

regards,
ziss_dm
Attachments
VibAnalyser.zip
(33.75 KiB) Downloaded 624 times

phebus
Posts: 13
Joined: Mon Nov 07, 2011 12:15 am

Re: To alex: ACC drift Issue: a bench test:

Post by phebus »

Hi Ziss_dm,

I am interested to run FFT on my BMA020 but I am not a specialist of C language. I f you can supply the software required, I will be happy to run the FFT.

Regards,

Phebus

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Phebius,

It is attached to the previous post. It has arduino sketch. And processing visualizer.
You also can connect RPM encoder to the PPM_SUM pin.
I will write up short instruction later.

regards,
ziss_dm

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

Re: To alex: ACC drift Issue: a bench test:

Post by Hamburger »

phebus, Ziss,
Would you care to explain the consequences of yur findings, please? While I understand the basics of your test setups and FFT, I have no clues as to what you would derive from that.
Thankks

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi Hamburger,

Problem:
ACC data is biased when motors running.

Theory:
Sensor (or ADC) is overloaded with high frequency vibrations.

ToDo:
1) Confirm theory (FFT should show "ski slope" http://www.helitune.com/application_cap ... r_overload in 2g mode)
2) Find source of this vibrations (in 8g mode), it could be:
- frame resonances (big spike on some RPM)
- motor cogging (the spike should be always at Frpm*12)
- motor commutation (the spike should be always at Frpm*7*6)
- bearings (???)
- other
3) Invent strategy to avoid overloading and preserve resolution
- mechanical filtering
- tweaking BMA settings
- oversampling
- etc..

This is initialization for BMA:

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
//  control = control | (0x00 << 3); //Range 2G
  control = control | (0x03 << 3); //Range 8G
  control = control | 0x06;        //Bandwidth 1500 Hz
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 255;
}


regards,
ziss_dm

didlawowo69
Posts: 38
Joined: Tue Oct 11, 2011 1:42 pm

Re: To alex: ACC drift Issue: a bench test:

Post by didlawowo69 »

very interesting analysis, but is there a chance that BMA180 is concerned by this topic ? or just BMA020 and other ADX ?

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

Re: To alex: ACC drift Issue: a bench test:

Post by Hamburger »

ziss,
thanks for that explanation.
Now I can admire the work you an phebus are doing even more.
Good luck!

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

ziss_dm wrote:This is initialization for BMA:

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
//  control = control | (0x00 << 3); //Range 2G
  control = control | (0x03 << 3); //Range 8G
  control = control | 0x06;        //Bandwidth 1500 Hz
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 255;
}


Hi Ziss_dm,

from datasheet:
Settings of full scale range register
range<1:0> Full scale acceleration range
00 +/- 2g
01 +/- 4g
10 +/- 8g
11 Not authorised code

i.e. it should be:

Code: Select all

control = control | (0x02 << 3); //Range 8G


p.s. I have compiled and run VibAnalyser with processing UI. Good job! Is it your?!
I will do the tests with bma020 this week in case of free time...

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi,
You righ! And also it is good idea to adjust acc_1g (it is used for graph scaling).

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
//  control = control | (0x00 << 3); //Range 2G
  control = control | (0x02 << 3); //Range 8G
  control = control | 0x06;        //Bandwidth 1500 Hz
  i2c_writeReg(0x70,0x14,control);
//  acc_1G = 255;
  acc_1G = 63; //Range 8G
}


Is it your?!

Not really.. ;) FFT library from here: http://code.google.com/p/neuroelec/downloads/list. The rest is just MWC code...

regards,
ziss_dm

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

Hi Guys,

you in depth tests are awesome but way over my financial and knowledege possibilities.

I do not understand everything but inspired by your work I did the following:

I own a BMA020 and the AccZ goes down from 255 to 140 when I throttle up on ground. I use it on the flydusense board so it is not dampended :-(
My conclusion of my first tests was it either could be an electrical or an mechanical problem.

Today I unscrewed the whole sensor tower and held it in my hand near the copter as I throttled up with props mounted, the accZ was stable around 255 (despite my shacky hands ;-) )

So the conclusion for me is: VIBRATIONS. This would be the same conclusion you discussed.

But what is the solutioon for that? Should I mount the sensor on Gyro tape, should I buy myself a BMA180 or is there a WORKING software solution?

I would like to have the electrical system and the software as optimized as possible becaues my frame is not very perfect due to my unskilled working.

Can you give a conclusion for the openminded but simple minded people!?

Thank you for that and more importtant: thank you guysfor your excellent work and the findings.!!!


Nils

KeesvR
Posts: 194
Joined: Fri May 27, 2011 6:51 pm
Location: The Netherlands

Re: To alex: ACC drift Issue: a bench test:

Post by KeesvR »

I did the same test with this sensor some time ago and came to the same conclusion.

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

jevermeister wrote:So the conclusion for me is: VIBRATIONS. This would be the same conclusion you discussed.

But what is the solutioon for that?


Hi jevermeister,

As for me set range to 8g fix this issue. I still have a little drift (I think it because there is low ACC resolution 64 LSB/g for 8g range) but it's acceptable...

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
  //control = control | (0x00 << 3); //Range 2G 00
  //control = control | (0x01 << 3); //Range 4G 01
  control = control | (0x02 << 3); //Range 8G 02
  control = control | 0x00;        //Bandwidth 25 Hz 000
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 127;
}

#define DELIMITER 32

void ACC_getADC(){
  TWBR = ((16000000L / 400000L) - 16) / 2;
  i2c_getSixRawADC(0x70,0x02);
  ACC_ORIENTATION(    ((rawADC[1]<<8) | rawADC[0])/DELIMITER ,
                      ((rawADC[3]<<8) | rawADC[2])/DELIMITER ,
                      ((rawADC[5]<<8) | rawADC[4])/DELIMITER );
  ACC_Common();
}
#endif


And of course with this Range you can TRUST ACC Z :)

Code: Select all

/* This option should be uncommented if ACC Z is accurate enough when motors are running*/
#define TRUSTED_ACCZ


thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi mahowik,

The devisor /64 was used to mask 6 (5+1bit "new_data") "unused" bits in data registers. With divisor /32 you relaying on the contents of those bits, which is probably not a good idea as you never know what can appear there. ;)

regards,
ziss_dm

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

ziss_dm wrote:The devisor /64 was used to mask 6 (5+1bit "new_data") "unused" bits in data registers. With divisor /32 you relaying on the contents of those bits, which is probably not a good idea as you never know what can appear there. ;)


Hi ziss_dm,

Yes, I know that for 8g the resolution is 64Lsb/g... I checked before, that unused bit always zero...
It was just an attempt to increase the resolution by this way + LPF with high accuracy...

thx-
Alex

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

Hi, so what will be the best solution for me!?

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: To alex: ACC drift Issue: a bench test

Post by EOSBandi »

This works for me with bma020, but what about the bma180, is it worth to change ? Is it also affected by this range overload issue?

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

Re: To alex: ACC drift Issue: a bench test

Post by Alexinparis »

EOSBandi wrote:This works for me with bma020, but what about the bma180, is it worth to change ? Is it also affected by this range overload issue?

Hi,

I think it is also.
There was an empirical suggestion to modify the mode_config to an undocumented setting, but I don't think it's the best solution.

Here is a code suggestion for the BMA180 (which works very well for my setups):

Code: Select all

#if defined(BMA180)
void ACC_init () {
  delay(10);
  //default range 2G: 1G = 4096 unit.
  i2c_writeReg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0  -- value: set bit ee_w to 1 to enable writing
  delay(5);
  uint8_t control = i2c_readReg(BMA180_ADDRESS, 0x20);
  control = control & 0x0F;        // save tcs register
  control = control | (0x00 << 4); // set low pass filter to 10Hz (bits value = 0000xxxx)
  i2c_writeReg(BMA180_ADDRESS, 0x20, control);
  delay(5);
  control = i2c_readReg(BMA180_ADDRESS, 0x30);
  control = control & 0xFC;        // save tco_z register
  control = control | 0x00;        // set mode_config to 0
  i2c_writeReg(BMA180_ADDRESS, 0x30, control);
  delay(5);
  control = i2c_readReg(BMA180_ADDRESS, 0x35);
  control = control & 0xF1;        // save offset_x and smp_skip register
  control = control | (0x05 << 1); // set range to 8G
  i2c_writeReg(BMA180_ADDRESS, 0x35, control);
  delay(5);
  acc_1G = 255;
}

void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2;  // Optional line.  Sensor is good for it in the spec.
  i2c_getSixRawADC(BMA180_ADDRESS,0x02);
  //usefull info is on the 14 bits  [2-15] bits  /4 => [0-13] bits  /4 => 12 bit resolution
  ACC_ORIENTATION(  - ((rawADC[1]<<8) | rawADC[0])/16 ,
                    - ((rawADC[3]<<8) | rawADC[2])/16 ,
                      ((rawADC[5]<<8) | rawADC[4])/16 );
  ACC_Common();
}
#endif

ruicam
Posts: 1
Joined: Fri Dec 30, 2011 3:42 am

Re: To alex: ACC drift Issue: a bench test:

Post by ruicam »

Great!! This piece of code save my quad with the V4 board! No drift!

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

@Alex, what does this change do!?

I ordered myself a BMA180 to replace my BMA020, I am sad to hear, that it has the same AccZ problems.

Given the fact that my MAG Problems may not be solveable in near future I want at least a working Baro Mode.

Nils

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by EOSBandi »

jevermeister wrote:@Alex, what does this change do!?

I ordered myself a BMA180 to replace my BMA020, I am sad to hear, that it has the same AccZ problems.

Given the fact that my MAG Problems may not be solveable in near future I want at least a working Baro Mode.

Nils


It seems that drift issues are mostly caused by overloading the accelerometer, so the quick fix is to increase the accelerometers range to 8G instead of 2G.
Go ahead and try the code mentioned above. I can fly pretty well with 8G setting and BMA150 and BMA020.
Bandi

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

I saw in the trunk, that alex also applied this code to the bma020, I once tested this solution but I heard that this changes the snesitivity so it is not that exact anymore.

Nils

capt
Posts: 54
Joined: Wed Jan 19, 2011 10:54 pm

Re: To alex: ACC drift Issue: a bench test:

Post by capt »

I have made this change in code 1.8p2 for my BMA020 and it loads and run fine but ACC Z now shows 61-63 in the GUI, is this OK? I have not test flown it yet.

#if defined(BMA020)
void ACC_init(){
i2c_writeReg(0x70,0x15,0x80);
uint8_t control = i2c_readReg(0x70, 0x14);
control = control & 0xE0;
// control = control | (0x00 << 3); //Range 2G
control = control | (0x02 << 3); //Range 8G
control = control | 0x06; //Bandwidth 1500 Hz
i2c_writeReg(0x70,0x14,control);
// acc_1G = 255;
acc_1G = 63; //Range 8G
}
Attachments
ACC Z.JPG

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by EOSBandi »

capt wrote:I have made this change in code 1.8p2 for my BMA020 and it loads and run fine but ACC Z now shows 61-63 in the GUI, is this OK? I have not test flown it yet.

#if defined(BMA020)
void ACC_init(){
i2c_writeReg(0x70,0x15,0x80);
uint8_t control = i2c_readReg(0x70, 0x14);
control = control & 0xE0;
// control = control | (0x00 << 3); //Range 2G
control = control | (0x02 << 3); //Range 8G
control = control | 0x06; //Bandwidth 1500 Hz
i2c_writeReg(0x70,0x14,control);
// acc_1G = 255;
acc_1G = 63; //Range 8G
}


Since 1G is now 63, then 61-62 is OK, when copter is sitting flat....

capt
Posts: 54
Joined: Wed Jan 19, 2011 10:54 pm

Re: To alex: ACC drift Issue: a bench test:

Post by capt »

OK thanks, I flew it and see the difference, still dips now and then but seems to recover better. Since I am running sensors (WM+ BMA020) on 3.3v the next change I think will be 5v and adding the 2k2 between sda/slc then loading 1.9.

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

jevermeister wrote:I saw in the trunk, that alex also applied this code to the bma020, I once tested this solution but I heard that this changes the snesitivity so it is not that exact anymore.


Hi guys!

It is nice to have anti-vibro solution by increasing the ACC range to +/-8..16g, but in case of bma020 it has low precision, only 64 lsb/g.
So I have tried a lot of solutions (diff LPF, oversampling etc) and done too many experiments to increase the ACC resolution/precision to keep it stable in level mode.
And it seems I found one very-very stable solution... :)

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80);
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
  //control = control | (0x00 << 3); //Range 2G 00
  //control = control | (0x01 << 3); //Range 4G 01
  control = control | (0x02 << 3); //Range 8G 10
  control = control | 0x00;        //Bandwidth 25 Hz 000
  //control = control | 0x02;        //Bandwidth 100 Hz 010
  //control = control | 0x03;        //Bandwidth 190 Hz 011
  //control = control | 0x04;        //Bandwidth 375 Hz 100
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 255;
}

#define DELIMITER 64

void ACC_getADC(){
  TWBR = ((16000000L / 400000L) - 16) / 2;
  i2c_getSixRawADC(0x70,0x02);
  ACC_ORIENTATION(    (((rawADC[1]<<8) | rawADC[0])/DELIMITER)*4 ,
                      (((rawADC[3]<<8) | rawADC[2])/DELIMITER)*4 ,
                      (((rawADC[5]<<8) | rawADC[4])/DELIMITER)*4 );
  ACC_Common();
}
#endif


Here we keep the acc_1G = 255 and using the float LPF (about 2-3hz for cycle time 3000us) in IMU to increase precision

Code: Select all

void getEstimatedAttitude(){
.......................
static float accTempF[3];
.......................

      //accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>4)) + accADC[axis];
      //accSmooth[axis] = accTemp[axis]>>4;
     
      #define lfpFactor 0.985f
      //lfpFactor = 0.9f + constrain(rcData[AUX2]-1000, 0, 990)/10000.0f; // k=0..0.99
      accTempF[axis] = accTempF[axis]*lfpFactor + accADC[axis]*(1.0f - lfpFactor);
      accSmooth[axis] = accTempF[axis];
.......................


Also you just need to slight increase the GYR_CMPF_FACTOR... 340 is good

Code: Select all

#define GYR_CMPF_FACTOR 340.0f


It would be nice to hear that is works not only for me ;)

thx-
Alex

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

Re: To alex: ACC drift Issue: a bench test

Post by mahowik »

Alexinparis wrote:
EOSBandi wrote:This works for me with bma020, but what about the bma180, is it worth to change ? Is it also affected by this range overload issue?

Hi,

I think it is also.
There was an empirical suggestion to modify the mode_config to an undocumented setting, but I don't think it's the best solution.

Here is a code suggestion for the BMA180 (which works very well for my setups):

Code: Select all

#if defined(BMA180)
void ACC_init () {
  delay(10);
  //default range 2G: 1G = 4096 unit.
  i2c_writeReg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0  -- value: set bit ee_w to 1 to enable writing
  delay(5);
  uint8_t control = i2c_readReg(BMA180_ADDRESS, 0x20);
  control = control & 0x0F;        // save tcs register
  control = control | (0x00 << 4); // set low pass filter to 10Hz (bits value = 0000xxxx)
  i2c_writeReg(BMA180_ADDRESS, 0x20, control);
  delay(5);
  control = i2c_readReg(BMA180_ADDRESS, 0x30);
  control = control & 0xFC;        // save tco_z register
  control = control | 0x00;        // set mode_config to 0
  i2c_writeReg(BMA180_ADDRESS, 0x30, control);
  delay(5);
  control = i2c_readReg(BMA180_ADDRESS, 0x35);
  control = control & 0xF1;        // save offset_x and smp_skip register
  control = control | (0x05 << 1); // set range to 8G
  i2c_writeReg(BMA180_ADDRESS, 0x35, control);
  delay(5);
  acc_1G = 255;
}

void ACC_getADC () {
  TWBR = ((16000000L / 400000L) - 16) / 2;  // Optional line.  Sensor is good for it in the spec.
  i2c_getSixRawADC(BMA180_ADDRESS,0x02);
  //usefull info is on the 14 bits  [2-15] bits  /4 => [0-13] bits  /4 => 12 bit resolution
  ACC_ORIENTATION(  - ((rawADC[1]<<8) | rawADC[0])/16 ,
                    - ((rawADC[3]<<8) | rawADC[2])/16 ,
                      ((rawADC[5]<<8) | rawADC[4])/16 );
  ACC_Common();
}
#endif


Hi Alex,

I would like to propose some changes for bma180:
1. I have found out that integrator for alt-hold works much better and gives more precision when the ACC bw ~= integrator frequency (I played and checked this with bma020 bw)

Code: Select all

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)

So I propose to set the bw for bma180 to 20hz also

Code: Select all

control = i2c_readReg(BMA180_ADDRESS, 0x20);
  control = control & 0x0F; // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 10Hz (bits value = 0000xxxx)
  control = control | (0x01<<4); // bw 20hz
  i2c_writeReg(BMA180_ADDRESS, 0x20, control);
  delay(5);

2. It has 1024 lsb/g for +/-8g range, so probably it make sense to keep acc_1G = 512 and decrease the data divider to 8 accordingly?

3. Also one more change for ADXL345. As for now it has sample rate 200hz (with bw = 100hz accordingly).
So it make sense to reduce sample rate to 50hz (bw = 25hz) as for the stab/level mode and also for alt-hold integrator...

Code: Select all

//i2c_writeReg(ADXL345_ADDRESS,0x2C,8+2+1);
i2c_writeReg(ADXL345_ADDRESS,0x2C,0x09); // rate=50hz, bw=20hz (see table 8 of the spec)


thx-
Alex

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

Re: To alex: ACC drift Issue: a bench test

Post by Alexinparis »

Hi,

mahowik wrote:1. I have found out that integrator for alt-hold works much better and gives more precision when the ACC bw ~= integrator frequency (I played and checked this with bma020 bw)

Code: Select all

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)

So I propose to set the bw for bma180 to 20hz also

Code: Select all

control = i2c_readReg(BMA180_ADDRESS, 0x20);
  control = control & 0x0F; // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 10Hz (bits value = 0000xxxx)
  control = control | (0x01<<4); // bw 20hz
  i2c_writeReg(BMA180_ADDRESS, 0x20, control);
  delay(5);


everything that can improve alt hold is good to take :)
could you explain the reason why it works better with another bw setting ? and how work the bw and LP inside the chip ?

2. It has 1024 lsb/g for +/-8g range, so probably it make sense to keep acc_1G = 512 and decrease the data divider to 8 accordingly?

it's true, but with a 8G setting, 1G=512 seems a little bit noisy from gui point of view.
that's why I set 1G to 256, which is still a very good resolution for what we do.

3. Also one more change for ADXL345. As for now it has sample rate 200hz (with bw = 100hz accordingly).
So it make sense to reduce sample rate to 50hz (bw = 25hz) as for the stab/level mode and also for alt-hold integrator...

Code: Select all

//i2c_writeReg(ADXL345_ADDRESS,0x2C,8+2+1);
i2c_writeReg(ADXL345_ADDRESS,0x2C,0x09); // rate=50hz, bw=20hz (see table 8 of the spec)


ok, we don't have a lot of feedback about this ACC.
has it also the vibration saturation problem with its G setting ?

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: To alex: ACC drift Issue: a bench test:

Post by jevermeister »

Hi,
I am using a BMA020 right now.
I applied that 8G solution alex introduced in the latest trunk and altitude hold works good now (+/-50cm oscillation but I am very satisfied) and there is no significant drift while using trusted accZ.

I ordered a BMA180 last week and it arrived friday and I want to know if it is worth to use it instead of the bma020 if the 8G fix is applied.

What is the difference in the behaviour with applied 8G fix. If I understand the last posts right it is mor precise. Correct? What does that mean inflight?

I use a Flydusense wich is built for holding the bma020 and using the bma180 would lead to new connections and a custom mount.

One more thing:
The BMA020 on the Flydusense is not dampened with foam tape, is it wise to dampen an ACC like a Gyro? I mounted the whole Flydusense and Flyduino on dampeners.

The photo shows the dampening
Image

thanks and sorry for the n00b questions...
Nils

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi,

Just a small note. Floating point LPF has the following advantages:
1) It is increasing precision due oversampling. It looks like we are easily regaining 2 lost bits.
2) It is not overflowing, so with BMA180 we can use full 14 bit resolution
3) We can have much lower cut-off frequencies.

With this notation:

Code: Select all

static float accLPF[3];
#define lfpFactor (1.0f/ACC_LPF_FACTOR)

accLPF[axis] = accLPF[axis] * (1.0f - lfpFactor) + accADC[axis] * lfpFactor;


@mahowic: why you using "inverted" notation? ;)

I'm getting really good results with ACC_LPF_FACTOR = GYR_CMPF_FACTOR / 4.
Such low cut-of freq for pre-filter surprisingly have lot of advantages ;)
- Significantly increased stability in horizontal plane. The filter just not reacting on short side accelerations.
- We can remove "acceptable acceleration" range as time constant for the filter is quite long

This is code I'm using:

Code: Select all

// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://wbb.multiwii.com/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Last Modified: 19/04/2011
// Version: V1.1   
// **************************************************

//******  advanced users settings *******************
/* Set the Low Pass Filter factor for ACC */
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
/* Comment this if  you do not want filter at all.*/
/* Default WMC value: 8*/
#define ACC_LPF_FACTOR 150

/* Set the Low Pass Filter factor for Magnetometer */
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
/* Comment this if  you do not want filter at all.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 8

/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#define GYR_CMPF_FACTOR 600.0f

/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 300.0f

//****** end of advanced users settings *************

#define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))
#define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#if GYRO
  #define GYRO_SCALE ((2000.0f * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.155f) 
  // +-2000/sec deg scale
  //#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)     
  // +- 200/sec deg scale
  // 1.5 is empirical, not sure what it means
  // should be in rad/sec
#else
  #define GYRO_SCALE (1.0f/200e6f)
  // empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
  // !!!!should be adjusted to the rad/sec
#endif
// Small angle approximation
#define ssin(val) (val)
#define scos(val) 1.0f

typedef struct {
  float X;
  float Y;
  float Z;
} t_fp_vector_def;

typedef union {
  float   A[3];
  t_fp_vector_def V;
} t_fp_vector;


int16_t _atan2(float y, float x){
  #define fp_is_neg(val) ((((byte*)&val)[3] & 0x80) != 0)
  float z = y / x;
  int16_t zi = abs(int16_t(z * 100));
  int8_t y_neg = fp_is_neg(y);
  if ( zi < 100 ){
    if (zi > 10)
     z = z / (1.0f + 0.28f * z * z);
   if (fp_is_neg(x)) {
     if (y_neg) z -= PI;
     else z += PI;
   }
  } else {
   z = (PI / 2.0f) - z / (z * z + 0.28f);
   if (y_neg) z -= PI;
  }
  z *= (180.0f / PI * 10);
  return z;
}

static float accLPF[3];
#define lfpFactor (1.0f/ACC_LPF_FACTOR)

void getEstimatedAttitude(){
  uint8_t axis;
  static t_fp_vector GEstG = {0,0,200};
  t_fp_vector EstG = GEstG;
  static t_fp_vector EstM = {10,10,200};
  float deltaGyroAngle;
  static uint16_t PreviousTime;
  static int16_t mgSmooth[3];  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
  uint16_t CurrentTime  = micros();
  float deltaTime = (CurrentTime - PreviousTime) * GYRO_SCALE;
  PreviousTime = CurrentTime;
  // Initialization
  for (axis = 0; axis < 3; axis++) {
    #if defined(ACC_LPF_FACTOR)
      // LPF for ACC values
      accLPF[axis] = accLPF[axis] * (1.0f - lfpFactor) + accADC[axis] * lfpFactor;
      accSmooth[axis] = accLPF[axis];
    #else 
      accLPF[axis] = accADC[axis];
      accSmooth[axis] = accADC[axis];
    #endif
    #if MAG
      #if defined(MG_LPF_FACTOR)
        // LPF for Magnetometer values
        mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;
        #define MAG_VALUE mgSmooth[axis]
      #else 
        #define MAG_VALUE magADC[axis]
      #endif
    #endif
  }
  // Rotate Estimated vector(s), ROLL
  deltaGyroAngle  = gyroADC[ROLL] * deltaTime;
  EstG.V.Z =  scos(deltaGyroAngle) * EstG.V.Z - ssin(deltaGyroAngle) * EstG.V.X;
  EstG.V.X =  ssin(deltaGyroAngle) * EstG.V.Z + scos(deltaGyroAngle) * EstG.V.X;
  #if MAG
    EstM.V.Z =  scos(deltaGyroAngle) * EstM.V.Z - ssin(deltaGyroAngle) * EstM.V.X;
    EstM.V.X =  ssin(deltaGyroAngle) * EstM.V.Z + scos(deltaGyroAngle) * EstM.V.X;
  #endif
  // Rotate Estimated vector(s), PITCH
  deltaGyroAngle  = gyroADC[PITCH] * deltaTime;
  EstG.V.Y =  scos(deltaGyroAngle) * EstG.V.Y + ssin(deltaGyroAngle) * EstG.V.Z;
  EstG.V.Z = -ssin(deltaGyroAngle) * EstG.V.Y + scos(deltaGyroAngle) * EstG.V.Z;
  #if MAG
    EstM.V.Y =  scos(deltaGyroAngle) * EstM.V.Y + ssin(deltaGyroAngle) * EstM.V.Z;
    EstM.V.Z = -ssin(deltaGyroAngle) * EstM.V.Y + scos(deltaGyroAngle) * EstM.V.Z;
  #endif
  // Rotate Estimated vector(s), YAW
  deltaGyroAngle  = gyroADC[YAW] * deltaTime;
  EstG.V.X =  scos(deltaGyroAngle) * EstG.V.X - ssin(deltaGyroAngle) * EstG.V.Y;
  EstG.V.Y =  ssin(deltaGyroAngle) * EstG.V.X + scos(deltaGyroAngle) * EstG.V.Y;
  #if MAG
    EstM.V.X =  scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;
    EstM.V.Y =  ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;
  #endif
  // Apply complimentary filter (Gyro drift correction)
  for (axis = 0; axis < 3; axis++)
    EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accLPF[axis]) * INV_GYR_CMPF_FACTOR;
  // Attitude of the estimated vector
  angle[ROLL]  =  _atan2(EstG.V.X, EstG.V.Z) ;
  angle[PITCH] =  _atan2(EstG.V.Y, EstG.V.Z) ;
  GEstG = EstG;
  #if MAG
    // Apply complimentary filter (Gyro drift correction)
    for (axis = 0; axis < 3; axis++)
      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
    // Attitude of the cross product vector GxM
    heading = _atan2(EstG.V.Z * EstM.V.X - EstG.V.X * EstM.V.Z, EstG.V.Y * EstM.V.Z - EstG.V.Z * EstM.V.Y) / 10;
  #endif
}


That is right:
#define ACC_LPF_FACTOR 150
#define GYR_CMPF_FACTOR 600.0f

@Alex: I think, moving rotation to the procedure was not really good idea. As the overhead of the procedure call is actually higher than code reduction (at least with gyro+acc configuration)

regards,
ziss_dm

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

Re: To alex: ACC drift Issue: a bench test

Post by mahowik »

Alexinparis wrote:and how work the bw and LP inside the chip ?

How I understand this: bw = sample_rate/2, where each sample is averaged value of internal measurements for current dt, where dt = 1/f_cut

Alexinparis wrote:could you explain the reason why it works better with another bw setting ?

It means that alt-hold integrator has their own cycle frequency (or update rate) which requires the ACC data on input with the same frequency/sample rate (bandwith accordingly).
E.g. if integrator requires 40 samples per second but ACC setup has bw=100hz (200 samples... where each sample is averaged separately), the integrator will take (snatch out) random values and it will produce increasing of the integration error...
Or if ACC bandwith is low or less than integrator frequency, it will also produce the integration error. The precision of the compensation by ACC Z will be low, i.e. it will not see the small vertical movements..

@Ziss, pls. correct me if i'm wrong here?

So also probably it make sense to tune alt-hold integrator to 80hz update rate to increase the precision?

Alexinparis wrote:
2. It has 1024 lsb/g for +/-8g range, so probably it make sense to keep acc_1G = 512 and decrease the data divider to 8 accordingly?

it's true, but with a 8G setting, 1G=512 seems a little bit noisy from gui point of view.
that's why I set 1G to 256, which is still a very good resolution for what we do.

Have you tried the second "ultra low noise mode"? Probably it will solve the noise...

Code: Select all

control = i2c_readReg(BMA180_ADDRESS, 0x30);
  control = control & 0xFC;        // save tco_z register
  control = control | 0x01;        // set mode_config to 01
  i2c_writeReg(BMA180_ADDRESS, 0x30, control);


Alexinparis wrote:
3. Also one more change for ADXL345. As for now it has sample rate 200hz (with bw = 100hz accordingly).
So it make sense to reduce sample rate to 50hz (bw = 25hz) as for the stab/level mode and also for alt-hold integrator...

Code: Select all

//i2c_writeReg(ADXL345_ADDRESS,0x2C,8+2+1);
i2c_writeReg(ADXL345_ADDRESS,0x2C,0x09); // rate=50hz, bw=25hz (see table 8 of the spec)


ok, we don't have a lot of feedback about this ACC.
has it also the vibration saturation problem with its G setting ?

According to feedback from russian forum for +/-16g (currently set) it hasn't vibration saturation problem but it's not so stable with sample rate 200hz.
And it became more stable with less bw...

P.s. One more time reminder: float LPF it's cool! Don't ignore this ;)
I have posted alternative 1.9 sketch (with hight 8/16G range + float LPF) on russian rcdesign forum and have a good feedbacks http://forum.rcdesign.ru/blogs/83206/blog13328.html


thx-
Alex
Last edited by mahowik on Tue Jan 10, 2012 4:43 pm, edited 2 times in total.

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

ziss_dm wrote:@mahowic: why you using "inverted" notation? ;)

It doesn't matter :) Mathematically it's the same :)

ziss_dm wrote:That is right:
#define ACC_LPF_FACTOR 150
#define GYR_CMPF_FACTOR 600.0f

Hmm... I suppose with GYR_CMPF_FACTOR=600 we can just the turn off the power of the ACC ))

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi mahowik ,


Hmm... I suppose with GYR_CMPF_FACTOR=600 we can just the turn off the power of the ACC ))


Touché ;)

So also probably it make sense to tune alt-hold integrator to 80hz update rate to increase the precision?

In case acc bw is 40hz, it is better to run integrator at 80hz.

regards,
ziss_dm

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

Re: To alex: ACC drift Issue: a bench test:

Post by mahowik »

ziss_dm wrote:
Hmm... I suppose with GYR_CMPF_FACTOR=600 we can just the turn off the power of the ACC ))

Touché ;)

Hi Ziss,

Let me fully understand how CF works. CF has their own compensation response time. I mean time which is necessary to compensate gyro vector drift by acc vector when they are not in sync (e.g. after set of fast maneuvers). Also it means that acc LPF cut frequency should not be less than CF frequency (i.e. should be tuned according to CF compensation response time). This is why we are increasing the gyro factor (CF compensation response time grows accordingly) when decreasing acc LPF cut frequency... Correct?
So in case of float LPF with 0.985 (67 in your case) acc lpfFactor + 340 gyro factor + and cycle time 3000us the compensation response time of CF is about 2-4sec (e.g. when you are doing the set of very fast rotation movements of the sensors board in hands).
So I'm afraid to say what is the CF compesation response time will be with 150 and 600 factors? 10-20 sec? In this case we can lost a horizont line after set of rotations or flips. E.g. When you flight in acro and then switch to level mode...
Probably dynamic acc and gyro factors for CF? :) I.e. in case of big diff between gyro and acc vectors the CF factors would be decreased to reduce CF compensation response time...

ziss_dm wrote:
So also probably it make sense to tune alt-hold integrator to 80hz update rate to increase the precision?

In case acc bw is 40hz, it is better to run integrator at 80hz.


I have meant with increasing the alt-hold integrator update rate (and sync acc bw accordingly) we can get more precision with alt-hold?
  

thx-
Alex

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

Re: To alex: ACC drift Issue: a bench test:

Post by ziss_dm »

Hi,

Let me fully understand how CF works. CF has their own compensation response time. I mean time which is necessary to compensate gyro vector drift by acc vector when they are not in sync (e.g. after set of fast maneuvers). Also it means that acc LPF cut frequency should not be less than CF frequency (i.e. should be tuned according to CF compensation response time).

Yes and no..
CF is "mixing" low frequencies from ACC and high frequencies from gyro. So, if you switch off gyro rotations it would behave like classic LPF with step response couple of seconds. Together with gyro, it is tracking attitude quite preciselly and small differences are compensated much faster.

Also it means that acc LPF cut frequency should not be less than CF frequency (i.e. should be tuned according to CF compensation response time).

Correct. Not even close. To prevent oscillations they should be far from each other. You can see this in GUI.

So I'm afraid to say what is the CF compesation response time will be with 150 and 600 factors? 10-20 sec? In this case we can lost a horizont line after set of rotations or flips. E.g. When you flight in acro and then switch to level mode...

Yes step response would be 15-20 sec. But why it should "lost" horison? ;)
For fun I even turned it on during flip and looks like it still tracking really good.

regards,
ziss_dm

UPUP
Posts: 2
Joined: Wed Feb 22, 2012 3:44 pm

Re: To alex: ACC drift Issue: a bench test:

Post by UPUP »

Hi all
Can anyone help I have manage to convert my tri to multwii and it flew fine.

I then fitted a BMA 020 ACC board and it now will not fly This happens when I have the BMA wired but not turned on the settings seems to move about and it has become uncontrollable even in normal mode

If I try to fly in level mode it flips on take off as I apply power to the motors, it seems to get worse and worse

Strangely it all looks fine in the GUI it seems to calibrate and all the columns operate fine


So I took the BMA board off reloaded the sofware back to 1.7 and it flys fine again but I would love to get this ACC working any ideas

Any Help Please regards Jeff

ronco
Posts: 317
Joined: Thu Aug 18, 2011 2:58 pm

Re: To alex: ACC drift Issue: a bench test:

Post by ronco »

hi jeff,

you may look at this thread viewtopic.php?f=8&t=986

or maybe that helps
Image

regards

felix

UPUP
Posts: 2
Joined: Wed Feb 22, 2012 3:44 pm

Re: To alex: ACC drift Issue: a bench test:

Post by UPUP »

Hi Felix
Thanks for that I will reread the rc cam thread on the pull ups ( having a struggle understanding) issue.
Your wiring is different than mine I have power to pin twelve and no external resistors (what do they do :oops: )

Regards Jeff

Post Reply