Sensors.pde changes...

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Sensors.pde changes...

Post by ToLuSe »

Hi there!

I just try to get a bit of an understanding for the sensor routines and was stumbling across some bugs in V1.7 for BMA180, which seem
to be already fixed now in the SVN trunk (factory calibration for offset_x killed and tcs set to -4% accidentely),
but i can't get my head around this bit of magic in the init routine for BMA020:

i2c_writeReg(0x70,0x14,0x71);
i2c_rep_start(0x71);
control = i2c_readNak();

Looks to me that we happily write 0x71 into register 14 and try to read it afterwards?!
Shouldn't it be more like this?

i2c_rep_start(0x70); // I2C write direction
i2c_write(0x14); // Register 0x14
i2c_rep_start(0x71); // I2C read direction
control = i2c_readNak();

So why are we writing 0x71? The data sheet just says to issue a stop condition after the control byte for the register address...
Is this some creative workaround to force a stop?

Sorry for the stupid question.

Kind Regards,
Tobias
Last edited by ToLuSe on Tue Jul 19, 2011 1:35 pm, edited 3 times in total.

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

Re: Sensor Init for BMA020...

Post by ziss_dm »

Hi ToLuSe,

I think, this is mistake. It should read 0x14h modify and write.
Something like this:

Code: Select all

....
uint8_t i2c_readReg(uint8_t add, uint8_t reg) {
  i2c_rep_start(add+0);  // I2C write direction
  i2c_write(reg);        //   register selection
  i2c_rep_start(add+1);  // I2C write direction
  return i2c_readNak();
}
....
#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70, 0x15, 0x80);  // Write B10000000 at 0x15 init BMA020
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
  control = control | (0x00 << 3); //Range 2G 00
  control = control | 0x00;        //Bandwidth 25 Hz 000
  i2c_writeReg(0x70, 0x14, control);
  acc_1G = 255;
}

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


regards,
ziss_dm

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

Re: Sensor Init for BMA020...

Post by ziss_dm »

Hi Alex,
Just made small patch to the Sensors.pde, to keep everything consistent. Also increased resolution of the BMA180 up to 11bit. (Theoretically should increase leveling precision up to 0.125 deg and also helps with acc/velocity stabilization)

regards,
ziss_dm
Attachments
sensors.pde.patch.zip
(1.85 KiB) Downloaded 159 times

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

Thanks ziss_dm. I like it :-)

Bests,
Tobias

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

Re: Sensor Init for BMA020...

Post by Alexinparis »

I think you're right about the BMA020.
About the BMA180, I think it's also a good idea to increaser the resolution.
I think you meant /32 and not /16 (/4/8)

Code: Select all

+  //useful info is on the 14 bits  [2-15] bits  /4 => [0-13] bits  /8 => 11 bit resolution
+  ACC_ORIENTATION(  - ((rawADC[1]<<8) | rawADC[0])/32 ,
+                    - ((rawADC[3]<<8) | rawADC[2])/32 ,
+                      ((rawADC[5]<<8) | rawADC[4])/32 );

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

Re: Sensor Init for BMA020...

Post by Hamburger »

I applied your provided patch and can see the new smoothness in the GUI.
As a consequence I had to substantially lower the level PI to P = 4.0 and I = 0.012 on Tricopter with 60cm M2M an 9" props.
it works.
Hamburger

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

Hi!

I was adding a few more comments mainly for my personal reference based on ziss_dm's changes.
Note that i have neither a BMA020 nor a BMA180 to test any of this. Just my personal pedantic attitude.

I also have no idea how to translate the sensor setup into an acc_1G value based on the used ACC_getADC() scaling.

Of course if any of the I2C reads fail, things will go horribly wrong. Any routine to check for a correct read before writing?

Bests,
Tobias

P.S.: Yes it compiles.
Attachments
Sensors.pde.zip
(2.69 KiB) Downloaded 143 times

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

Re: Sensor Init for BMA020...

Post by ziss_dm »

Hi Alex,

You are absolutely right /32, sorry about that. :oops:
I have looked SVN, and saw you also did the same for BMA020. I do not think it is good idea, as per datashit it is 10bit and currently we are getting "reserved" bit in LSB. So it is better to leave it as 10bit (If you are not planning to kill acc_1G variable):

Code: Select all

#if defined(BMA020)
void ACC_init(){
  i2c_writeReg(0x70,0x15,0x80); // Write B10000000 at 0x15 init BMA020
  uint8_t control = i2c_readReg(0x70, 0x14);
  control = control & 0xE0;
  control = control | (0x00 << 3); //Range 2G 00
  control = control | 0x00; //Bandwidth 25 Hz 000
  i2c_writeReg(0x70,0x14,control);
  acc_1G = 255;
}

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


regards,
ziss_dm
Attachments
sensors.pde.patch.zip
(2.71 KiB) Downloaded 314 times

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

Good idea to move the register description to the function header.
How about this?

Bests,
Tobias
Attachments
Sensors.pde.diff.zip
(3.47 KiB) Downloaded 135 times

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

More confused questions. As it's a rainy day i stared at the reading routines for the data:

- BMA020 is 10 Bit so divide by 64 or more easier to understand for me right shift by 6 bits, positions everything nicely as far as i understand.

- BMA180 is 14 Bits, so using all 14 would mean divide by 4 or right shift by 2 bits, however the code is just using 11, so are we deliberately dropping/shifting out the 3 - maybe noisy - LSB. Bug or feature?

And completely off topic:
WiiBrew says the conversion factor between the sensors is 2000/440. Can't we do a multiply by 2 and divide by 9 instead of a divide by 5?
Should hopefully still fit in 16bit. Not sure if that will improve anything though. Like this:

Code: Select all

    GYRO_ORIENTATION(   (rawADC[3]&0x01)     ? (gyroADC[ROLL]<<1)/9  : gyroADC[ROLL] ,
                        (rawADC[4]&0x02)>>1  ? (gyroADC[PITCH]<<1)/9 : gyroADC[PITCH] ,
                        (rawADC[3]&0x02)>>1  ? (gyroADC[YAW]<<1)/9   : gyroADC[YAW]   );


And a last one:
Why is GYRO_Common called before we check & convert for slow mode? Is the zero compensation been done on the raw data with the bigger resolution?

Bests,
Tobias

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

Re: Sensor Init for BMA020...

Post by ziss_dm »

Hi ToLuSe,

- BMA020 is 10 Bit so divide by 64 or more easier to understand for me right shift by 6 bits, positions everything nicely as far as i understand.

You are forgeting, that operand is signed. :) And you need to preserve sign...

- BMA180 is 14 Bits, so using all 14 would mean divide by 4 or right shift by 2 bits, however the code is just using 11, so are we deliberately dropping/shifting out the 3 - maybe noisy - LSB. Bug or feature?

I think, historically all ACC was "aligned" to the NK (+-2g 10bit)

WiiBrew says the conversion factor between the sensors is 2000/440

Not sure about that, seems to be working fine. If something working fine - do not touch it. :D

Why is GYRO_Common called before we check & convert for slow mode? Is the zero compensation been done on the raw data with the bigger resolution?

Do not forget, that slow/fast is just scale, it should not change zero point.

regards,
ziss_dm

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

Hi!

You are forgeting, that operand is signed. :) And you need to preserve sign...


Sorry i didn't get that. ACC MSB data is a full byte. ACC LSB data is 2 bytes. There are 5 unused bits and the new_data bit at the bottom.
So >>6 or /(2*2*2*2*2*2) or /64 seems to be fine to position the data.

I think, historically all ACC was "aligned" to the NK (+-2g 10bit)


So using 11bit is still fine, but using all 14bit would cause problems (overflow e.t.c.)?

Not sure about that, seems to be working fine. If something working fine - do not touch it. :D


I'll give it a try the IDG650 datasheet specifies 2000°/s and 440°/s. BaronPilot is also using /0.22 as a conversion factor.
So 0.22222222period is maybe a little improvement compared to 0.2. Though of course nobody knows if the IDG600 spec is the same and what the XV-3500 for the Z-Axis says to that.

Do not forget, that slow/fast is just scale, it should not change zero point.


Yes, but assuming we have not adjusted the zero point and the sensor has an offset e.g. +9 in slow mode for one axis.
Wouldn't that offset then change to +2 in fast mode (if you could see it :) and thus always subtracting -9 would lead to wrong results in fast mode?

Bests,
Tobias <-Still learning

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensor Init for BMA020...

Post by ToLuSe »

FYI, attached my current Sensors.pde tinkering for your reference.
Btw. i think acc_1G needs to be initialized earlier in the WMP-Nunchuk Init.

Bests,
Tobias
Attachments
Sensors.pde.patch.zip
(3.01 KiB) Downloaded 144 times

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ToLuSe »

More tinkering with Sensors.pde.

Tobias
Attachments
Sensors.pde.patch.zip
(3.67 KiB) Downloaded 143 times

toshirotamigi
Posts: 8
Joined: Fri Mar 18, 2011 3:47 pm

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by toshirotamigi »

I think that there are problems with the augmented resolution for BMA180.
After a bad crash i had today after activating Stable Mode (the quad instantaneously gone on one side and crashed to the ground before i even realized what was happening).
After some checks on the gui i seen that now the 1G value is more than 500 and if you rotate the quad for more than 90° on one side it seems that some attitude calculation overflow (the image of the quad is oriented wrongly on the configurator).

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

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ziss_dm »

Hi toshirotamigi,
The 1g value for BMA180 should be +512 normal and -512 inverted.

if you rotate the quad for more than 90° on one side it seems that some attitude calculation overflow (the image of the quad is oriented wrongly on the configurator)

Could you describe this little bit more, or attach screenshots?

regards,
ziss_dm

toshirotamigi
Posts: 8
Joined: Fri Mar 18, 2011 3:47 pm

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by toshirotamigi »

ziss_dm wrote:Hi toshirotamigi,
The 1g value for BMA180 should be +512 normal and -512 inverted.

if you rotate the quad for more than 90° on one side it seems that some attitude calculation overflow (the image of the quad is oriented wrongly on the configurator)

Could you describe this little bit more, or attach screenshots?

regards,
ziss_dm


I know this, but what is the range that the algorithm does expect for the accelerometer values?
I'll try to investigate further as i came back at home this evening.

Thanks and regards,

Alessio.

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

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by Alexinparis »

512 is not enough to cause an overflow in the angle calculation.
The attitude singularity of axis near 90deg is normal, and is only a transition state.
It can't cause by itself a sudden flip starting from a level state.

toshirotamigi
Posts: 8
Joined: Fri Mar 18, 2011 3:47 pm

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by toshirotamigi »

Alexinparis wrote:512 is not enough to cause an overflow in the angle calculation.
The attitude singularity of axis near 90deg is normal, and is only a transition state.
It can't cause by itself a sudden flip starting from a level state.

Ok, then i have recreate the problem and investigate further on what happened.

Thanks and regards.

Alessio

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ToLuSe »

Hi Guys!

I had some fun rearranging some parts on my MultiWii board to get space for a BMA180. Today i wired it up and did some sanity checks with the code in the SVN trunk. Of course i couldn't keep my fingers of the code, so attached are my latest changes. If the weather holds i could give it a test tomorrow.

As people understandably are not keen to blindly add untested changes, i guess i should add a bit of explanation, why i changed what.

Adding attachment doesn't work anymore for me, so you have to download it from my webspace.
MultiWii_SW_Mods.zip: http://www.toluse.de/scripts/ccount/click.php?id=11
This is a SVN diff/patch from trunk revision 209. I might update the content of the file as i go along...


config.h - Well my setup, but i added a NO_VBAT variable

MultiWii.pde - Avoid beeping / blinking of the LiPo Alarm, if the voltage is smaller NO_VBAT. This is because my Arduino Nano is powered via USB and i often have no Battery connected to the motors.

Sensors.pde
Line 112 - Also reset the ACC when the WMP is reset. This is because i have both powered from D12 at the moment. Not sure if it works, but i thought if we kill the power to all sensors, all sensors should be reinitialized :-)

Line 148-169 - Made the GYRO calibration function a bit easier to understand (for me :D ). We also do real 400 readings now and not just 399. The Line " gyroZero[axis]=(g[axis]+200)/399;" didn't make any sense to me... Adding a "fake" 200 reading would only make sense to me if we would divide by 400. I don't really get that.

Line 184-208 - Same code changes for ACC calibration

Line 520: The ADXL345 Datasheet says 256 LSB/g Typical for +-g resolution

Line 545-588: Read the BMA180 Datasheet and wrote a hopefully safe init function. The BMA180 has an EEPROM part and a shadow, where the EEPROM content is copied into on start-up as far as i understood. So in theory we could change the EEPROM content ONCE and then never never ever would have to initialize the chip again. However then we would need to do a "read eeprom content, unlock eeprom, write eeprom" and there are also calibration values in, which have to be carefully preserved. I felt safer to just do a read-modify-write on the shadow variables. If you have a virgin / unfiddled BMA180 the setting of the range should not be necessary, as 2g is the default setup. As i don't know where you got your chip from, it's maybe safer to set the value to 2g anyhow.
Feel free to correct me if i got i all wrong. Reading datasheets on the Laptop is terrible, i need to print it out and use a textmarker :-P.
(Note: Every >>1 is a divide by 2. So >>5 equals /(2*2*2*2*2) or /32)

I spotted a mistake in my changes, we still need to set the ee_w bit. The datasheet says: Also the image registers corresponding to the locked EEPROM area are locked by the EEPROM lock mechanism in order to avoid mal-functions on the overall system. So even though we just update the image registers and not write to the EEPROM we still need to unlock it.

Line 599-633: Read the BMA020 datasheet. I think bits of might have made it into SVN... Not sure... So it's just comments here. OR-ing zeros has no effect whatsoever. I got rid of these lines. One could keep them as a "example" how to set a different Range & Bandwith. The default range and bandwith 2G / 25Hz means setting zeroes, so they are already cleared by the previous &=0xE0. >>6 equals /64.

Line 859-863: I didn't remove the Nunchuk in my setup yet, so i just initialize the WMP, if an ACC is used. So i can switch between Nunchuk and BMA180.

Line 866-877: Just changed the comments. I move the acc_1G define up, as it is uninitialized at the point of time it is used. My Nunchuck was using a value of 204 for 1G. Usually it's just 200.

Line 894-913: We can check for Nunchuck or WMP by looking at the two lower bits at the same time. All the other sensor code was OR-ing the data together, so i do it here as well. Changed the conversion factor for slowmode to "/4,5" by multiplying by 2 and dividing by 9. Ideally it should be 4,5454545454 e.t.c.


Ok that's it. I will post if i can successfully fly with these changes or if i get an amazing crash out of it.

Bests,
Tobias
Last edited by ToLuSe on Tue Jul 19, 2011 12:43 pm, edited 1 time in total.

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ToLuSe »

Added DL link to my webspace as Attachments don't seem to work anymore for me. See last posting on previous page.

Tobias

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

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ziss_dm »

Hi ToLuSe,

You shifting right Two's complement. Is it really working?

As I understand:

Code: Select all

  int16_t(int16_t(-1) >> 2) = 16383


Or I'm missing something? ;)

regards,
ziss_dm

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes... (Was "Sensor Init for BMA020...")

Post by ToLuSe »

Sorry i am slow. Can you be more specific?
Which exact lines in my changes do you not like?

Generally all the data from the sensors is smaller 16bit, so does shifting instead of dividing for scaling really make a difference?
But that poses the question how did it ever work?! Even without my changes.
I'll have another look in the datasheets.

Edit: Found this nice PDF http://cache.freescale.com/files/sensors/doc/app_note/AN4076.pdf
See "7.1 Converting 14-bit 2's complement HEX Number to Signed Integer (Counts)"
Basically check for the signed bit and do a bit-inversion plus 1, if it's set.

The real question is: What does the SW expect and what does a Nunchuk usually do?

Tobias

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Ok.. Doh.. I think i understand now why the original code works :-). Told you i was slow!
Simply based on the fact that we put the MSB at Bit 15 and use the divide function, thus the sign is preserved.
An explicit cast would have been nice. I bet Lint would go berserk on that.

I'll have another go at the code.

Thanks,
Tobias

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

Re: Sensors.pde changes...

Post by Alexinparis »

@ToLuSe
I will integrate some of your mod in the next dev

NO_VBAT: OK

ACC_init() after a power pin reset: NOK
the only device that need to be reseted is the WMP or WMP+NK.
Other ACC don't need to be on the power PIN.
This adaptation works but is setup dependent

GYRO & ACC calibration:
200 is here because (x+200)/400 is more accurate than x/400 in math integer but it's not so important here
there is effectively 400 sample and not 399 with your implementation
- added come comments

ACC_1G for ADXL345: OK

BMA180: OK
I keep however /32

BMA020: OK
I keep however /64

WMP: we can keep the nunchuk pass-through mode whatever the config. If not, no side effect

WMP: the way to assemble data is the same between fast & slow mode. The neutral value is also the same. The division for slow mode is done after, and is ok with the already defined neutral value.

WMP: the coef /5 was always fine. I know about the ratio 0,22, but it works like this and is a faster code ;)
Last edited by Alexinparis on Wed Jul 20, 2011 8:57 am, edited 1 time in total.

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Hi Alex...

Thanks for the reply. Will comment tomorrow. I am already 2h beyond my bedtime for getting into work tomorrow and am still struggling with ziss_dm's cryptic 2's complement comments. I will be dead tomorrow morning and i haven't cracked that nut grrr. :evil: *sigh*

Last desperate attempt:

Code: Select all

  for (i=0; i<3; i++)
  {
    // Assemble 14bit data from sensor
    conv[i] = ( (rawADC[i*2+1]<<8) | rawADC[i*2] ) >> 2;
 
    // Throw away 3LSB to get 11bit data
    conv[i] >>= 3;

    // If Bit 11 is set, do sign extension for higher bits
    if (conv[i] & 0x0400 )
      conv[i] |= 0xF800;
  }

  ACC_ORIENTATION( - conv[0] , - conv[1] , conv[2] );


Ok getting closer, one should just RTFM from the Arduino docu at ttp://www.arduino.cc/en/Reference/Bitshift
When you shift x right by y bits (x >> y), and the highest bit in x is a 1, the behavior depends on the exact data type of x. If x is of type int, the highest bit is the sign bit, determining whether x is negative or not, as we have discussed above. In that case, the sign bit is copied into lower bits, for esoteric historical reasons:

int x = -16; // binary: 1111111111110000
int y = x >> 3; // binary: 1111111111111110

This behavior, called sign extension, is often not the behavior you want. Instead, you may wish zeros to be shifted in from the left. It turns out that the right shift rules are different for unsigned int expressions, so you can use a typecast to suppress ones being copied from the left:

int x = -16; // binary: 1111111111110000
int y = (unsigned int)x >> 3; // binary: 0001111111111110


So the example from ziss_dm would still result in -1, as the signed bit is "sticky". So i guess the fact that we <<8 the first word, already makes the signed bit stick, if the compiler is clever enough to detect that the target is an int16_t. So whether to use >> 5 or /32 does not matter, but only if the MSB was shifted to Bit 15 first. If we would do an: (rawADC[i*2+1]<<6) | (rawADC[i*2]>>2); then we would need to check for Bit 11 and fill the upper bits with 1's manually.

I will verify that tomorrow... Ehm today... Now 3h lack of sleep. Alas, at least i am learning something... :roll:

Does anybody buy that new theory?

If the test is successfull, i suggest to add an explicit typecast to (int16_t), even if we keep the division...

Bests,
Tobias

P.S.: The "Quadrocopter" Model in MultiWiiConf is doing random flips... with any ACC value >180 ... Is this due to my shoddy programming or some overflow in the app?!

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

Re: Sensors.pde changes...

Post by ziss_dm »

Hi Tobias,
The biggest question: Is it really worth it? ;)

You also can define macro like this:

Code: Select all

#define shrs(val,num) (((val & 0x7FFF) >> num) | (val & 0x8000))


But GCC is smart enough to compile:

Code: Select all

  ACC_ORIENTATION(  - ( uint16_t(rawADC[1]<<8) | rawADC[0] ) / 64 ,
                    - ( uint16_t(rawADC[3]<<8) | rawADC[2] ) / 64 ,
                      ( uint16_t(rawADC[5]<<8) | rawADC[4] ) / 64 );


to the:

Code: Select all

    21e8:   f0 e0          ldi   r31, 0x00   ; 0
    21ea:   4e 2b          or   r20, r30
    21ec:   5f 2b          or   r21, r31
    21ee:   50 95          com   r21
    21f0:   41 95          neg   r20
    21f2:   5f 4f          sbci   r21, 0xFF   ; 255
    21f4:   36 e0          ldi   r19, 0x06   ; 6
    21f6:   56 95          lsr   r21
    21f8:   47 95          ror   r20
    21fa:   3a 95          dec   r19
    21fc:   e1 f7          brne   .-8         ; 0x21f6 <_Z10ACC_getADCv+0x56>

So basically, it is replacing division by 5 shifts. ;)

regards,
ziss_dm

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Why are you suddenly casting to uint16_t ? You are on a mission to confuse me, aren't you? :lol:

The current code for the BMA020 calls a function __divmodhi4:

Code: Select all

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


Code: Select all

    1a8e:   90 91 c1 02    lds   r25, 0x02C1
    1a92:   80 e0          ldi   r24, 0x00   ; 0
    1a94:   20 91 c0 02    lds   r18, 0x02C0
    1a98:   30 e0          ldi   r19, 0x00   ; 0
    1a9a:   82 2b          or   r24, r18
    1a9c:   93 2b          or   r25, r19
    1a9e:   60 e4          ldi   r22, 0x40   ; 64
    1aa0:   70 e0          ldi   r23, 0x00   ; 0
    1aa2:   0e 94 51 1c    call   0x38a2   ; 0x38a2 <__divmodhi4>
    1aa6:   70 93 b8 03    sts   0x03B8, r23
    1aaa:   60 93 b7 03    sts   0x03B7, r22
    1aae:   90 91 c3 02    lds   r25, 0x02C3
    1ab2:   80 e0          ldi   r24, 0x00   ; 0
    1ab4:   20 91 c2 02    lds   r18, 0x02C2
    1ab8:   30 e0          ldi   r19, 0x00   ; 0
    1aba:   82 2b          or   r24, r18
    1abc:   93 2b          or   r25, r19
    1abe:   60 e4          ldi   r22, 0x40   ; 64
    1ac0:   70 e0          ldi   r23, 0x00   ; 0
    1ac2:   0e 94 51 1c    call   0x38a2   ; 0x38a2 <__divmodhi4>
    1ac6:   70 93 ba 03    sts   0x03BA, r23
    1aca:   60 93 b9 03    sts   0x03B9, r22
    1ace:   90 91 c5 02    lds   r25, 0x02C5
    1ad2:   80 e0          ldi   r24, 0x00   ; 0
    1ad4:   20 91 c4 02    lds   r18, 0x02C4
    1ad8:   30 e0          ldi   r19, 0x00   ; 0
    1ada:   82 2b          or   r24, r18
    1adc:   93 2b          or   r25, r19
    1ade:   60 e4          ldi   r22, 0x40   ; 64
    1ae0:   70 e0          ldi   r23, 0x00   ; 0
    1ae2:   0e 94 51 1c    call   0x38a2   ; 0x38a2 <__divmodhi4>
    1ae6:   70 93 bc 03    sts   0x03BC, r23
    1aea:   60 93 bb 03    sts   0x03BB, r22


Tobias

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

@alexinparis:

Ok here is my reply... I am half way back from Zombie to Human :mrgreen:

ACC_init():
I didn't expect you to take that change :-) Just specific to my current wiring...
Only connecting the WMP to D12 and the Accelerometer to e.g. the 3.3V Output of the Arduino Nano means a complete new board, so i didn't do it for now.

GYRO calibration:
Sorry didn't catch that, how is doing 399 measurements, adding a fake 200 measurement and dividing by 399 more accurate ?!
399 measurements plus 200 divided by 400 would have made sort of sense for me. :?:

BMA180:
I accidentely removed the ee_w unlocking. This is still needed. I misread the datasheet. My bad... Even though we just update the image / shadow registers, we still need to unlock the eeprom, the image and eeprom registers are both protected by ee_w.
Reading values - I am still working on it.. The /32 calls a function <__divmodhi4>. Not sure how fast that is compared to shifting.

BMA020:
Same comment as above /64 calls <__divmodhi4>

WMP Pass Through Mode:
Another change specific to my current HW. Sorry, i just added the BMA180 in addition to my WMP + Nunchuk setup, so i guess i really have to stop getting data from the Nunchuk, if i use the BMA180.

WMP Zero Calibration:
Hm... Again slow thinker - The Sensor has a slow and fast mode... So basically in slow-mode the value is just multiplied by the sensor to get less noise out of the ADC. What we measure for zero calibration is basically the "amplified" zero-offset in slow mode... So when the ACC switches to fast mode, why doesn't the zero-offset of the sensor input then also change the scaling to fast mode? It's the same input...
Or rephrased: Aren't we subtracting a slow mode value range zero-offset from data which could also come in fast mode? *headscratch*

SlowModeData (raw)
-SlowModeZeroOffset (raw)
Result scaled to FastModeRange, if SlowMode detected

And

FastModeData (raw)
- ZeroSlowModeOffset(raw)

Is that correct?


WMP Slow/Fastmode scaling:
Well i found on the Internet a few posts suggesting 2000/440 scaling, which comes from IDG-650 datasheet. But i also found 2000/500 scaling, which matches an IDG-600 Product Brief PDF. So the factors i found was divide by 4.54period and divide by 4.. :-) Dind't find a divide by 5 yet.
Ah well i guess there are a lot of people flying happily with divide by 5... Would be nice to have some empiric data though.

WMP:
I changed acc_1G to 202 for my Nunchuk! Should be reverted to 200. Sorry!

Ok i guess that's it :-) Back to the evil bitshifting from hell :twisted:
I am getting better reading assembler data :>

Bests & Thanks
Tobias

P.S.: Will update my diff with the rebased SVN trunk later... Guess i am making a real idiot out of myself here in this forum, alas at least it's good fun :lol:

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Wrote a little test sketch for my shifting tests. Not sure how accurate millis() is or if i have a major cock-up in there somewhere.
This should loop through the int16 value range and does a "/32" and a ">>5 plus rounding towards zero" calculation. It tries to measure the time for both methods.
Alas here is the code:

Code: Select all

#define SHIFT  (5)
#define START (-32768)
#define STOP  (32767)

/*-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------*/

#define DIV_BY (1<<SHIFT)
int16_t dummy;

void setup()
{
  uint32_t start_time, time1, time2;
  int16_t result1, result2;
 
  Serial.begin(115200);
 
  Serial.print("### Shifty Test: ");
  Serial.print("Loop from ");
  Serial.print(START);
  Serial.print(" to ");
  Serial.print(STOP);
  Serial.println(": \n");
 
  Serial.print("Timing /");
  Serial.print(DIV_BY);
  Serial.print(": "); 
  start_time = millis();
  for (int16_t loop=START; loop<STOP; loop++)
  {
     result1 = loop / DIV_BY;
     
     // If we don't use result1, the compiler optimizes everything away :)
     StoreOutput(result1);
  }
  time1 = millis()-start_time;
  Serial.println(time1);
 
  Serial.print("Timing >>");
  Serial.print(SHIFT);
  Serial.print(": ");
  start_time = millis();
  for (int16_t loop=START; loop<STOP; loop++)
  {
     result2 = loop;
     if ( result2 & 0x8000 )
       result2 += (1<<SHIFT)-1; // 31;
     result2 >>= SHIFT;
 
     // If we don't use result2, the compiler optimizes everything away :)
     StoreOutput(result2);
  }
  time2 = millis()-start_time;
  Serial.println(time2);
 
  Serial.print("Sanity check: ");
  for (int16_t loop=START; loop<STOP; loop++)
  {
     result1 = loop / DIV_BY;
     
     result2 = loop;
     if ( result2 & 0x8000 )
       result2 += (1<<SHIFT)-1;
     result2 >>= SHIFT;
     
     if (result1 != result2 )
     {
     Serial.print("ERROR(");
     Serial.print(loop);
     Serial.print("): ");
     Serial.print(result1);
     Serial.print(" <-> ");
     Serial.print(result2);
     Serial.print("\n");
     }
  }
 
  Serial.print("DONE\n");

  Serial.println("\n### END");
}

void loop()                       // run over and over again
{
                                  // do nothing!
}

void StoreOutput(int16_t output)
{
  dummy = output;
}


And here is the output from my Arduino Nano

Code: Select all

### Shifty Test: Loop from -32768 to 32767: 

Timing /32: 1022
Timing >>5: 154
Sanity check: DONE

### END


Would be better to wiggle a pin and connect a scope, but i don't have the proper equipment here. Alas just a relative A<->B comparision. I haven't looked at the generated assembler yet. Didn't get much sleep this week. I just can't let go, if i mull over such an issue :roll:
So next step is to implant that into Sensors.pde.

My draft looks really nasty... This does really not help readability... :cry:

Code: Select all

  for (i=0; i<3; i++)
  {
    conv[i] = hi[i*2+1]<<8 | lo[i*2];
    if (hi[i*2+1] & 0x80)
      conv[i] += (1<<5)-1;
    conv[i] >>= 5;
  }


Bests,
Tobias

P.S.: Just went completely crazy and ordered a LPC1769 LXPresso board with a Cortex-M3 monster... :mrgreen:

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

Re: Sensors.pde changes...

Post by ziss_dm »

Hi Tobias

Why not this: http://mbed.org/ ? ;)
Costs less than Arduino Mega.

Maybe this would inspare you:

Code: Select all

inline int16_t int_pack(uint8_t msb, uint8_t lsb, uint8_t shr) {
  union {uint16_t uval; uint16_t val; uint8_t raw[2]; } data;
  data.raw[0] = lsb; data.raw[1] = msb;
  uint8_t neg = (data.raw[1] & 0x80);
  data.raw[1] &= 0x7F;
  data.uval = data.uval >> shr;
  if (neg) data.raw[1] |= 0x80;
  return data.val;
}

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  /8 => 11 bit resolution
  ACC_ORIENTATION(  -int_pack(rawADC[1], rawADC[0], 5) ,
                    -int_pack(rawADC[3], rawADC[2], 5) ,
                     int_pack(rawADC[5], rawADC[4], 5)
                  );
  ACC_Common();
}



regards,
ziss_dm

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Hi ziss_dm!

(mbed would have been more expensive and i don't really like the "compiler in the cloud" idea. 23,50€ for ARM Cortex M-3 board plus debug interface is a bargain i think.)

Alas thanks for the suggestion with the union and the inline function. Turns out using the union avoids two assembler load and or instructions, as the compiler just sticks the data where we want, no need to to a 16bit or on the data.

So i guess this is my final version:

Code: Select all

inline int16_t pack_and_divide(uint8_t high_byte, uint8_t low_byte, uint8_t shift_right)
{
 union { int16_t packed; uint8_t raw[2]; } data;
 
 // Pack sensor bytes into word
 // (Note: Using the union avoids two unnecessary or instructions in generated assembler)
 data.raw[1] = high_byte;
 data.raw[0] = low_byte;
 
 // Round towards zero for negative values
 if ( high_byte & 0x80 )
   data.packed += ( 1<<shift_right ) - 1;

 // Shift right and return value
 return ( data.packed >>= shift_right );
}


The generated assembler looks also quite nice :-)

Code: Select all

 union { int16_t packed; uint8_t raw[2]; } data;
 
 // Pack sensor bytes into word
 // (Note: Using the union avoids two unnecessary or instructions in generated assembler)
 data.raw[1] = high_byte;
 1da:   38 2f          mov   r19, r24
 data.raw[0] = low_byte;
 1dc:   24 2f          mov   r18, r20
 
 // Round towards zero for negative values
 if ( high_byte & 0x80 )
 1de:   87 ff          sbrs   r24, 7
 1e0:   02 c0          rjmp   .+4         ; 0x1e6 <setup+0x126>
   data.packed += ( 1<<shift_right ) - 1;
 1e2:   21 5e          subi   r18, 0xE1   ; 225
 1e4:   3f 4f          sbci   r19, 0xFF   ; 255

 // Shift right and return value
 return ( data.packed >>= shift_right );
 1e6:   c9 01          movw   r24, r18
 1e8:   25 e0          ldi   r18, 0x05   ; 5
 1ea:   95 95          asr   r25
 1ec:   87 95          ror   r24
 1ee:   2a 95          dec   r18
 1f0:   e1 f7          brne   .-8         ; 0x1ea <setup+0x12a>


Time to implant it into Sensors.pde...

Bests,
Tobias

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Updated the download link with my current changes:

MultiWii_SW_Mods.zip: http://www.toluse.de/scripts/ccount/click.php?id=11
This is a SVN diff/patch from trunk revision 214. I will update the content of the file as i go along...

Edit: Updated 23.07.2011 23:01 pm CEST
Edit: Updated 31.07.2011 19:42 pm CEST

Not sure, if it was worth the effort, but hey at least i learned something. :P

Watch out for the sensor orientation for BMA180. I had mine mounted according to a PDF called "Accelerometer test for MultiWii", which explained a few odd results i saw in MultiWiiConf.

Did a testflight today. Well, the accelerometer is really fast, but all my V1.7 settings seem useless with the SVN trunk and the change from Nunchuk to BMA180.
I want a "How to migrate MultiWii Settings N to Release N+1" :mrgreen:

Bests,
Tobias
Last edited by ToLuSe on Sun Jul 31, 2011 6:46 pm, edited 1 time in total.

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Updated my Diff; See above!

ToLuSe
Posts: 27
Joined: Thu Jun 16, 2011 9:17 pm
Location: near Munich, Germany

Re: Sensors.pde changes...

Post by ToLuSe »

Hi everybody!

I admit i lost a bit my motivation with my MultiWii Quadrocopter. The need to completely retune everything from scratch when using a sensor with a different value range and also the different behavior i saw when going to V1.8 was getting me a bit frustrated :roll:

(I have a BMA180 and ITG-3200 here, but will probably hook it to an LPC1769 (ARM Cortex M3) instead and leave the MultiWii Board as-is.)

So as this is probably a wrap-up, i attach my last mods for your reference. This is a Diff against SVN trunk rev 297.
The divide and round towards zero routine was tested with a debug sketch, so should be OK.
The rest was done by looking at the datasheets, meaning there might be still bugs in there.

Bests & Happy flying,
Tobias
Attachments
MultiWii_SW_Mods.zip
(3.31 KiB) Downloaded 120 times

Post Reply