LIS3LV02DQ acceleromter

Post Reply
Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

LIS3LV02DQ acceleromter

Post by Doron »

I have a Sparkfun LIS3LV02DQ breakout board that I am trying to use with Alex's MultiWii code. (as my clone NK hardware does not initialize consistently ~50% of the time )

Here is the datasheet http://www.sparkfun.com/datasheets/IC/LIS3LV02DQ.pdf

I connected my LIS3LV02DQ breakout into the Jussi v1.1 shield (using 3.3v) and I can get good results using the following simple code (using the Wire library) :

#include <Wire.h>
int XAccel, YAccel,ZAccel;
float x,y,z;

void setup()
{
Serial.begin(38400);
Serial.println("starting");
Wire.begin();
Wire.beginTransmission(0x1D);
Wire.send(0x20); // CTRL_REG1 20h
// 10: device on, 00: decimate by 512, 0: normal mode, 111: all axis enabled
Wire.send(0x87);
Wire.endTransmission();
}


void loop(void)
{

byte bytearray[6]; // low, high;
int cnt;

// trnsmit to device with address 0x1D or 0011101b
// send the sub address for the register we want to read
// this is for the OUTX_L register
// masking the register address with 0x80 enables auto
// increment for subsequent reads

Wire.beginTransmission(0x1D);
Wire.send(0x28|0x80); // auto increment
Wire.endTransmission();
// now do a transfer reading all six bytes
Wire.requestFrom(0x1D,6);
cnt=0;
while(cnt<6)
{
if(Wire.available())
{
bytearray[cnt]=Wire.receive();
cnt++;
}
}

XAccel=(bytearray[1]<<8)+bytearray[0];
YAccel=(bytearray[3]<<8)+bytearray[2];
ZAccel=(bytearray[5]<<8)+bytearray[4];

x=XAccel/1060.0;
y=YAccel/1060.0;
z=ZAccel/1060.0;

Serial.print(x);
Serial.print(" ");
Serial.print(y);
Serial.print(" ");
Serial.println(z);
delay(10);
}


I now took Alex's core I2C routines (the one not using Wire.h) and I get no good data.
Can you please see if you can spot my ignorance ? I only modified i2c_ACC_init () and i2c_ACC_getADC ().

Here is the code that does not work :

//**************************
// LIS3LV02DQ routines
// 1st attempt : Doron (dby@adelphie.net)
// Feb 2010
//**************************
#define ROLL 0
#define PITCH 1
#define YAW 2

#define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL)
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5);
#define I2C_SPEED 100000L //100kHz normal mode
#define POWERPIN_ON PORTB |= 1<<4;
#define POWERPIN_OFF PORTB &= ~(1<<4); //switch OFF WMP, digital PIN 12

static int16_t acc_1G = 1050; //this is the 1G measured acceleration (nunchuk)
static int16_t acc_25deg = 444; //this is the the ~ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t accPresent = 0; //I2C or ADC acc present
static uint32_t neutralizeTime;
static int16_t accADC[3];

static uint8_t rawADC_LISL3[6];

// *********************
// I2C general functions
// *********************

// Mask prescaler bits : only 5 bits of TWSR defines the status of each I2C request
#define TW_STATUS_MASK (1<<TWS7) | (1<<TWS6) | (1<<TWS5) | (1<<TWS4) | (1<<TWS3)
#define TW_STATUS (TWSR & TW_STATUS_MASK)

void i2c_init(void) {

#if defined(INTERNAL_I2C_PULLUPS)
I2C_PULLUPS_ENABLE
#else
I2C_PULLUPS_DISABLE
#endif

TWSR = 0; // no prescaler => prescaler = 1
TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
TWCR = 1<<TWEN; // enable twi module, no interrupt
}

void i2c_rep_start(uint8_t address) {
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWSTO); // send REAPEAT START condition
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
TWDR = address; // send device address
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C(); // wail until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}

void i2c_write(uint8_t data ) {
TWDR = data; // send data to the previously addressed device
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}

uint8_t i2c_readAck() {
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
waitTransmissionI2C();
return TWDR;
}

uint8_t i2c_readNak(void) {
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C();
return TWDR;
}

void waitTransmissionI2C() {
uint8_t count = 255;
while (count-->0 && !(TWCR & (1<<TWINT)) );
if (count<2) { //we are in a blocking state => we don't insist
TWCR = 0; //and we force a reset on TWINT register
neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay after the hard reset
}
}

void checkStatusI2C() {
if ( (TW_STATUS & 0xF8) == 0xF8) { //TW_NO_INFO : this I2C error status indicates a wrong I2C communication.
TWCR = 0;
POWERPIN_OFF //switch OFF WMP
delay(1);
POWERPIN_ON //switch ON WMP
delay(10);
// #if not defined(ITG3200)
// i2c_WMP_init(0);
// #endif
neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay (20ms) after the hard reset
}
}

void i2c_ACC_init () {
delay(10);
i2c_rep_start(0x1D+0); // I2C write direction
i2c_write(0x20); // ctrl_reg 20
i2c_write(0x87); // 11: device on, 01: decimate by 128, 0: normal mode, 111: all axis enabled
accPresent = 1;
}

void i2c_ACC_getADC () {

i2c_rep_start(0x1D); // I2C write direction
i2c_write(0x28|0x80); // auto increment
i2c_rep_start(0x1D+1); // I2C read direction => 1
for(uint8_t i = 0; i < 5; i++) {
rawADC_LISL3[i]=i2c_readAck();
}
rawADC_LISL3[5]= i2c_readNak();

accADC[ROLL] = (((rawADC_LISL3[1]<<8) | (rawADC_LISL3[0])))/1050;
accADC[ROLL] = (((rawADC_LISL3[3]<<8) | (rawADC_LISL3[2])))/1050;
accADC[YAW] = (((rawADC_LISL3[5]<<8) | (rawADC_LISL3[4])))/1050;
}
void setup()
{
Serial.begin(38400);
Serial.println("starting");
i2c_init();
}

void loop()
{
i2c_ACC_getADC();
/*Serial.print(accADC[ROLL] );
Serial.print(" ");
Serial.print(accADC[ROLL] );
Serial.print(" ");
Serial.println(accADC[YAW]);
*/
for(uint8_t i = 0; i <= 5; i++) {
Serial.print(rawADC_LISL3[i]);
Serial.print(" : ");
}
Serial.println();


}

Thanks !

-Doron.

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

I forgot to add
i2c_ACC_init ();
at the end of setup ();
but it still does not work...

fax8
Posts: 61
Joined: Mon Feb 14, 2011 5:29 pm

Re: LIS3LV02DQ acceleromter

Post by fax8 »

Hi everybody,

This question has been asked by mail directly to me.. I asked Doron to post it in a publicly available place so that everyone could get the solution (if I ever find it).

Looking at the code, it seems that MultiWii related code isn't enabling ATMEGA I2C internal pullups resistors while the Wire code does that (it happens implicitly when you initialize Wire). So, if you can't communicate with the MultiWii code but you can with the Wire one, that's probably something to investigate.

Seems that a simple

Code: Select all

define INTERNAL_I2C_PULLUPS

on the first lines of the Wii code might be enough for enabling the pullups.

So that's something you might wanna try. Just remember to use an LLC if that device has 3.3V logic levels if you connect it to a 5V 16MHz Arduino.

Hope this helps,

Fabio Varesano

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

I put it on the scope and figured it out...

The "WIre" library lets us get away with specifying 8 bit I2C address... Alex's routing need 7 bit...
As for the pull-ups... I understand that the shield has external resistors thus 'don't care'...

Here is my working code :


//**************************
// LIS3LV02DQ routines
// Doron (dby@adelphie.net)
// Feb 2010
//**************************
#define ROLL 0
#define PITCH 1
#define YAW 2

#define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL)
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5);
#define I2C_SPEED 100000L //100kHz normal mode
#define POWERPIN_ON PORTB |= 1<<4;
#define POWERPIN_OFF PORTB &= ~(1<<4); //switch OFF WMP, digital PIN 12

static int16_t acc_1G = 1050; //this is the 1G measured acceleration (nunchuk)
static int16_t acc_25deg = 444; //this is the the ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t accPresent = 0; //I2C or ADC acc present
static uint32_t neutralizeTime;
static int16_t accADC[3];
static uint8_t rawADC_LISL3[6];

// *********************
// I2C general functions
// *********************

// Mask prescaler bits : only 5 bits of TWSR defines the status of each I2C request
#define TW_STATUS_MASK (1<<TWS7) | (1<<TWS6) | (1<<TWS5) | (1<<TWS4) | (1<<TWS3)
#define TW_STATUS (TWSR & TW_STATUS_MASK)

void i2c_init(void) {

#if defined(INTERNAL_I2C_PULLUPS)
I2C_PULLUPS_ENABLE
#else
I2C_PULLUPS_DISABLE
#endif

TWSR = 0; // no prescaler => prescaler = 1
TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
TWCR = 1<<TWEN; // enable twi module, no interrupt
}

void i2c_rep_start(uint8_t address) {
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWSTO); // send REAPEAT START condition
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
TWDR = address; // send device address
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C(); // wail until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}

void i2c_write(uint8_t data ) {
TWDR = data; // send data to the previously addressed device
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}

uint8_t i2c_readAck() {
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
waitTransmissionI2C();
return TWDR;
}

uint8_t i2c_readNak(void) {
TWCR = (1<<TWINT) | (1<<TWEN);
waitTransmissionI2C();
return TWDR;
}

void waitTransmissionI2C() {
uint8_t count = 255;
while (count-->0 && !(TWCR & (1<<TWINT)) );
if (count<2) { //we are in a blocking state => we don't insist
TWCR = 0; //and we force a reset on TWINT register
neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay after the hard reset
}
}

void checkStatusI2C() {
if ( (TW_STATUS & 0xF8) == 0xF8) { //TW_NO_INFO : this I2C error status indicates a wrong I2C communication.
TWCR = 0;
POWERPIN_OFF //switch OFF WMP
delay(1);
POWERPIN_ON //switch ON WMP
delay(10);
// #if not defined(ITG3200)
// i2c_WMP_init(0);
// #endif
neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay (20ms) after the hard reset
}
}

void i2c_ACC_init () {
delay(10);
// i2c_rep_start(0x1D+0); // I2C write direction
i2c_rep_start(0x3A+0); // I2C write direction 7bit

i2c_write(0x20); // ctrl_reg 20
i2c_write(0x87); // 11: device on, 01: decimate by 128, 0: normal mode, 111: all axis enabled
accPresent = 1;
}

void i2c_ACC_getADC () {

// i2c_rep_start(0x1D); // I2C write direction (** does not work if you send 8 bit**)
i2c_rep_start(0x3A); // I2C 7bit write direction
i2c_write(0x28|0x80); // auto increment
// i2c_rep_start(0x1D+1); // I2C read direction => 1 (** does not work if you send 8 bit**)
i2c_rep_start(0x3A+1); // I2C read direction => 1
for(uint8_t i = 0; i < 5; i++) {
rawADC_LISL3[i]=i2c_readAck();
}
rawADC_LISL3[5]= i2c_readNak();

accADC[ROLL] = (((rawADC_LISL3[1]<<8) + (rawADC_LISL3[0])));
accADC[ROLL] = (((rawADC_LISL3[3]<<8) + (rawADC_LISL3[2])));
accADC[YAW] = (((rawADC_LISL3[5]<<8) + (rawADC_LISL3[4])));
}
void setup()
{
Serial.begin(38400);
Serial.println("starting");
i2c_init();
delay(5);
i2c_ACC_init ();
}

void loop()
{
i2c_ACC_getADC();
Serial.print(accADC[ROLL] );
Serial.print(" ");
Serial.print(accADC[ROLL] );
Serial.print(" ");
Serial.println(accADC[YAW]);
/*
for(uint8_t i = 0; i <= 5; i++) {
Serial.print(rawADC_LISL3[i]);
Serial.print(" : ");
}
Serial.println();
*/

}


the value ranges I get are -1080 (for -1G) to +1080 for (+1G).... Do I need to scale it for Alex ?

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Also, according to my scope,
at 100khz the I2C buss is only busy for 880 micro seconds... for one reading
at 400khz the I2C buss is only busy for 238 micro seconds... not bad...

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

fax8 wrote:Just remember to use an LLC if that device has 3.3V logic levels if you connect it to a 5V 16MHz Arduino.
Fabio Varesano


I connected it to 3.3v VCC and to a 5v Arduino and I am not using LLC... seems to work fine...so far.... do you think it is being damaged over time ?

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

I just measure on the scope the signals and they seem to be ~3.48v

Is it possible that since the device is fed with VCC of 3.3v , the logic level between the two are down to 3.3v ?)

fax8
Posts: 61
Joined: Mon Feb 14, 2011 5:29 pm

Re: LIS3LV02DQ acceleromter

Post by fax8 »

No, if you have the internal pullups off and the external one tied to 3.3v will be just fine as the ATMEGA will get any voltage > 2.5V as high. Problem is when you have long lines and noisy line so the delta between 3.3 and 2.5 is small so you might get into problems (especially at 400KHz).

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

Re: LIS3LV02DQ acceleromter

Post by Alexinparis »

Hi,
As long as you already have external pullups on 3.3V, it's much better to disable the internal pullups in the arduino.
Some I2C devices like BMP085 don't like at all 5V on I2C BUS.
A proper way would be to use a LLC, but the margin 2.5V-3.3V seems to be ok so far even at 400kHz.

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Thanks for the 3.3 advice.

I am still waiting for Hobbyking (my order has been on 'backorder' for weeks on their 25A and 30A Turnigy Plush speed controllers)
so I will not be able to try this setup near 'noise'....Will keep you posted.
-Doron.

P.S.
Any other suggestions for a low cost / good speed controller (~20-25a) that can process our ~500hz PWM ?

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

Re: LIS3LV02DQ acceleromter

Post by Alexinparis »

all Super Simple ESCs are ok

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Just 'ok' or 'as-good' ?

can you tell the difference in performance ? response time ? stability ?

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Hi Alex,

I got the code integrated into your v1.6 base.

I have a WMP and the LIS3LV02DQ attached.

All 6 lines move on the GUI but not the picture of the Quad... I suspect I have a sign (+/-) convention issue/problem....

This is what I have now

Code: Select all

  accADC[PITCH]  =  (((rawADC_LISL3[1]<<8) + (rawADC_LISL3[0]))>>2); 
  accADC[ROLL]  =  (((rawADC_LISL3[3]<<8) + (rawADC_LISL3[2])))>>2;
  accADC[YAW]   =  (((rawADC_LISL3[5]<<8) + (rawADC_LISL3[4])))>>2;


Can you please tell me what I should see for the 3 ACC lines

1) the 3 ACC_ROLL, ACC_PITCH, ACC_Z expected values for 'level'
2) which way should the green line move for "Nose up"
3) which way should the red line move for "a Bank right"

How can I send you the whole code so the LIS3LV02DQ is another option?

-Doron.
Last edited by Doron on Tue Feb 15, 2011 2:02 am, edited 1 time in total.

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Since there were not too many permutations ...I found my error in two tries...;)

Code: Select all

  accADC[PITCH]  =  -(((rawADC_LISL3[1]<<8) + (rawADC_LISL3[0]))>>2); 
  accADC[ROLL]  =  (((rawADC_LISL3[3]<<8) + (rawADC_LISL3[2])))>>2;
  accADC[YAW]   =  -(((rawADC_LISL3[5]<<8) + (rawADC_LISL3[4])))>>2;


I think the sensors and CPU are ready....
Now I need to focus on the rest of the hardware.

bradleyk
Posts: 48
Joined: Wed Jan 19, 2011 10:58 pm

Re: LIS3LV02DQ acceleromter

Post by bradleyk »

so do you have this line of code in you acc part?


accPresent = 1;


it will give you the dirgram

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

I used a real test....

Code: Select all

  i2c_rep_start(0x3A+0);
  i2c_write(0x0F);            // Who_am_i ?
  i2c_rep_start(0x3A+1);
  who_am_i = i2c_readNak();
  if (who_am_i == 0x3A) accPresent = 1;
 


That was not the problem ... regardless everything works fine .... at least in the GUI....;)

spagoziak
Posts: 171
Joined: Thu Jan 20, 2011 1:18 am

Re: LIS3LV02DQ acceleromter

Post by spagoziak »

fax8 wrote:No, if you have the internal pullups off and the external one tied to 3.3v will be just fine as the ATMEGA will get any voltage > 2.5V as high. Problem is when you have long lines and noisy line so the delta between 3.3 and 2.5 is small so you might get into problems (especially at 400KHz).


Interesting! How long is a "long line"?

fax8
Posts: 61
Joined: Mon Feb 14, 2011 5:29 pm

Re: LIS3LV02DQ acceleromter

Post by fax8 »

Can't say a specific number.. I ran into a similar problem when I was mounting my FreeIMU on my arm and running I2C on the desk with 1mt long cables.

Doron
Posts: 21
Joined: Mon Feb 14, 2011 3:01 pm

Re: LIS3LV02DQ acceleromter

Post by Doron »

Alexinparis wrote:all Super Simple ESCs are ok


I just found out I had 4 x TowerPro 25A from a couple of years ago (when people were converting them to I2c... I never got to it)
Do you know if it will / or not wor with the MultiWi ~490hz PWM ?

Thanks
-Doron.

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

Re: LIS3LV02DQ acceleromter

Post by Alexinparis »

Hi,
Not sure about the tower pro.
Look at this list:
http://info.tt-rc.de/wiki.php?file=faq.txt&edit=1
The same techno is used to drive the ESCs.

Post Reply