Revisions to Arduino Mega and gyroscope erroneous values

deleted 9 characters in body

Source Link

edited Sep 7, 2018 at 15:38

Jot

3.3k
1
14
21

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float(t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z) {
  // Get the accelerometergyro values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float(t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z) {
  // Get the accelerometer values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float(t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z) {
  // Get the gyro values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

deleted 1 character in body; deleted 5 characters in body

Source Link

edited Sep 7, 2018 at 15:14

Jot

3.3k
1
14
21

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float( t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z)
  {
  // Get the accelerometer values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float( t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z)
 {
  // Get the accelerometer values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float(t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z) {
  // Get the accelerometer values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

Source Link

answered Sep 7, 2018 at 15:07

Jot

3.3k
1
14
21

It is possible to use your sketch and get angle information. It is not very accurate and it drifts.

I started with your sketch, and improved it until I had the angle. The Serial.println functions take time and they are printed too often. To measure the interval between the samples, I used millis.

This test is done with:

arduino M0 board (with 3.3v i2c bus)
gy-521 module with onboard voltage regulator, but no level shifter
vcc of the gy-521 module connected to 5v (because of the voltage regulator)

The arduino M0 is faster than the arduino mega or uno.

// test with arduino M0 board
#include <Wire.h>

#define SER SerialUSB    // 'Serial' for arduino Uno, 'SerialUSB' for Leonardo and Arduino M0

const int MPU = 0x68;   // I2C address of MPU

int16_t x_offset = 0;
int16_t y_offset = 0;
int16_t z_offset = 0;

float pitch = 0.0;
float roll  = 0.0;
float yaw   = 0.0;

unsigned long previousMillisSample;
unsigned long previousMillisOutput;
const unsigned long intervalOutput = 500;  // half a second update to serial monitor

void setup() {
  SER.begin(9600);
  while(!SER);         // wait for serial monitor to open for Leonardo and M0
  SER.println("Angle from gyro");
  
  Wire.begin();

  Wire.beginTransmission(MPU);
  Wire.write(0x6B); //power management register 1
  Wire.write(0);
  Wire.endTransmission();

  // get the values of the gyro and use them as offset
  GetXYZ(x_offset, y_offset, z_offset);
}

void loop() {

  int16_t GyX;
  int16_t GyY;
  int16_t GyZ;

  GetXYZ(GyX, GyY, GyZ);
  
  unsigned long currentMillisSample = millis();  // time in millis when this sample is taken

  // remove offset
  GyX -= x_offset;
  GyY -= y_offset;
  GyZ -= z_offset;

  float v_pitch = float(GyX) / 131.0;    // convert to float and calculate degrees
  float v_roll  = float(GyY) / 131.0;
  float v_yaw   = float(GyZ) / 131.0;

  // calculate time in milliseconds between previous and current sample
  unsigned long t = currentMillisSample - previousMillisSample;

  // time in seconds as a float
  float float_t = float( t) / 1000.0;

  // adjust the number of degrees during the time
  float a_pitch = v_pitch * float_t;
  float a_roll  = v_roll  * float_t;
  float a_yaw   = v_yaw   * float_t;

  // remember the time in millis when the sample was taken for the next time
  previousMillisSample = currentMillisSample;

  pitch += a_pitch; 
  roll  += a_roll;
  yaw   += a_yaw;

  // Once in a while, print the output
  if(millis() - previousMillisOutput >= intervalOutput) {
    previousMillisOutput = millis();
 
    SER.print("pitch = ");
    SER.print(pitch);
    SER.print(" | roll = ");
    SER.print(roll);
    SER.print(" | yaw = ");
    SER.println(yaw);
  }
}

void GetXYZ( int16_t &x, int16_t &y, int16_t &z)
{
  // Get the accelerometer values
  Wire.beginTransmission(MPU);
  Wire.write(0x43);     // first register of gyro values
  Wire.endTransmission(false);

  Wire.requestFrom(MPU,6);
  x = Wire.read() << 8 | Wire.read();
  y = Wire.read() << 8 | Wire.read();
  z = Wire.read() << 8 | Wire.read();
}

Can you try this sketch? It drifts between 10 and 60 degrees per 5 minutes. That is a lot. I think the sensor is better than that, so that means the sketch is far from perfect.

Stack Exchange Network

Return to Answer