TEENSY 4.0 reading wrong data on GY-521 MPU6050 Breakout Board

Status
Not open for further replies.
D

Droid

Member
Hello all,

I am new to using the TEENSY platform so excuse me if this is a noob question.

My project requires the use of an accelerometer/gyroscope to get pitch and roll values. My code works on the Arduino nano but it was not able to detect rapid movements of the GY-521 board, hence why I switched to a TEENSY 4.0

I hooked up the VCC of the GY-521 to 5V (I also tried 3.3V), GND to GND, SDA to pin 18, and SCL to pin 19.

I loaded up the code on the teensy and it worked...sort of. Only the pitch and roll values in the positive directions would give accurate data. Whenever I tilted the GY-521 in the opposite directions, the pitch and roll values would blow up to very large values.

To figure out what was wrong, I printed all the necessary information to the serial monitor and what I found was that the raw accelerometer values seemed to be correct, however the raw gyro values along the negative pitch and negative roll axis were obviously false.

For example, when tilting the board towards the positive pitch axis, I would get a reasonable POSITIVE result of around 5 deg/sec. However, when I tilted it towards the negative pitch axis I was reading a POSITIVE result of around 400 deg/sec. Clearly something is wrong with the raw gyro values.

I tried the code again on the arduino and it worked flawlessly. Please help me and thanks in advance :) !

My code is attached to this post.

View attachment main.cpp
 
Looking at code the only odd thing is this:
Code: 
  gyro_x_error = gyro_x_sum / average_cycle_count; //divide by average_cycle_count to get average deg/sec
  gyro_y_error = gyro_y_sum / average_cycle_count;
  gyro_z_error = gyro_z_sum / average_cycle_count;

And how that feeds back:
Code: 
gyro_x = (Wire.read() << 8 | Wire.read()) / 131.0 - [B]gyro_x_error[/B];

Not sure why it would calc different if it works on nano - but wondering what happens if "calculate_gyro_error(); //get the gyroscope error values" is commented in setup()?

Also - Paul noted recently - the T_4.0 will do math in DOUBLE not float when given constants like this:
Code: 
  acc_z = (Wire.read() << 8 | Wire.read()) / [B][COLOR="#FF0000"]16384.0[/COLOR][/B];
To limit that calc to float put an 'f' on all those constants:: acc_z = (Wire.read() << 8 | Wire.read()) / 16384.0f;
> The T_4.0 can handle double in just two cycles, but then it gets reduced to float that may or may not be worthwhile it impactful in the results.
 
Hello defragster, thanks for the response!

The reason the calculate_gyro_error() is there is to ensure that the raw gyro values are at 0 during startup. Without it, the initial gyro measurements have some error upon startup and reads (for example) 5 deg/sec. This makes it so that even when stationary, the pitch and roll values drift over time because it thinks the IMU has an angular velocity of 5deg/sec when really it's at rest. So the idea was that if we get an average of the initial raw values, we can subtract that from the subsequent values to make the controller think that it's at rest (which it really is)

Regardless, I commented out the line to see if it would help, and yes the pitch and roll values now drift, but what's interesting is that now the raw gyro values work for the other axis (the one that was having the problem), but the same problem now persists with the other.

Also I tried adding the "f" to all the constants but unfortunately it didn't seem to help.
 
Always in cases like this, I would wonder things like assumptions about word size and the like:
Example: gyro_x = (Wire.read() << 8 | Wire.read()) / 131.0 - gyro_x_error;

Is Gyro supposed to be a signed number? What I read here is I am assuming always a positive value especially since our word size is 32 bits.
What happens if you change the code like: gyro_x = (int16_t)(Wire.read() << 8 | Wire.read()) / 131.0 - gyro_x_error;
 
THANK YOU KurtE!

Your suggestion seemed to have worked! However, I am a bit confused what the addition of (int16_t) did and why I didn't have to do it on the nano. Are there any more things like this that I need to worry about on a Teensy but not on an arduino?

Thanks once again!
 
Good answer @KurtE - var type 'size' was my first thought before reading code - then I got distracted seeing 'float' - and that post obviously a good summary of the issue.

Without cast of (int16_t) the ARM Teensy as noted defaults to 32 bits - when the sign bit isn't in the right spot it is just another bit and sign bit is 16 bits over on int32_t
 
I've changed the program quite a bit since this post, but here you go!
Code: 
#include <Arduino.h>
#include <Wire.h>

double gyro_x, gyro_y, gyro_z;
double gyro_x_error, gyro_y_error, gyro_z_error;
double gyro_x_sum, gyro_y_sum, gyro_z_sum;
unsigned long cycle_start;
double dT = .002;
double dPitch, dRoll;
double acc_x, acc_y, acc_z;
double acc_pitch, acc_roll, acc_magnitude;
double acc_pitch_error, acc_roll_error;
double pitch, roll;
double pitch_feedback_error, roll_feedback_error;
double desired_pitch, desired_roll;
double acc_magnitude_sum, acc_magnitude_initial;
double servo_roll, servo_pitch;
int average_cycle_count = 2000;

//FUNCTIONS

void setup_mpu_6050_registers(){
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission(true);
}

void read_mpu_6050_data(){
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);

  //Store accelerometer values and divide by 16384 as per the datasheet
  acc_x = (int16_t)(Wire.read() << 8 | Wire.read()) / 16384.0;
  acc_y = (int16_t)(Wire.read() << 8 | Wire.read()) / 16384.0;                                 
  acc_z = (int16_t)(Wire.read() << 8 | Wire.read()) / 16384.0;
  acc_magnitude = sqrt(acc_x * acc_x + acc_y * acc_y + acc_z * acc_z);

  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);

  //Store gyroscope values and divide by 131.0 as per the datasheet to convert to degrees/sec
  gyro_x = (int16_t)(Wire.read() << 8 | Wire.read()) / 131.0 - gyro_x_error;
  gyro_y = (int16_t)(Wire.read() << 8 | Wire.read()) / 131.0 - gyro_y_error;
  gyro_z = (int16_t)(Wire.read() << 8 | Wire.read()) / 131.0 - gyro_z_error;
}

//Calculate the average initial gyroscope values to find the error
//Supposed to be 0 deg/sec because it is initially at rest
//This allows us to eliminate drift along the roll and pitch axes
void calculate_gyro_error(){
    for (int cal_int = 0; cal_int < average_cycle_count; cal_int++){ //average_cycle_count cycles
    read_mpu_6050_data(); //Retrieve gyro data
    gyro_x_sum += gyro_x; //sum the values
    gyro_y_sum += gyro_y;
    gyro_z_sum += gyro_z;

    delay(2);
  }

  gyro_x_error = gyro_x_sum / average_cycle_count; //divide by average_cycle_count to get average deg/sec
  gyro_y_error = gyro_y_sum / average_cycle_count;
  gyro_z_error = gyro_z_sum / average_cycle_count;
}

//Calculate the average initial accelerometer pitch and roll values
//This will be the error since we take the startup position to be 0 on the pitch and roll axes
void calculuate_accelerometer_error(){
  
  for (int cal_acc_int = 0; cal_acc_int < average_cycle_count; cal_acc_int++){
    
    read_mpu_6050_data(); //get the accelerometer data

    acc_pitch_error += (atan(acc_y / sqrt(pow(acc_z, 2) + pow(acc_x, 2))) * 180 / PI); //sum the accelerometer pitch and roll angles in degrees
    acc_roll_error += (atan(-1 * acc_z / sqrt(pow(acc_y, 2) + pow(acc_x, 2))) * 180 / PI);
    acc_magnitude_sum += acc_magnitude;

    delay(2);
  }

  acc_pitch_error /= average_cycle_count; //divide by average_cycle_count to get the average pitch and roll values
  acc_roll_error /= average_cycle_count;
  acc_magnitude_initial = acc_magnitude_sum / average_cycle_count;
  
}

//Calculuate pitch and roll values
void calculate_pitch_roll(){
  dPitch = gyro_z * dT; //small change in angle for each cycle
  dRoll = gyro_y * dT;

  //Accelerometer-based calculations
  acc_pitch = (atan(acc_y / sqrt(pow(acc_z, 2) + pow(acc_x, 2))) * 180 / PI) - acc_pitch_error; //calculate pitch and roll values according to accelerometer and subtract the error
  acc_roll = (atan(-1 * acc_z / sqrt(pow(acc_y, 2) + pow(acc_x, 2))) * 180 / PI) - acc_roll_error;

  //Calculating the final pitch and roll values
  if(abs(acc_magnitude - acc_magnitude_initial) <= .01){
    pitch = (pitch + dPitch) *.97 + acc_pitch * .03; //adding the small change in pitch and roll
    roll = (roll + dRoll) * .97 + acc_roll * .03; //gyro values are more precise but accel helps to eliminate drift
  }
  else{
    pitch = (pitch + dPitch);
    roll = (roll + dRoll);
  }
  

}

//MAIN
void setup() {
  
  Wire.begin();
  Serial.begin(9600);

  delay(100);

  setup_mpu_6050_registers(); //setup the registers

  Serial.println("Calibrating Gyro");

  calculate_gyro_error(); //get the gyroscope error values

  Serial.println("Gyro Calibration Complete");
  Serial.println("Calibrating Accelerometer");

  calculuate_accelerometer_error(); //get the accelerometer error values

  Serial.println("Accelerometer Calibration Complete");

}

void loop() {

  cycle_start = micros(); //start of cycle
  read_mpu_6050_data(); //get data from MPU6050
  calculate_pitch_roll(); //calculate pitch and roll from MPU6050

  Serial.print(pitch);
  Serial.print(" / ");
  Serial.println(roll);

  

  while(((micros() - cycle_start) / 1000000.0) < dT);
}
 
Status
Not open for further replies.
Back
Top