Jump to content

Recommended Posts

Hello everybody, I'm now working on Gy-521 MPU6050 and Stellaris Launchpad. I use the ported Jeff Rowberg's library from here (https://code.google.com/p/launchpad-stellaris-energia/wiki/Mpu6050Usage) and it work great to get the raw values from sensors. But when I apply complementary to compute the angles, it does not work whether I add the acc-angles calculation, it builds successfully but the serial monitor stop at "Initializing I2C devices...". I've attached my code below, anyone could help me out, thanks.
//Sorry for my bad english.

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

bool blinkState = false;
float acc_x, acc_y, acc_z;
float gyro_x, gyro_y, gyro_z;
float base_ax, base_ay, base_az, base_gx, base_gy, base_gz;
float acc_angle_x, acc_angle_y, acc_angle_z;
float gyro_angle_x, gyro_angle_y, gyro_angle_z;
unsigned long last_read_time;
float last_x, last_y, last_z;
float dt, t_now;

void calib()
  int samples = 10;
  float acc_x_calib = 0;
  float acc_y_calib = 0;
  float acc_z_calib = 0;
  float gyro_x_calib = 0;
  float gyro_y_calib = 0;
  float gyro_z_calib = 0;
  int16_t Ax, Ay, Az, Gx, Gy, Gz;
  for(int i=0; i<samples;i++)
    accelgyro.getMotion6(&Ax, &Ay, &Az, &Gx, &Gy, &Gz);
    acc_x_calib += float(Ax);
    acc_y_calib += float(Ay);
    acc_z_calib += float(Az);
    gyro_x_calib += float(Gx);
    gyro_y_calib += float(Gy);
    gyro_z_calib += float(Gz);
  base_ax = acc_x_calib/samples;
  base_ay = acc_y_calib/samples;
  base_az = acc_z_calib/samples;
  base_gx = gyro_x_calib/samples;
  base_gy = gyro_y_calib/samples;
  base_gz = gyro_z_calib/samples;

void set_last_read(unsigned long time, float x, float y, float z) 
  last_read_time = time;
  last_x = x;
  last_y = y;
  last_z = z; 

inline unsigned long get_last_time() { return last_read_time;}
inline float get_last_x() { return last_x; }
inline float get_last_y() { return last_y; }
inline float get_last_z() { return last_z; }

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)

    // initialize device
    Serial.println("Initializing I2C devices...");

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // configure Arduino LED for
    pinMode(GREEN_LED, OUTPUT);
    set_last_read(millis(), 0, 0, 0);

void loop() 
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    unsigned long t_now = millis();
    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);
    float acc_x = float(ax)/16384;
    float acc_y = float(ay)/16384;
    float acc_z = float(az)/16384;
    float gyro_x = (float(gx) - base_gx)/131;
    float gyro_y = (float(gy) - base_gy)/131;
    float gyro_z = (float(gz) - base_gz)/131;
    /* - not work
    float R2D = 180/M_PI;
    float acc_angle_y = atan2(acc_x,  sqrt(acc_y*acc_y + acc_z*acc_z))*R2D;
    float acc_angle_x = atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z))*R2D;
    float acc_angle_Z = atan2(sqrt(acc_x*acc_x + acc_y*acc_y), acc_z);
    //filter gyro
    float dt = (t_now - get_last_time())/1000.0;
    float gyro_angle_x = gyro_x*dt + get_last_x();
    float gyro_angle_y = gyro_y*dt + get_last_y();
    float gyro_angle_z = gyro_z*dt + get_last_z(); 
    float alpha = 0.96;
    float angle_x = alpha*gyro_angle_x + (1 - alpha)*acc_angle_x;
    float angle_y = alpha*gyro_angle_y + (1 - alpha)*acc_angle_y;
    float angle_z = gyro_angle_z;

    set_last_read(t_now, angle_x, angle_y, angle_z);
    Serial.print(acc_x, 2); Serial.print("\t");
    Serial.print(acc_y, 2); Serial.print("\t");
    Serial.print(acc_z, 2); Serial.print("\t");
    Serial.print(gyro_x, 2); Serial.print("\t");
    Serial.print(gyro_y, 2); Serial.print("\t");
    Serial.print(gyro_z, 2); Serial.println("\t");
    Serial.print(dt, 4); Serial.print("\t");
    Serial.print(angle_x, 3); Serial.print(",");
    Serial.print(angle_y, 3); Serial.print(",");
    Serial.print(angle_z, 3); Serial.println();
    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(GREEN_LED, blinkState);
Link to post
Share on other sites
  • 2 weeks later...

If I understand correctly then if you uncomment the following piece of code the Sketch runs fine?

    float alpha = 0.96;
    float angle_x = alpha*gyro_angle_x + (1 - alpha)*acc_angle_x;
    float angle_y = alpha*gyro_angle_y + (1 - alpha)*acc_angle_y;
    float angle_z = gyro_angle_z;
Link to post
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

  • Create New...