 |
 |
| View previous topic :: View next topic |
| Author |
Message |
perseo963
Joined: 02 Feb 2013
Posts: 4
Location: colombia
|
| 6DOF imu (ADXL345 and itg3200) |
 Posted: Tue Feb 19, 2013 9:19 pm |
 |
|
Hi all, I'm working with a 6DOF imu (ADXL345 and itg3200).
The goal is to find the degree of inclination for each axis, and display it on an LCD. I designed this code based on the ADXL345 accelerometer datasheet, but found no reply from accelerometer. As the pic also works at 5v I made an interface using mosfet as indicated by Philips for attaching different voltage levels 5v to 3.3v in i2c line.
Thank you for your help.
| Code: |
#include <16f877a.h>
#fuses hs,NOWDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP
#use delay(clock=20M)
#use i2c(MASTER, SDA=PIN_c4, SCL=PIN_c3, force_hw)
#include <stdlib.h>
#include <math.h>
#include <lcd.c>
#define WRITE_DEV_ADD 0xA6
#define READ_DEV_ADD 0xA7
#define FIFO_MODE 0x38
#define BW_RATE 0x2C
#define POWER_CTL 0x2D
#define DATA_FORMAT 0x31
#define DATAX0 0x32
#define DATAX1 0x33
#define DATAY0 0x34
#define DATAY1 0x35
#define DATAZ0 0x36
#define DATAZ1 0x37
#DEFINE INT_ENABLE 0X2E
char x0,x1,y0,y1,z0,z1;
long int x,y,z;
void inicializa_acelerometro()
{
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(DATA_FORMAT);
i2c_write(0x0B);
I2C_STOP();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(POWER_CTL);
i2c_write(0x08);
I2C_STOP();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(INT_ENABLE);
i2c_write(0x80);
I2C_STOP();
}
void main ()
{
enable_interrupts(int_rda);
enable_interrupts(GLOBAL);
WHILE(true)
{
lcd_init();
inicializa_acelerometro();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x32);
i2c_start();
i2c_read(READ_DEV_ADD);
x0=i2c_read(0x32);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x32);
i2c_start();
i2c_read(READ_DEV_ADD);
x0=i2c_read(0x32);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x33);
i2c_start();
i2c_read(READ_DEV_ADD);
x1=i2c_read(0x33);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x34);
i2c_start();
i2c_read(READ_DEV_ADD);
y0=i2c_read(0x34);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x35);
i2c_start();
i2c_read(READ_DEV_ADD);
y1=i2c_read(0x35);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x36);
i2c_start();
i2c_read(READ_DEV_ADD);
z0=i2c_read(0x36);
i2c_read(0);
i2c_stop();
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x37);
i2c_start();
i2c_read(READ_DEV_ADD);
z1=i2c_read(0x37);
i2c_read(0);
i2c_stop();
//////////concatenar datos adxl345/////////////
x=((x1)<<8)|x0 ;
y=((y1)<<8)|y0;
z=((z1)<<8)|z0;
////////////////////////////////////////////////
printf(lcd_putc," \f %ld %ld %ld ",x,y,z);
}
} |
|
|
 |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19215
|
|
| Posted: Wed Feb 20, 2013 2:00 am |
|
|
As a comment, I presume you are referring to AN97055 for the level shifting?. This does work, _but_ selection of the MOSFET is critical. 90% of MOSFET's have too high a Vgs for this circuit. You need ones with Vgs <2v, and quite low gate capacitances (<100pF). Unless these parameters are met, the circuit will be unreliable.
As PCM programmer says, use the tester first, and if this doesn't work, then you know you have a hardware problem.
Also, what values are you using for the Rp resistors?. Remember these need to be significantly _lower_ on the 3.3v part of the bus.
Best Wishes |
|
|
perseo963
Joined: 02 Feb 2013
Posts: 4
Location: colombia
|
| perseo963 |
| Posted: Wed Feb 20, 2013 5:02 pm |
|
|
hello
I tested the code I found in the link you sent me PCM programmer and not hardware problem, it works perfectly, I visualize the direction of the gyroscope (DO) and accelerometer (A6). |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Wed Feb 20, 2013 7:08 pm |
|
|
| Quote: | i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x32);
i2c_start();
i2c_read(READ_DEV_ADD);
x0=i2c_read(0x32);
i2c_read(0);
i2c_stop();
|
These read routines are incorrect. Delete all your existing axis data
read routines (there are several of them in your code).
You're supposed to read all 6 bytes in one pass, according to the
ADXL345 data sheet. Your axis read code should look like this:
| Code: |
i2c_start();
i2c_write(WRITE_DEV_ADD);
i2c_write(0x32); // x0 data register address
i2c_start();
i2c_write(READ_DEV_ADD);
x0 = i2c_read();
x1 = i2c_read();
y0 = i2c_read();
y1 = i2c_read();
z0 = i2c_read();
z1 = i2c_read(0); // NACK on last read
i2c_stop();
|
Each of those axis data variables should be declared as 'int8'.
When you combine the msb and lsb bytes for each axis, use the
CCS function make16(). Put the result in an 'int16' variable. It's easier.
Also you have these two lines in your code:
| Quote: | enable_interrupts(int_rda);
enable_interrupts(GLOBAL); |
But I don't see any #int_rda routine. Delete those two lines.
Don't enable interrupts without an interrupt handler routine. |
|
|
perseo963
Joined: 02 Feb 2013
Posts: 4
Location: colombia
|
| perseo963 |
| Posted: Tue Mar 12, 2013 9:28 pm |
|
|
Hello again, from the code that I built with his help I received sensor data, on 3 axes. I tested for concatenation of several ways, with make16 and bit shifting. My ultimate goal is to find the angle sensor inclination from the horizontal. However I do not understand because the sensor does not give negative readings for certain positions as stated in the datasheet and so to get a total reading 360 degrees in all three axes, I studied and I used AN-1057 formulations, but the output values are not consistent.
Appreciate your help.
| Code: |
#include <16f877a.h>
#fuses XT,NOWDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP
#use delay(clock=4M)
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <lcd.c>
//#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, bits=8, parity=N)
#use i2c(MASTER, SDA=PIN_c4, SCL=PIN_c3,FORCE_HW)
#define ACCEL_WRITE_ADDR 0XA6//0xA6 //0x53//0x3a
#define ACCEL_READ_ADDR 0XA7//0xA7 //0x53//0x3b
#define ACCEL_DATA_ADDR 0x32
#define ACCEL_PWRCTRL_ADDR 0x2d
#define ACCEL_MEASURE_MODE 0x08
int i,accel_data[6],j;
int16 x,y,z,x2,y2,z2;
//unsigned int off_x=6,off_y=9,off_z=232;
float x1,y1,z1,R;
float teta=0, gama=0,fi=0;
void main ()
{
// initialize
for (i = 0; i < 6; i++) {
accel_data[i] = 0;
}
while(true) {
lcd_init();
// Tell the accelerometer to wake up
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(0x31);
i2c_write(0x0B);
i2c_stop();
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(ACCEL_PWRCTRL_ADDR);
i2c_write(ACCEL_MEASURE_MODE);
i2c_stop();
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(0X2E);
i2c_write(0X80);
i2c_stop();
//////////////////////////////////////////////
// Read data from the accel
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(ACCEL_DATA_ADDR);
i2c_start();
i2c_write(ACCEL_READ_ADDR);
accel_data[0] = i2c_read(); //x0
accel_data[1] = i2c_read(); //x1
accel_data[2] = i2c_read(); //y0
accel_data[3] = i2c_read(); //y1
accel_data[4] = i2c_read(); //z0
accel_data[5] = i2c_read(); // z1, NACK on last read
i2c_read(0);
i2c_stop();
//////////concatenar datos adxl345/////////////
// x=((accel_data[1])<<8)+ accel_data[0] ;
//y=((accel_data[3])<<8)+ accel_data[2];
//z=((accel_data[5])<<8)+ accel_data[4];
x=make16((accel_data[1]),(accel_data[0]));
y=make16((accel_data[3]),(accel_data[2]));
z=make16((accel_data[5]),(accel_data[4]));
x1=(float)x/256;
y1=(float)y/256;
z1=(float)z/256;
////////////////////////////////////////////////
R=sqrt((x1*x1)+(y1*y1)+(z1*z1));
teta=acos(x1/R);
teta=teta * 57,296;
gama=acos(y1/R);
gama=gama*57,296;
fi=acos(z1/R);
fi=fi*57,296;
printf(lcd_putc,"\f %.2f %.2f %.2f ",x1,y1,z1);
printf(lcd_putc,"\n %f %f %f ",teta,gama,fi);
}
} |
|
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Tue Mar 12, 2013 10:03 pm |
|
|
| Quote: | accel_data[0] = i2c_read(); //x0
accel_data[1] = i2c_read(); //x1
accel_data[2] = i2c_read(); //y0
accel_data[3] = i2c_read(); //y1
accel_data[4] = i2c_read(); //z0
accel_data[5] = i2c_read(); // z1, NACK on last read
i2c_read(0);
i2c_stop(); |
This is not correct. I showed you sample code in an earlier post, but
you ignored it. Why ? To do a NACK on the last read, means that
you should do it on the last read operation. It doesn't mean that you
should stick in another i2c_read line and do the NACK there.
Look at my example code and follow it.
| Quote: | | However I do not understand because the sensor does not give negative readings |
But the variables that you declared, such as these:
| Code: | | int16 x,y,z,x2,y2,z2; |
are unsigned. Look in the CCS manual. CCS has a lot of quirks.
It's different, in many ways, from "normal" C. One of the differences is
that the CCS data types, such as int8, int16, and int32 are unsigned.
If you want signed variables, then you have to tell the compiler. Example:
| Code: | | signed int16 x,y,z,x2,y2,z2; |
This is in the CCS manual, on page 45 (page 57 in the Acrobat reader):
http://www.ccsinfo.com/downloads/ccs_c_manual.pdf
The manual says:
| Quote: | | unsigned - Data is always positive. This is the default data type if not specified. |
|
|
|
perseo963
Joined: 02 Feb 2013
Posts: 4
Location: colombia
|
|
| Posted: Wed Apr 10, 2013 8:31 pm |
|
|
Hello again,
Thanks for your help PCM,
I need to get the angle between the horizontal and the floor but I have not been able... I add the code to see if there is an error, I read the AN1057 and followed their instructions but did not work ...
Many thanks again.
| Code: |
#include <16f877a.h>
#fuses xt,NOWDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP
#use delay(clock=4M)
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <lcd.c>
#use i2c(MASTER, SDA=PIN_c4, SCL=PIN_c3)
#define ACCEL_WRITE_ADDR 0XA6//0xA6 //0x53//0x3a
#define ACCEL_READ_ADDR 0XA7//0xA7 //0x53//0x3b
#define ACCEL_DATA_ADDR 0x32
#define ACCEL_PWRCTRL_ADDR 0x2d
#define ACCEL_MEASURE_MODE 0x08
signed int accel_data[6];
signed int x,y,z;
float x1,y1,z1,R1,R2,R3;
float teta=0, gama=0, fi=0;
float teta1=0, gama1=0 , fi1=0 ;
signed int16 AX,AY,AZ;
void main ()
{
// initialize
while(true) {
lcd_init();
// Tell the accelerometer to wake up
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(0X31);
i2c_write(0X0B);////+-16
//i2c_write(0X01);///+-4g
i2c_stop();
I2C_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(ACCEL_PWRCTRL_ADDR);
i2c_write(ACCEL_MEASURE_MODE);
I2C_stop();
// Read data from the accel
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(ACCEL_DATA_ADDR);
i2c_start();
i2c_write(ACCEL_READ_ADDR);
accel_data[0] = i2c_read(); //x0
accel_data[1] = i2c_read(); //x1
accel_data[2] = i2c_read(); //y0
accel_data[3] = i2c_read(); //y1
accel_data[4] = i2c_read(); //z0
accel_data[5] = i2c_read(0); // z1, NACK on last read
i2c_stop();
//////////concatenar datos adxl345/////////////
x=((accel_data[1])<<8)|accel_data[0] ;
y=((accel_data[3])<<8)|accel_data[2];
z=((accel_data[5])<<8)|accel_data[4];
//////////////////////////multiplicar 16*2/2^13//////////////////
x1=(float)x * 0.00390625;
y1=(float)y * 0.00390625;
z1=(float)z * 0.00390625;
////////////////////////////////////////////////
// R=sqrt((x1*x1)+(y1*y1)+(z1*z1));
R1=sqrt((y1*y1)+(z1*z1));
R2=sqrt((x1*x1)+(z1*z1));
R1=sqrt((y1*y1)+(x1*x1));
// teta=acos(x1/R);
teta=atan(x1/R1);
teta=teta * 57,296;
// gama=acos(y1/R);
gama=atan(y1/R2);
gama=gama*57,296;
// fi=acos(z1/R);
fi= atan(R3/z1);
fi=fi*57,296;
AX=(TETA*10)*0.1;
AY=(GAMA*10)*0.1;
AZ=(FI*10)*0.1;
printf(lcd_putc,"\f %d %d %d ",x,y,z);
printf(lcd_putc,"\n %LD %LD %LD ",AX,AY,AZ);
DELAY_MS(100);
}
}
|
|
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Wed Apr 10, 2013 11:27 pm |
|
|
When you have a program that has many steps for complicated math
operations, you can debug it by putting in printf statements after each
set of calculations. You can check if the intermediate values look correct.
If they don't look correct, then study the lines of code for those
calculations and fix the code.
This is a standard debugging technique: Display intermediate results.
| Quote: |
// Read data from the accel
i2c_start();
i2c_write(ACCEL_WRITE_ADDR);
i2c_write(ACCEL_DATA_ADDR);
i2c_start();
i2c_write(ACCEL_READ_ADDR);
accel_data[0] = i2c_read(); //x0
accel_data[1] = i2c_read(); //x1
accel_data[2] = i2c_read(); //y0
accel_data[3] = i2c_read(); //y1
accel_data[4] = i2c_read(); //z0
accel_data[5] = i2c_read(0); // z1, NACK on last read
i2c_stop();
// Display results of reading ADXL345.
printf("x1: %x x0: %x \n\r", accel_data[1], accel_data[0]);
printf("y1: %x y0: %x \n\r", accel_data[3], accel_data[2]);
printf("z1: %x z0: %x \n\r", accel_data[5], accel_data[4]);
//////////concatenar datos adxl345/////////////
x=((accel_data[1])<<8)|accel_data[0] ;
y=((accel_data[3])<<8)|accel_data[2];
z=((accel_data[5])<<8)|accel_data[4];
// Display results of combining 8-bit values into 16-bit.
printf("x = %x \n\r", x);
printf("y = %x \n\r", y);
printf("z = %x \n\r", z);
|
If the printf statements show incorrect intermediate results, then
look closely at your code and find the problems. |
|
|
dorinm
Joined: 07 Jan 2006
Posts: 38
|
|
| Posted: Sat Apr 13, 2013 3:37 pm |
|
|
I have ported the AVR AHRS code for sparkfun 9dof imu (https://www.sparkfun.com/products/10724) to 18f26K22 (clocked@64MHZ because I needed 50Hz output and calculations take ~3msec. plus other functionalities...). If it helps I can send it to you (it uses the magnetometer but it can be easily cut off). It is pretty straightforward (DCM algorithm it's easy to understand, just plain vector math).
Just for one's curiosity, the same code for Atmega @8MHz (8 (avr)MIPS) runs in about 4-4.8milliseconds (compared to 16(pic)MIPS ), so it seems that AVR is still faster...
...however!: with some code optimizations (inlines and fast trig.math algo's), the PIC can return the same result in ~1.5milliseconds, while the avr in ~3msec =)) , so it seems that the CCS compiler is really good at code generation and optimization, compared to "de facto" winAVR. |
|
|
phuongnt
Joined: 25 Apr 2013
Posts: 3
|
|
| Posted: Thu May 02, 2013 3:21 am |
|
|
| dorinm wrote: | I have ported the AVR AHRS code for sparkfun 9dof imu (https://www.sparkfun.com/products/10724) to 18f26K22 (clocked@64MHZ because I needed 50Hz output and calculations take ~3msec. plus other functionalities...). If it helps I can send it to you (it uses the magnetometer but it can be easily cut off). It is pretty straightforward (DCM algorithm it's easy to understand, just plain vector math).
Just for one's curiosity, the same code for Atmega @8MHz (8 (avr)MIPS) runs in about 4-4.8milliseconds (compared to 16(pic)MIPS ), so it seems that AVR is still faster...
...however!: with some code optimizations (inlines and fast trig.math algo's), the PIC can return the same result in ~1.5milliseconds, while the avr in ~3msec =)) , so it seems that the CCS compiler is really good at code generation and optimization, compared to "de facto" winAVR. |
Hi dorinm,
I'm interested in your ported AHRS code. Could you please share?
Thanks |
|
|
DIEGODAVICINO
Joined: 24 Jun 2020
Posts: 1
|
|
| Posted: Wed Jun 24, 2020 12:00 pm |
|
|
Hello. I am new to this, could someone explain why I divide by 256 ? The rest I understand everything.
| Code: |
x = make16((accel_data[1]), (accel_data[0]));
y = make16((accel_data[3]), (accel_data[2]));
z = make16((accel_data[5]), (accel_data[4]));
x1 = (float)x/256;
y1 = (float)y/256;
z1 = (float)z/256; |
|
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19215
|
|
| Posted: Wed Jun 24, 2020 11:49 pm |
|
|
For the question 'why /256', this is from the chip's datasheet.
The sensitivity is given as min 230, max 282, typical 256 counts/g.
So by dividing the counts by 256, you get a figure in g. |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
