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 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();
}
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.