Hi guys, a month ago i bought an MPU6050 module, wich i want to interface with a 8bit PIC18F microcontroler.
This is my first project wich contains a gyroscope/acelerometer.
I have problems interpreting the data from the MPU.
The I2C comunication works fine
Here is the mpu mpu6060.c file
void mpu_init(){
I2C1_Start();
I2C1_Wr(0xd0);
i2c1_wr(0x6B);//
i2c1_wr(0x00); // enable temp sensor, and set internal osc 8mhz
i2c1_stop();
I2C1_Start();
I2C1_Wr(0xd0); //
i2c1_wr(0x23);// aceess FIFO register
i2c1_wr(0x00); // Disable FIFO Register
i2c1_stop();
I2C1_Start();
I2C1_Wr(0xd0); //
i2c1_wr(0x37);// aceess INT_pin register 55
i2c1_wr(0x00); // Disable INT Register
i2c1_stop();
I2C1_Start();
I2C1_Wr(0xd0); //
i2c1_wr(0x38);// aceess INT_pin register 56
i2c1_wr(0x00); // Disable INT Register
i2c1_stop();
I2C1_Start();
I2C1_Wr(0xd0);
i2c1_wr(0x19);// aceess register 25
i2c1_wr(0x00); // set sample write 0
i2c1_stop();
delay_ms(5);
I2C1_Start();
I2C1_Wr(0xd0);
i2c1_wr(0x1a);// aceess register 26
i2c1_wr(0x00); // disable FSYNC&DLPF & DLPF_CFG / set accelerometer 260hz & gyroscope 8khz
i2c1_stop();
delay_ms(5);
///// Gyroscope configuration register /////
I2C1_Start();
I2C1_Wr(0xd0);
i2c1_wr(0x1B);// aceess register 27
i2c1_wr(0x00); // set gyro scale to 250d/s
i2c1_stop();
delay_ms(5);
/////// Accelerometer configuration ///////
I2C1_Start();
I2C1_Wr(0xd0);
i2c1_wr(0x1C);// aceess register 28
i2c1_wr(0x00); // set accelerometer scale to +/-2g
i2c1_stop();
delay_ms(5);
}
float gyro_scale=131.0;
float acc_scale=16384.0;
unsigned short int xg_msb, xg_lsb;
unsigned int gx;
float Xg=0;
void get_Xg(){ //read y gyro axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x43); // y msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
xg_msb=i2c1_rd(1); // 8bit MSB
xg_lsb=i2c1_rd(0); // 8bit LSB
i2c1_stop();
gx=(xg_msb<<8)+xg_lsb;
Xg=gx/gyro_scale;
}
unsigned short int yg_msb, yg_lsb;
unsigned int gy;
float Yg=0;
void get_Yg(){ //read y gyro axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x45); // y msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
yg_msb=i2c1_rd(1); // 8bit MSB
yg_lsb=i2c1_rd(0); // 8bit LSB
i2c1_stop();
gy=(yg_msb<<8)+yg_lsb;
Yg=gy/gyro_scale;
}
unsigned short int zg_msb,zg_lsb;
unsigned int gz;
float Zg=0;
void get_Zg() {
//read z gyro axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x47); // z msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
zg_msb=i2c1_rd(1); // 8 Bit MSB
zg_lsb=i2c1_rd(0); // 8 Bit LSB
i2c1_stop();
gz=(zg_msb<<8)+zg_lsb;
Zg=gz/gyro_scale; // convert to deg/s
}
unsigned short int xa_msb,xa_lsb;
unsigned int ax;
float Xa=0;
void get_Xa() { // read accelerometer Y axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x3B); // x msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
xa_msb=i2c1_rd(1); // Hi(raw_x)=i2c1_rd(1); //temperature msb
xa_lsb=i2c1_rd(0); //Lo(raw_x)=i2c1_rd(0); //temperature lsb
i2c1_stop();
ax=(xa_msb<<8)+xa_lsb;
Xa=ax/acc_scale;
}
unsigned short int ya_msb,ya_lsb;
float Ya=0;
unsigned int ay;
void get_Ya() { // read accelerometer Y axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x3D); // x msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
ya_msb=i2c1_rd(1); // Hi(raw_x)=i2c1_rd(1); //temperature msb
ya_lsb=i2c1_rd(0); //Lo(raw_x)=i2c1_rd(0); //temperature lsb
i2c1_stop();
ay=(ya_msb<<8)+ya_lsb;
Ya=ay/acc_scale;
}
unsigned short int za_msb,za_lsb;
unsigned int az;
float Za=0;
void get_Za(){ // read acc z axis
i2c1_start();
i2c1_wr(0xd0);//adress +w
i2c1_wr(0x3F); // x msb
i2c1_repeated_start();
i2c1_wr(0xd1);//adress +r
za_msb=i2c1_rd(1); // 8 bit MSB
za_lsb=i2c1_rd(0); //8 bit LSB
i2c1_stop();
az=(za_msb<<8)+za_lsb;
Za=az/acc_scale; }
Here is my code main program
#include "include/mpu6050.c"
#include "built_in.h"
#define PI 3.14159265359
#define RAD_TO_DEG 57.29578
sbit LCD_RS at Rd5_bit;
sbit LCD_EN at Rd6_bit;
sbit LCD_D4 at Rd4_bit;
sbit LCD_D5 at Rd3_bit;
sbit LCD_D6 at Rd2_bit;
sbit LCD_D7 at Rd1_bit;
sbit LCD_RS_Direction at TRISd5_bit;
sbit LCD_EN_Direction at TRISd6_bit;
sbit LCD_D4_Direction at TRISd4_bit;
sbit LCD_D5_Direction at TRISd3_bit;
sbit LCD_D6_Direction at TRISd2_bit;
sbit LCD_D7_Direction at TRISd1_bit;
float filterAngle;
float dt=0.02;
float comp_filter(float newAngle, float newRate) {
float filterTerm0;
float filterTerm1;
float filterTerm2;
float timeConstant;
timeConstant=0.4; // default 1.0
filterTerm0 = (newAngle - filterAngle) * timeConstant * timeConstant;
filterTerm2 += filterTerm0 * dt;
filterTerm1 = filterTerm2 + ((newAngle - filterAngle) * 2 * timeConstant) + newRate;
filterAngle = (filterTerm1 * dt) + filterAngle;
//return previousAngle; // This is actually the current angle, but is stored for the next iteration
}
void main() {
trisd=0x00;
trisc=0x00;
ADCON0=0x00;
lcd_init();
lcd_cmd(_lcd_cursor_off);
I2C1_Init(100000);
delay_ms(10);
mpu_init();
while(1){
get_Xg() ; // gyro X in d/s
get_Yg(); // gyro Y in d/s
get_Zg() ; // gyro Z in d/s
get_Xa(); // acc X in g
get_Ya(); // acc Y in g
get_Za(); // acc Z in g
accx =(atan2(Ya,Za)+PI)*RAD_TO_DEG ;
comp_filter(accx,Xg);
floattostr(filterAngle,txt);
lcd_out(1,1,txt);
delay_ms(100);
}
}
Like i said before, i have no experience with gyros and accelerometers, but its been almost 4 weeks since i "google" it and found less info on how to calculate the actual angle usinging gyro and acc data.
I the programed posted above i tryied to get the X axis angle and display on a 16x2 lcd
I just need some guideance in getting the raw gyro angles and the pass trough a complementary filter...
Thanks